算法update

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

@@ -14,7 +14,7 @@ namespace o0.Bow
         public IMU.HardwareVariance AccHardwareVariance;
         public IMU.HardwareVariance MagHardwareVariance;
 
-        public static List<UnityEngine.Quaternion> QuaTest = new List<UnityEngine.Quaternion>();
+        // public static List<UnityEngine.Quaternion> QuaTest = new List<UnityEngine.Quaternion>();
         public o09AxisAfterXiaMenFromDll(Vector<int> GyrByteIndex = default, Vector<int> AccByteIndex = default, Vector<int> MagByteIndex = default)
         {
             Attitude = new IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
@@ -22,8 +22,8 @@ namespace o0.Bow
             AccHardwareVariance = new IMU.HardwareVariance();
             MagHardwareVariance = new IMU.HardwareVariance();
 
-            QuaTest.Add(UnityEngine.Quaternion.identity);
-            QuaTest.Add(UnityEngine.Quaternion.identity);
+            // QuaTest.Add(UnityEngine.Quaternion.identity);
+            // QuaTest.Add(UnityEngine.Quaternion.identity);
         }
 
         static public Vector<double> AccIdentity = new Vector<double>(0, -1, 0);
@@ -141,19 +141,65 @@ namespace o0.Bow
             // TestVector.SetAcc(state.AccSmooth.ToUnityVector(), 1);
             // TestVector.SetMag(state.MagSmooth.ToUnityVector(), 1);
         }
+        public double CalGyrVariance(State Last, State state)
+        {
+            //return Last.Variance + (state.Gyr * state.TimeGap).Length * 0.3 + 0.001;
+            return Last.Variance + (state.Gyr * state.TimeGap).Length * 0.3 + 0.001 * state.TimeGap;//0.00000002331017
+        }
+        public double CalAccVariance(State Last, State state)
+        {
+            double AccLengthToAngle = 10000;//1倍引力差相当于多少度方差
+            return AccVarianceInput != default ? AccVarianceInput :
+                Math.Max(
+                    (state.Gyr * state.TimeGap).Length * 1000 ,
+                    Math.Abs(state.Acc.Length - 1) * AccLengthToAngle
+                );
+            //return AccVarianceInput != default ? AccVarianceInput : Math.Max((state.Gyr * state.TimeGap).Length, AccAngleDiff) * 1 + Math.Pow(Math.Abs(state.Acc.Length - 1) / 1 * AccLengthToAngle, 4);
+        }
+        public double CalMagVariance(State Last, State state)
+        {
+            return 100;
+        }
+        public (double, double, double) CalVariance(State Last, State state)
+        {
+            var GyrVariance = CalGyrVariance(Last, state);
+            var AccVariance = CalAccVariance(Last, state);
+            var MagVariance = CalMagVariance(Last, state);
+            if (double.IsInfinity(GyrVariance) || double.IsNaN(GyrVariance))
+                GyrVariance = InitVariance;
+            if (double.IsInfinity(AccVariance) || double.IsNaN(AccVariance))
+                AccVariance = InitVariance;
+            if (double.IsInfinity(MagVariance) || double.IsNaN(MagVariance))
+                MagVariance = InitVariance;
+            var max = Math.Max(AccVariance, MagVariance);
+            state.Variance = GyrVariance * max / (GyrVariance + max);
+            if (double.IsInfinity(state.Variance) || double.IsNaN(state.Variance))
+                state.Variance = InitVariance;
+            return (GyrVariance, AccVariance, MagVariance);
+        }
+
+        public void QuaCheck(ref Geometry.Quaternion quaternion, Geometry.Quaternion backUp)
+        {
+            if (double.IsInfinity(quaternion.x) || double.IsInfinity(quaternion.y) || double.IsInfinity(quaternion.z) || double.IsInfinity(quaternion.w)
+                || double.IsNaN(quaternion.x) || double.IsNaN(quaternion.y) || double.IsNaN(quaternion.z) || double.IsNaN(quaternion.w))
+                quaternion = backUp;
+        }
+
         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;
+            var Gyr = state.Gyr;//误差0.0001-0.0006之间
+            //var Gyr = Vector<double>.One.Normalized * 0.001;//误差0.0001-0.0006之间
+            var Mag = state.Mag;
             double TimeGap = state.TimeGap;
 
             o0UIRawImageTester.UpdateAllOffset();
 
