合并最新的九轴算法

Assets/BowArrow/Scripts/Bluetooth/AimHandler.cs

@@ -30,8 +30,6 @@ public class AimHandler : MonoBehaviour
     Vector3 Mag = default;
     o09Axis _9Axis = new o09Axis();
 
-    o0SigmoidIntegrationFilterQuaternion filter = new o0SigmoidIntegrationFilterQuaternion(0.2f);
-
     Vector3 cMaxVector = new Vector3(0,0,0);
     Vector3 cMinVector = new Vector3(0, 0, 0);
 
@@ -275,9 +273,6 @@ public class AimHandler : MonoBehaviour
         msOld = ms;
 
         AMesh.transform.localRotation = newRotation = _9Axis.Update(Acc * 10, Gyr, Mag, TimeGap);
-        Quaternion nextRotation = controlObj.localRotation;
-        filter.Update(ref nextRotation, newRotation);
-        newRotation = nextRotation;
 
         // 记录一些旋转角---start
         if (ArmBow.ins) {

Assets/BowArrow/Scripts/Bluetooth/o09Axis.cs

@@ -666,10 +666,17 @@ public class o09Axis
     {
         public long TimeGap;
         public Vector3 Acc = AccIdentity;
+        public Vector3 AccSmooth = AccIdentity;
+        public double AccVariance = 1;
+
         public Vector3 Gyr;
         public Vector3 Mag = MagIdentity;
-        public Quaternion Qua;
+        public Vector3 MagSmooth = MagIdentity;
+        public Quaternion Qua = Quaternion.identity;
+        public Quaternion QuaSmooth = Quaternion.identity;
         public double Variance = 1;
+
+
     }
 
     o0Project.Variance HardwareVarianceGyr = new o0Project.Variance(1000);
@@ -687,8 +694,10 @@ public class o09Axis
     public float z;
 
     long TimeGapOld;
-    o0Aien.o0SigmoidIntegrationFilterVector3 AccFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(0.2f,1);
-    o0Aien.o0SigmoidIntegrationFilterVector3 MagFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(0.2f,1);
+    //o0Aien.o0SigmoidIntegrationFilterVector3 AccFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(0.2f, 1);
+    //o0Aien.o0SigmoidIntegrationFilterVector3 MagFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(0.2f,1);
+    o0Aien.o0WeightedAverageFilterVector3 AccFilter = new o0Aien.o0WeightedAverageFilterVector3(20);
+    o0Aien.o0WeightedAverageFilterVector3 MagFilter = new o0Aien.o0WeightedAverageFilterVector3(20);
     // o0Aien.o0SigmoidIntegrationFilterVector3 AccFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(5.2f,5);
     // o0Aien.o0SigmoidIntegrationFilterVector3 MagFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(5.2f,5);
     /////////////////////g       degree/ms         
@@ -698,9 +707,9 @@ public class o09Axis
 
         //Debug.Log(TimeGapOld);
         var Acc = this.AccOld;
-        var Gyr = this.GyrOld;
+        var Gyr = (this.GyrOld + GyrOld)/2;
         var Mag = this.MagOld;
-        float TimeGap = (TimeGapOld + this.TimeGapOld) / 2;
+        float TimeGap = this.TimeGapOld;
         this.AccOld = AccOld;
         this.GyrOld = GyrOld;
         this.MagOld = MagOld;
@@ -712,21 +721,24 @@ public class o09Axis
         if (States.Count > 200)
             States.RemoveAt(0);
         var state = States.Last();
-        /*
         state.Acc = Acc;
+
+        state.AccSmooth = AccFilter.Update(Acc);
+
+        //state.AccSmooth = Vector3.Slerp(Last.AccSmooth, Acc, 0.3f);
         state.Gyr = Gyr;
         state.Mag = Mag;/**/
+        state.MagSmooth = MagFilter.Update(Mag);
+        if (States.Count <=1)
+            return Quaternion.identity;
+
+
+        /*
         state.Acc = Last.Acc;
         AccFilter.Update(ref state.Acc, Acc);
-        // state.Acc = Vector3.Lerp(Acc, state.Acc,0.99);
-        
         state.Gyr = Gyr;
-        // state.Gyr = Quaternion.Lerp(ref state.Gyr, Gyr,0.01);
-
         state.Mag = Last.Mag;
-        MagFilter.Update(ref state.Mag, Mag);
-        // state.Mag = Quaternion.Slerp(this.MagOld,state.Mag,0.99);
-        //Debug.Log(TimeGap);
+        MagFilter.Update(ref state.Mag, Mag);/**/
 
         HardwareVarianceGyr.Update((Gyr).magnitude);//每毫秒方差2.331017E-09 度左右   0.00000002331017
         HardwareVarianceAcc.Update(Vector3.Angle(state.Acc, Last.Acc));//方差0.0012度左右
@@ -787,29 +799,30 @@ public class o09Axis
         // //Debug.Log(Vector3.Angle(Mag, Last.Mag));
         // double MagVariance = Math.Max(3.5, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 4) + Math.Pow(Vector3.Angle(Mag, Last.Mag) * 0.07, 2));
 
-        double GyrVariance = Last.Variance + 0.00000002331017 * TimeGap + Math.Pow((Gyr * TimeGap).magnitude , 2) * 0.04;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
+        /*
+        double GyrVariance = Last.Variance + 0.00000002331017 * TimeGap + Math.Pow((Gyr * TimeGap).magnitude, 2) * 0.04;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
         double AccVariance = Math.Max(0.01, Math.Pow((Acc.magnitude - 9.8) / 9.8 * AccLengthToAngle, 4) + Math.Pow(Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc) / TimeGap), 2) * 400);
         //double AccVariance = Math.Max(0.01, Math.Pow(Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc) / TimeGap), 2) * 1000);
-        double MagVariance = Math.Max(3.5, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 4) + Math.Pow(Vector3.Angle(Mag, Last.Mag), 2) * 0.005);
+        double MagVariance = Math.Max(3.5, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 4) + Math.Pow(Vector3.Angle(Mag, Last.Mag), 2) * 0.005);/**/
+
+        double GyrVariance = Last.Variance + 0.00000002331017 * TimeGap + Math.Pow((Gyr * TimeGap).magnitude, 2) * 0.1;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
+        double AccVariance = Math.Max(0.01, Math.Pow((Acc.magnitude - 9.8) / 9.8 * AccLengthToAngle, 4) + Math.Pow(Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc) / TimeGap), 2) * 20);
+        //double AccVariance = Math.Max(0.01, Math.Pow(Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc) / TimeGap), 2) * 1000);
+        double MagVariance = Math.Max(3.5, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 4) + Math.Pow(Vector3.Angle(Mag, Last.Mag), 2) * 0.005);/**/
 
         /*
-        Tester?[1].DrawLine(TimeGap / 100f, new Color(0, 0, 1));
-        Tester?[2].DrawLine((int)(Last.Variance / 90), new Color(0, 0, 0));
-        double GyrVariance = Last.Variance + Math.Max(0.00000002331017 * TimeGap, Math.Pow((Gyr * TimeGap).magnitude * 0.001f, 2));// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
-        //Debug.Log(Math.Max(0.00000002331017 * TimeGap, Math.Pow((Gyr * TimeGap).magnitude * 0.001f, 2)));
-        Tester?[6].DrawLine((Gyr * TimeGap).magnitude * 0.05f / 90, new Color(0, 0, 0));
-        double AccVariance = Vector3.Angle(Acc, Last.Acc) < 0.01 ? 0.0012f : 99999;
-        //double AccVariance = Math.Max(0.0012f, Math.Pow((Acc.magnitude - 9.8) / 9.8 * AccLengthToAngle, 2) + Math.Pow(Vector3.Angle(Acc, Last.Acc) * 0.8, 2));
-        //Debug.Log(Vector3.Angle(Mag, Last.Mag));
-        double MagVariance = Vector3.Angle(Mag, Last.Mag) < 5 && Vector3.Angle(Mag, Last.Mag) != 0 ? 3.5 : 99999;/**/
-        //double MagVariance = Math.Max(3.5f, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 2) + Math.Pow(Vector3.Angle(Mag, Last.Mag) * 0.05, 2));
-        //Debug.Log(MagVariance);
+        double GyrVariance = Last.Variance + (Gyr * TimeGap).magnitude;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
+        double AccVariance = Math.Max(0.01, Math.Pow((Acc.magnitude - 9.8) / 9.8 * AccLengthToAngle, 4) + Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc) / TimeGap) * 100);
+        //double AccVariance = Math.Max(0.01, Math.Pow(Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc) / TimeGap), 2) * 1000);
+        double MagVariance = Math.Max(3.5, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 4) + Vector3.Angle(Mag, Last.Mag) * 100);/**/
+
+
+
+
         state.Variance = GyrVariance;
         state.Variance = state.Variance * (AccVariance+ MagVariance) / (state.Variance + (AccVariance + MagVariance));
