加速计的平滑去掉了，改了下方差计算方式

Assets/BowArrow/Scripts/Bluetooth/o09Axis.cs

@@ -696,8 +696,8 @@ public class o09Axis
     long TimeGapOld;
     //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.o0WeightedAverageFilterVector3 AccFilter = new o0Aien.o0WeightedAverageFilterVector3(5);
+    o0Aien.o0WeightedAverageFilterVector3 MagFilter = new o0Aien.o0WeightedAverageFilterVector3(10);
     // o0Aien.o0SigmoidIntegrationFilterVector3 AccFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(5.2f,5);
     // o0Aien.o0SigmoidIntegrationFilterVector3 MagFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(5.2f,5);
     /////////////////////g       degree/ms         
@@ -724,8 +724,8 @@ public class o09Axis
         state.Acc = Acc;
 
         state.AccSmooth = AccFilter.Update(Acc);
-
-        //state.AccSmooth = Vector3.Slerp(Last.AccSmooth, Acc, 0.3f);
+        //state.AccSmooth = Vector3.Slerp(Last.AccSmooth, Acc, 0.2f);
+        
         state.Gyr = Gyr;
         state.Mag = Mag;/**/
         state.MagSmooth = MagFilter.Update(Mag);
@@ -805,16 +805,22 @@ public class o09Axis
         //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 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);/**/
 
         /*
-        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 GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).magnitude, 2) * 0.1;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
+        double AccVariance = 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) + Vector3.Angle(Mag, Last.Mag) * 100);/**/
+        double MagVariance = Math.Pow(Vector3.Angle(Mag, Last.Mag), 2) * 0.005;/**/
+
+        double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).magnitude * 0.3,3);// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
+        double AccVariance = Math.Max((Gyr * TimeGap).magnitude, Vector3.Angle(state.AccSmooth, Last.AccSmooth)) * 1 + Math.Pow(Math.Abs(state.AccSmooth.magnitude - 9.8) / 9.8 * AccLengthToAngle,4);
+        //double AccVariance = Math.Max(0.01, Math.Pow(Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc) / TimeGap), 2) * 1000);
+        double MagVariance = Math.Max((Gyr * TimeGap).magnitude, Vector3.Angle(state.MagSmooth, Last.MagSmooth)) * 1 + Math.Pow(Math.Abs(state.MagSmooth.magnitude - 1) / 1 * MagLengthToAngle,4);/**/
 
 
 
@@ -822,6 +828,14 @@ public class o09Axis
         state.Variance = GyrVariance;
         state.Variance = state.Variance * (AccVariance+ MagVariance) / (state.Variance + (AccVariance + MagVariance));
 
+        if (double.IsNaN(GyrVariance))
+            GyrVariance = double.MinValue;
+        if (double.IsNaN(AccVariance))
+            AccVariance = double.MinValue;
+        if (double.IsNaN(MagVariance))
+            MagVariance = double.MinValue;
+        if (double.IsNaN(state.Variance))
+            state.Variance = double.MinValue;
 
         TextTester[1].text = "GyrVariance:" + GyrVariance;
         TextTester[2].text = "StaticGyrVariance:" + 0.00000002331017 * TimeGap;
@@ -846,6 +860,15 @@ public class o09Axis
         var quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));
         var quaMaxRate = GyrVariance / (GyrVariance + Math.Min(AccVariance, MagVariance));
         Quaternion quaFirst = Quaternion.Slerp(quaGyr, quaAccMag, (float)quaMinRate).normalized;
+        if (float.IsNaN(quaFirst.w))
+            quaFirst = Last.Qua;
+        /*
+        Debug.Log("start");
+        Debug.Log(Last.Qua);
+        Debug.Log(quaFirst);
+        Debug.Log(quaGyr);
+        Debug.Log(quaAccMag);
+        Debug.Log("end");/**/
 
         var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
         // Gyrwit.transform.localRotation = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, Acc, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, Mag, (float)quaSecondRate);
@@ -853,6 +876,9 @@ public class o09Axis
         // state.Qua = Gyrwit.transform.localRotation;
 
         state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, state.AccSmooth, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, state.MagSmooth, (float)quaSecondRate);
+        
+        if (float.IsNaN(state.Qua.w))
+            state.Qua = Last.Qua;/**/
         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);
@@ -877,14 +903,14 @@ public class o09Axis
         TextTester[14].text = "z轴角度:\n" + (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90:270));
 
 
-        // 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));
+     /*   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;
+        return state.Qua;
     }
 
     public void SetIdentity()

Assets/BowArrow/Scripts/Bluetooth/o0Project.cs

@@ -181,7 +181,30 @@ namespace o0Project
             Vector3 gVector = Vector3.RotateTowards(global, DirectionIdentityNormalized, angle / 180 * Mathf.PI * rate, 1);//偏移量,从陀螺仪数据往加速计数据的偏移量
 
             var newQua = new Quaternion();
+            if (float.IsNaN(gVector.x))
+                return original;
             newQua.SetFromToRotation(original * Direction1New, gVector);
+            /*
+            Debug.Log("startIn");
+            Debug.Log(original * Direction1New);
+            Debug.Log(original);
+            Debug.Log(Direction1New);
+            Debug.Log(gVector);
+            Debug.Log(newQua);
+            Debug.Log("end");/**/
+            if (float.IsNaN(newQua.w))
+                return original;
+            /*
+            try
+            {
+                newQua.SetFromToRotation(original * Direction1New, gVector);
+                if(float.IsNaN(newQua.w))
+                    return original;
+            }
+            catch (Exception)
+            {
+                return original;
+            }/**/
             return newQua * original;//一定要反过来乘
         }
     }