算法update

Assets/BowArrow/Scripts/Bluetooth/New/o09AxisAfterXiaMenFromDll.cs

@@ -7,7 +7,7 @@ using UnityEngine;
 
 namespace o0.Bow
 {
-    public class o09AxisAfterXiaMenFromDll
+    public class o09AxisAfterXiaMenFromDll//卡尔曼滤波优化地磁计向量版
     {
         public IMU._9AxisPreProcessor Attitude;
         public IMU.HardwareVariance GyrHardwareVariance;
@@ -88,6 +88,10 @@ namespace o0.Bow
                 States.RemoveAt(0);
             var state = States.Last();
             state.Acc = Acc;
+
+
+            //Debug.Log(Gyr.magnitude);
+
             state.Gyr = Gyr;
             state.Mag = Mag;/**/
             state.TimeGap = TimeGap;
@@ -116,13 +120,35 @@ namespace o0.Bow
             this.ShakeFrame = ShakeFrame;
             Debug.Log("OnShot");/**/
         }
-        public double diff = 0.00001;
-        public Geometry.Quaternion Process9Axis(State Last, State state)
+        //public double diff = 0.001;
+        public double diff = 0.0001;
+
+        public void MagSmooth(State Last, State state)
         {
             var Acc = state.Acc;
             var Gyr = state.Gyr;
             var Mag = state.Mag;
             double TimeGap = state.TimeGap;
+            double GyrVariance = Last.MagVariance + Math.Pow((Gyr * TimeGap).Length, 2);
+            double AccVariance = AccVarianceInput != default ? AccVarianceInput : Math.Pow((Gyr * TimeGap).Length * 1, 2) + Math.Pow(Math.Abs(Acc.Length - 1) / 1 * 30, 2);
+            double MagVariance = 25;
+
+            state.AccSmooth = Last.AccSmooth + (state.Acc - Last.AccSmooth) * GyrVariance / (AccVariance + GyrVariance);
+            state.AccVariance = (GyrVariance + AccVariance) / GyrVariance / AccVariance;
+            state.MagSmooth = Last.MagSmooth + (state.Mag - Last.MagSmooth) * GyrVariance / (MagVariance + GyrVariance);
+            state.MagVariance = (GyrVariance + MagVariance) / GyrVariance / MagVariance;
+
+            // TestVector.SetAcc(state.AccSmooth.ToUnityVector(), 1);
+            // TestVector.SetMag(state.MagSmooth.ToUnityVector(), 1);
+        }
+        public Geometry.Quaternion Process9Axis(State Last, State state)
+        {
+            MagSmooth(Last, state);
+            //var Acc = state.AccSmooth;
+            var Acc = state.Acc;
+            var Gyr = state.Gyr;
+            var Mag = state.MagSmooth;
+            double TimeGap = state.TimeGap;
 
             o0UIRawImageTester.UpdateAllOffset();
 
@@ -131,85 +157,114 @@ namespace o0.Bow
 
 
 
-            Vector<double> gyrApplied;
-            if (Gyr.Length > 0.001)
-                gyrApplied = Gyr;
-            else
-                gyrApplied = Vector<double>.Zero;
-
-            var GyrOperator = Geometry.Quaternion.Euler(gyrApplied * TimeGap);
+            var GyrOperator = Geometry.Quaternion.Euler(Gyr * TimeGap);
             var quaGyr = LastQuaternion * GyrOperator;
 
-            Debug.Log("Gyr.Length: " + (Gyr).Length);
 
             //TestVector.Update9AxisRotation(GyrOperator, 1);
             //TestVector.SetAcc(Acc / 10, 1);
             //TestVector.SetMag(Mag, 1);
             // var accTest = Geometry.Quaternion.FromToRotation(Last.Acc, Acc).Inversed;
-            // TestVector.Update9AxisRotation(accTest.ToUnityQuaternion(), 2);
+            // TestVector.Set9AxisRotation(Last.QuaSmooth.ToUnityQuaternion(), 2);
             // var magTest = Geometry.Quaternion.FromToRotation(Last.Mag, Mag).Inversed;
-            // TestVector.Update9AxisRotation(magTest.ToUnityQuaternion(), 3);
+            // TestVector.Update9AxisRotation(GyrOperator.ToUnityQuaternion(), 3);
+            //TestVector.Set9AxisRotation(quaGyr.ToUnityQuaternion(), 3);
             //TestVector.Set9AxisRotation(Last.Qua, 3);
 
             double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
-            double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差
+            // double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差
 
-            double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).Length * 0.3, 3) + diff;// + diff;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
+            double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).Length * 0.3, 3) + diff;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
 