-        //state.Variance = state.Variance * MagVariance / (state.Variance + MagVariance);
-        //Debug.Log(state.Variance);
-        //Debug.Log(TextTester[0]);
-        // TextTester[0].text = "Variance:" + state.Variance;
+
+
         TextTester[1].text = "GyrVariance:" + GyrVariance;
         TextTester[2].text = "StaticGyrVariance:" + 0.00000002331017 * TimeGap;
         TextTester[3].text = "MothonGyrVariance:" + Math.Pow((Gyr * TimeGap).magnitude * 0.07, 2);
@@ -828,7 +841,7 @@ public class o09Axis
             Debug.Log(Gyr);
         }/**/
 
-        var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, (float)(AccVariance / (AccVariance + MagVariance)));
+        var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance)));
 
         var quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));
         var quaMaxRate = GyrVariance / (GyrVariance + Math.Min(AccVariance, MagVariance));
@@ -839,7 +852,8 @@ public class o09Axis
 
         // state.Qua = Gyrwit.transform.localRotation;
 
-        state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, Acc, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, Mag, (float)quaSecondRate);
+        state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, state.AccSmooth, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, state.MagSmooth, (float)quaSecondRate);
+        state.QuaSmooth = Quaternion.Slerp(Last.QuaSmooth, state.Qua, 0.3f);
         // state.Qua = Quaternion.Lerp(state.Qua, state.Qua,0.99);
         // state.Qua = Quaternion.Slerp(state.Qua, state.Qua,0.99);
         //Tester?[0].DrawLine(TimeGap / 200, new Color(1, 0, 0));