+            //Debug.Log(Gyr.Length);
 
-            var LastQuaternion = Last.Qua;
+            var LastQuaternion = Last.QuaSmooth;
 
 
 
@@ -161,9 +207,6 @@ namespace o0.Bow
             var quaGyr = LastQuaternion * GyrOperator;
 
 
-            //TestVector.Update9AxisRotation(GyrOperator, 1);
-            //TestVector.SetAcc(Acc / 10, 1);
-            //TestVector.SetMag(Mag, 1);
             // var accTest = Geometry.Quaternion.FromToRotation(Last.Acc, Acc).Inversed;
             // TestVector.Set9AxisRotation(Last.QuaSmooth.ToUnityQuaternion(), 2);
             // var magTest = Geometry.Quaternion.FromToRotation(Last.Mag, Mag).Inversed;
@@ -171,102 +214,24 @@ namespace o0.Bow
             //TestVector.Set9AxisRotation(quaGyr.ToUnityQuaternion(), 3);
             //TestVector.Set9AxisRotation(Last.Qua, 3);
 
-            double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
-            // double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差
-
-            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 AccVariance = AccVarianceInput != default ? AccVarianceInput : Math.Max((Gyr * TimeGap).Length, AccAngleDiff) * 1 + Math.Pow(Math.Abs(state.Acc.Length - 1) / 1 * AccLengthToAngle, 4);
-            double MagVariance = 3;// + Math.Pow(Math.Abs(state.MagSmooth.Length - 1) / 1 * MagLengthToAngle, 4);/**/
 
+            var (GyrVariance, AccVariance, MagVariance) = CalVariance(Last, state);
 
-
-            if (double.IsInfinity(GyrVariance))
-            {
-                Debug.Log("GyrVariance IsInfinity: " + state.GyrVariance);
-                GyrVariance = InitVariance;
-            }
-            if (double.IsNaN(GyrVariance))
-            {
-                Debug.Log("GyrVariance IsNaN: " + state.GyrVariance);
-                GyrVariance = InitVariance;
-            }
-            if (double.IsNaN(AccVariance))
-            {
-                //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))
-            {
-                Debug.Log("MagVariance IsNaN: " + state.MagVariance);
-                MagVariance = InitVariance;
-            }
-
-            state.Variance = GyrVariance;
-//state.MagVariance = Math.Min(Last.MagVariance + GyrVariance, 10000);
-            //state.AccVariance = Math.Min(Last.AccVariance + GyrVariance, 10000);
-
-
-            if (double.IsNaN(state.Variance))
-            {
-                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, 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 quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));//静止时约0.01效果最好
             var quaMaxRate = GyrVariance / (GyrVariance + Math.Min(AccVariance, MagVariance));
             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;
-            }
+            QuaCheck(ref quaFirst, LastQuaternion);//防止数值溢出
 
             var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
 
-            //state.Qua = AccVariance < MagVariance ? Geometry.Quaternion.FormQuaternion(quaFirst, AccIdentity, Acc, (float)quaSecondRate) : Geometry.Quaternion.FormQuaternion(quaFirst, MagIdentity, Mag, (float)quaSecondRate);/**/
-            state.Qua = quaFirst;
+            Debug.Log((quaMinRate, AccVariance, 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
-            //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[1] = o0Project.o0.FormQuaternion(QuaTest[1] * GyrOperator, MagIdentity, state.MagSmooth, 1);
-            //Debug.Log(o09AxisCS.QuaTest[0]);
+            QuaCheck(ref state.Qua, LastQuaternion);//防止数值溢出
+            state.QuaSmooth = state.Qua;
 
             if (ShakeFrame > 0)
             {
@@ -277,7 +242,19 @@ namespace o0.Bow
             return state.QuaSmooth;
         }
 
-        public double InitVariance = 1;
+        public double InitVariance
+        {
+            get
+            {
+                var minVariance = double.MaxValue;
+                foreach(var i in States)
+                    if (!double.IsInfinity(i.Variance)
+                        && !double.IsNaN(i.Variance)
+                        && minVariance > i.Variance)
+                        minVariance = i.Variance;
+                return minVariance;
+            }
+        }
         public void Init()
         {
             States.Last().AccVariance = 1000;