+            //double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).magnitude * 0.3, 3) + 0.1;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
+            //    Debug.Log("GyrVariance==" + GyrVariance);
             double AccAngleDiff = state.Acc.Angle(Last.Acc);
             if (double.IsNaN(AccAngleDiff))
             {
                 AccAngleDiff = 0;
             }
-            //double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).magnitude * 0.3, 3) + 0.1;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
-            //    Debug.Log("GyrVariance==" + GyrVariance);
+
             double AccVariance = AccVarianceInput != default ? AccVarianceInput : Math.Max((Gyr * TimeGap).Length, AccAngleDiff) * 1 + Math.Pow(Math.Abs(state.Acc.Length - 1) / 1 * AccLengthToAngle, 4);
-            double MagVariance = 10 + Math.Pow(Math.Abs(state.Mag.Length - 1) / 1 * MagLengthToAngle, 4);/**/
+            double MagVariance = 3;// + Math.Pow(Math.Abs(state.MagSmooth.Length - 1) / 1 * MagLengthToAngle, 4);/**/
 
 
 
             if (double.IsInfinity(GyrVariance))
-                GyrVariance = 0.0000001;
+            {
+                Debug.Log("GyrVariance IsInfinity: " + state.GyrVariance);
+                GyrVariance = InitVariance;
+            }
             if (double.IsNaN(GyrVariance))
-                GyrVariance = 0.0000001;
+            {
+                Debug.Log("GyrVariance IsNaN: " + state.GyrVariance);
+                GyrVariance = InitVariance;
+            }
             if (double.IsNaN(AccVariance))
-                AccVariance = 0.0000001;
+            {
+                //Debug.Log("AccVariance IsNaN: " + state.AccVariance + " " + AccVarianceInput);
+                //Debug.Log("AccVariance IsNaN: " + Math.Max((Gyr * TimeGap).Length, state.Acc.Angle(Last.Acc)) * 1);
+                Debug.Log("AccVariance IsNaN: " + (Gyr * TimeGap).Length  + AccAngleDiff);
+                AccVariance = InitVariance;
+            }
             if (double.IsNaN(MagVariance))
-                MagVariance = 0.0000001;
+            {
+                Debug.Log("MagVariance IsNaN: " + state.MagVariance);
+                MagVariance = InitVariance;
+            }
 
             state.Variance = GyrVariance;
-            state.Variance = state.Variance * (AccVariance + MagVariance) / (state.Variance + (AccVariance + MagVariance));
+//state.MagVariance = Math.Min(Last.MagVariance + GyrVariance, 10000);
+            //state.AccVariance = Math.Min(Last.AccVariance + GyrVariance, 10000);
 
 
             if (double.IsNaN(state.Variance))
-                state.Variance = 0.0000001;
+            {
+                Debug.Log("Variance IsNaN: " + state.Variance);
+                state.Variance = InitVariance;
+            }
+
+            /*
+            state.Qua = quaGyr;
+            state.Qua = Geometry.Quaternion.FormQuaternion(state.Qua, MagIdentity, Mag, (float)(state.MagVariance / (state.MagVariance + MagVariance)));
+            state.MagVariance = state.MagVariance * MagVariance / (state.MagVariance + MagVariance);
+            state.Qua = Geometry.Quaternion.FormQuaternion(state.Qua, AccIdentity, Acc, (float)(state.AccVariance / (state.AccVariance + AccVariance)));
+            state.AccVariance = state.AccVariance * AccVariance / (state.AccVariance + AccVariance);/**/
 
-            var quaAccMag = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.Acc, state.Mag, (float)(AccVariance / (AccVariance + MagVariance)));
+            //var quaAccMag = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, (float)(MagVariance / (AccVariance + MagVariance)));
+            var quaAccMag = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, (float)(AccVariance / (AccVariance + MagVariance)));
+            //var quaAccMag = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.Acc, state.Mag, 0.5f);
             var quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));
             var quaMaxRate = GyrVariance / (GyrVariance + Math.Min(AccVariance, MagVariance));
-            Geometry.Quaternion quaFirst = Geometry.Quaternion.SLerp(quaGyr, quaAccMag, (float)quaMinRate);
+            Geometry.Quaternion quaFirst = Geometry.Quaternion.SLerp(quaGyr, quaAccMag, (float)quaMinRate);//运算到这里如果视角校准有漂移，说明需要重新校准陀螺仪//不是代码问题
+            //Geometry.Quaternion quaFirst = UnityEngine.Quaternion.Slerp(quaGyr.ToUnityQuaternion(), quaAccMag.ToUnityQuaternion(), (float)quaMinRate).Too0Quaternion();
             if (double.IsNaN(quaFirst.x) || double.IsNaN(quaFirst.y) || double.IsNaN(quaFirst.z) || double.IsNaN(quaFirst.w))