@@ -861,7 +875,16 @@ public class o09Axis
         TextTester[12].text = "x轴角度:\n" + (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90:270));
         TextTester[13].text = "y轴角度:\n" + (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180+(frontV.x < 0 ? 90:270));
         TextTester[14].text = "z轴角度:\n" + (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90:270));
-        return state.Qua;
+
+
+        Tester?[0].DrawLine(Vector3.Angle(Last.Acc, state.Acc) / 3f, new Color(0, 0, 1));
+        Tester?[1].DrawLine(Vector3.Angle(Last.AccSmooth, state.AccSmooth) / 3f, new Color(0, 0, 1));
+        Tester?[3].DrawLine(Vector3.Angle(Last.Mag, state.Mag) / 3f, new Color(0, 0, 1));
+        Tester?[4].DrawLine(Vector3.Angle(Last.MagSmooth, state.MagSmooth) / 3f, new Color(0, 0, 1));
+        Tester?[6].DrawLine(Quaternion.Angle(Last.Qua, state.Qua) / 3f, new Color(0, 0, 1));
+        Tester?[7].DrawLine(Quaternion.Angle(Last.QuaSmooth, state.QuaSmooth) / 3f, new Color(0, 0, 1));
+
+        return state.QuaSmooth;
     }
 
     public void SetIdentity()

Assets/BowArrow/Scripts/Bluetooth/o0Filter.cs

@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Generic;
 using UnityEngine;
 
 namespace o0Aien
@@ -158,6 +159,49 @@ namespace o0Aien
         }
     }
 
+    public class o0WeightedAverageFilterVector3
+    {
+        static public float QuarterCircleIntegration(float to)//原点为中心，面积为4的，四分之一圆
+        {
+            return (Mathf.Asin(to) + Mathf.Sqrt(1-to * to)*to) * 2f / Mathf.PI;
+            //return (radius + to * to / Mathf.Tan(radius)) * 2 / Mathf.PI;
+            /*
+            return radius / 2  + to * to / Mathf.Tan(radius) / 2;/**/
+        }
+
+
+        int RecordCount;
+        float[] DefiniteIntegration;
+        public o0WeightedAverageFilterVector3(int RecordCount)
+        {
+            this.RecordCount = RecordCount;
+            var DefiniteIntegrationBuffer = new float[RecordCount + 1];
+            for (var i = 0; i < DefiniteIntegrationBuffer.Length; ++i)
+                DefiniteIntegrationBuffer[i] = QuarterCircleIntegration(i / (float)RecordCount);
+            DefiniteIntegration = new float[RecordCount];
+            for (var i = 0; i < RecordCount; ++i)
+                DefiniteIntegration[RecordCount - 1 - i] = DefiniteIntegrationBuffer[i + 1] - DefiniteIntegrationBuffer[i];
+        }
+        //public Vector3 value { get; protected set; } = default;
+        List<Vector3> Record = new List<Vector3>();
+        public Vector3 Update(Vector3 Value)
+        {
+            if (Record.Count >= RecordCount)
+                Record.RemoveAt(0);
+            Record.Add(Value);
+            if (Record.Count != RecordCount)
+                return Value;
+
+            var direction = Vector3.zero;
+            float length = 0;
+            for(var i = 0;i< RecordCount; ++i)
+            {
+                direction += Record[i] * DefiniteIntegration[i];
+                length += Record[i].magnitude * DefiniteIntegration[i];
+            }
+            return direction.normalized * length;
+        }
+    }
     public class o0SigmoidIntegrationFilter : Filter<float>
     {
         static public float SigmoidIntegration(float to)//计算优化过的sigmoid算法