滤波算法

Assets/BowArrow/Scripts/Bluetooth/o09Axis.cs

@@ -57,6 +57,11 @@ public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境
         }
         set
         {
+            if (value == default)
+            {
+                _CorrectMatrix = null;
+                return;
+            }
             _CorrectMatrix = CreateMatrix.Dense<double>(3,3);
             for (var i = 0; i < 3; ++i)
                 for (var j = 0; j < 3; ++j)
@@ -244,7 +249,8 @@ public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境
                             //MathNetV = (MathNetV * mat_Scale) * mat_Correct;
                             MathNetV = (MathNetV) * mat_Correct;
                             v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
-                            if(Math.Abs(v.magnitude - AverageDistance) > 0.1* AverageDistance) {
+                            if (Math.Abs(v.magnitude - AverageDistance) > 0.1 * AverageDistance)
+                            {
                                 BadRecords.Add(i);
                             }
                         }
@@ -681,6 +687,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(5.2f,5);
+    // o0Aien.o0SigmoidIntegrationFilterVector3 MagFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(5.2f,5);
     /////////////////////g       degree/ms         
     public Quaternion Update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
     {
@@ -702,9 +712,20 @@ public class o09Axis
         if (States.Count > 200)
             States.RemoveAt(0);
         var state = States.Last();
+        /*
         state.Acc = Acc;
         state.Gyr = Gyr;
-        state.Mag = Mag;
+        state.Mag = Mag;/**/
+        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);
 
         HardwareVarianceGyr.Update((Gyr).magnitude);//每毫秒方差2.331017E-09 度左右   0.00000002331017
@@ -819,7 +840,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 = 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));
         //Image1.DrawLine();
         //Debug.Log((Gyr * TimeGap).magnitude);

Assets/BowArrow/Scripts/Bluetooth/o0Filter.cs

@@ -158,12 +158,6 @@ namespace o0Aien
         }
     }
 
-
-
-
-
-
-
     public class o0SigmoidIntegrationFilter : Filter<float>
     {
         static public float SigmoidIntegration(float to)//计算优化过的sigmoid算法
@@ -196,12 +190,14 @@ namespace o0Aien
     }
     public class o0SigmoidIntegrationFilterVector3 : Filter<Vector3>
     {
-        public o0SigmoidIntegrationFilterVector3(float noise)
+        public o0SigmoidIntegrationFilterVector3(float noise,float index = 1)
         {
             this.noise = Mathf.Abs(noise);
+            this.index = index;
         }
         //public Vector3 value { get; protected set; } = default;
         float noise = 10;
+        float index = 1;
 
         public override void Update(ref Vector3 value, Vector3 measuredValue)
         {
@@ -213,17 +209,19 @@ namespace o0Aien
             var noiseBuffer = noise / 10;
             var differenceVector = measuredValue - value;
             float sigmoidResult = o0SigmoidIntegrationFilter.SigmoidIntegration(differenceVector.magnitude / noiseBuffer - 5) * noiseBuffer;//处理value过大时为无穷
-            value += sigmoidResult * differenceVector.normalized;
+            value += Mathf.Pow(sigmoidResult,index) * differenceVector.normalized;
         }
     }
     public class o0SigmoidIntegrationFilterQuaternion : Filter<Quaternion>
     {
-        public o0SigmoidIntegrationFilterQuaternion(float noise)
+        public o0SigmoidIntegrationFilterQuaternion(float noise, float index = 1)
         {
             this.noise = Mathf.Abs(noise);
+            this.index = index;
         }
         //public Vector3 value { get; protected set; } = default;
         float noise = 10;
+        float index = 1;
 
         public override void Update(ref Quaternion value, Quaternion measuredValue)
         {
@@ -236,7 +234,8 @@ namespace o0Aien
             var difference = Quaternion.Angle(measuredValue, value);
             //var differenceVector = measuredValue - value;
             float sigmoidResult = o0SigmoidIntegrationFilter.SigmoidIntegration(difference / noiseBuffer - 5) * noiseBuffer;//处理value过大时为无穷
-            value = Quaternion.Slerp(value, measuredValue, sigmoidResult);
+            // Quaternion.Slerp(value, measuredValue, Mathf.Pow(sigmoidResult, index));
+            value = Quaternion.Slerp(value, measuredValue, Mathf.Pow(sigmoidResult, index));
             //value += sigmoidResult * differenceVector.normalized;
         }
     }