+            //if (double.IsNaN(quaFirst.w))
+            {
+                Debug.Log("quaFirst IsNaN: " + quaFirst.ToJson());
                 quaFirst = Last.Qua;
+            }
 
             var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
 
-            state.Qua = AccVariance < MagVariance ? Geometry.Quaternion.FormQuaternion(quaFirst, AccIdentity, state.Acc, (float)quaSecondRate) : Geometry.Quaternion.FormQuaternion(quaFirst, MagIdentity, state.Mag, (float)quaSecondRate);
-
+            //state.Qua = AccVariance < MagVariance ? Geometry.Quaternion.FormQuaternion(quaFirst, AccIdentity, Acc, (float)quaSecondRate) : Geometry.Quaternion.FormQuaternion(quaFirst, MagIdentity, Mag, (float)quaSecondRate);/**/
+            state.Qua = quaFirst;
 
 
 
             ///////////////////////////////////////////////////////////////
 
             if (double.IsNaN(state.Qua.x) || double.IsNaN(state.Qua.y) || double.IsNaN(state.Qua.z) || double.IsNaN(state.Qua.w))
+            //if (double.IsNaN(state.Qua.w))
+            {
+                Debug.Log("quaFirst IsNaN: " + state.Qua.ToJson());
                 state.Qua = Last.Qua;/**/
-            state.QuaSmooth = Geometry.Quaternion.SLerp(Last.QuaSmooth, state.Qua, 0.3f);//Last.QuaSmooth - state.Qua    0 - 1
-
-            if (double.IsNaN(state.QuaSmooth.x) || double.IsNaN(state.QuaSmooth.y) || double.IsNaN(state.QuaSmooth.z) || double.IsNaN(state.QuaSmooth.w))
-                state.QuaSmooth = Last.Qua;/**/
+            }
+            state.QuaSmooth = Geometry.Quaternion.SLerp(Last.QuaSmooth, state.Qua, 1f);//Last.QuaSmooth - state.Qua    0 - 1
+            //state.QuaSmooth = state.Qua;
 
             //QuaTest[0] = o0Project.o0.FormQuaternion(QuaTest[0] * GyrOperator, AccIdentity, state.AccSmooth, 1);
-            QuaTest[0] = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance))).ToUnityQuaternion();
-            QuaTest[1] = state.QuaAccMag.ToUnityQuaternion();
+            //QuaTest[0] = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance))).ToUnityQuaternion();
+            //QuaTest[1] = state.QuaAccMag.ToUnityQuaternion();
             //QuaTest[1] = o0Project.o0.FormQuaternion(QuaTest[1] * GyrOperator, MagIdentity, state.MagSmooth, 1);
             //Debug.Log(o09AxisCS.QuaTest[0]);
 
@@ -222,6 +277,7 @@ namespace o0.Bow
             return state.QuaSmooth;
         }
 
+        public double InitVariance = 1;
         public void Init()
         {
             States.Last().AccVariance = 1000;
@@ -240,8 +296,8 @@ namespace o0.Bow
             Vector<double> aveMag = Vector<double>.Zero;
             for (var i = States.Count - averageCount; i < States.Count; ++i)
             {
-                aveAcc += States[i].Acc;
-                aveMag += States[i].Mag;
+                aveAcc += States[i].AccSmooth;
+                aveMag += States[i].MagSmooth;
             }
             aveAcc /= averageCount;
             aveMag /= averageCount;
@@ -255,14 +311,14 @@ namespace o0.Bow
             States.Last().Qua = Geometry.Quaternion.Identity;
             States.Last().QuaSmooth = Geometry.Quaternion.Identity;
             //States.Last().Qua = qua*States.Last().Qua;//Quaternion.identity;
-            States.Last().Variance = 0.0000001;
-            States.Last().AccVariance = 0.0000001;
-            States.Last().GyrVariance = 0.0000001;
-            States.Last().MagVariance = 0.0000001;
+            States.Last().Variance = InitVariance;
+            States.Last().AccVariance = InitVariance;
+            States.Last().GyrVariance = InitVariance;
+            States.Last().MagVariance = InitVariance;
             States.Last().QuaAccMag = Geometry.Quaternion.Identity;
             States.Last().QuaAccMagCount = 0;
-            States.Last().AccMagVariance = 0.0000001;
-            States.Last().TotalVariance = 0.0000001;
+            States.Last().AccMagVariance = InitVariance;
+            States.Last().TotalVariance = InitVariance;
 
 
             return States.Last().Qua;