换成旧版算法

Assets/BowArrow/Scenes/Home.unity

@@ -965,7 +965,7 @@ RectTransform:
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
   m_AnchorMin: {x: 0, y: 0}
   m_AnchorMax: {x: 0, y: 0}
-  m_AnchoredPosition: {x: 0, y: 0}
+  m_AnchoredPosition: {x: 8.775623, y: 0}
   m_SizeDelta: {x: 0, y: 0}
   m_Pivot: {x: 0.5, y: 0.5}
 --- !u!114 &219397737
@@ -10658,7 +10658,7 @@ RectTransform:
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
   m_AnchorMin: {x: 0, y: 0}
   m_AnchorMax: {x: 0, y: 0}
-  m_AnchoredPosition: {x: 0, y: 0}
+  m_AnchoredPosition: {x: 96.32687, y: 0}
   m_SizeDelta: {x: 0, y: 0}
   m_Pivot: {x: 0.5, y: 0.5}
 --- !u!114 &1698308730
@@ -10935,7 +10935,6 @@ GameObject:
   - component: {fileID: 1711599508}
   - component: {fileID: 1711599507}
   - component: {fileID: 1711599506}
-  - component: {fileID: 1711599505}
   m_Layer: 5
   m_Name: LabelTemperature
   m_TagString: Untagged
@@ -10962,22 +10961,6 @@ RectTransform:
   m_AnchoredPosition: {x: -53.27, y: -1.2039509}
   m_SizeDelta: {x: 0, y: 0}
   m_Pivot: {x: 1, y: 1}
---- !u!114 &1711599505
-MonoBehaviour:
-  m_ObjectHideFlags: 0
-  m_CorrespondingSourceObject: {fileID: 0}
-  m_PrefabInstance: {fileID: 0}
-  m_PrefabAsset: {fileID: 0}
-  m_GameObject: {fileID: 1711599503}
-  m_Enabled: 1
-  m_EditorHideFlags: 0
-  m_Script: {fileID: 11500000, guid: b7cbfcb0916cf694fb3d059ea4b4c1da, type: 3}
-  m_Name: 
-  m_EditorClassIdentifier: 
-  textID: 113
-  layoutRebuildObject: {fileID: 1590289749}
-  textFormatArgs: []
-  languageFontSizes: []
 --- !u!114 &1711599506
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -11025,7 +11008,7 @@ MonoBehaviour:
     m_HorizontalOverflow: 0
     m_VerticalOverflow: 0
     m_LineSpacing: 1
-  m_Text: "\u6E29\u5EA6"
+  m_Text: 
 --- !u!222 &1711599508
 CanvasRenderer:
   m_ObjectHideFlags: 0

Assets/BowArrow/Scripts/Bluetooth/AimHandler.cs

@@ -269,9 +269,9 @@ public class AimHandler : MonoBehaviour
             }
             return;
         }
-        if (DeviceBatteryView.ins) {
-            DeviceBatteryView.ins.labelTemperature.text = (TwoByteToFloat(bytes[5], bytes[6]) / 100).ToString("#0.00") + "℃";
-        }
+        // if (DeviceBatteryView.ins) {
+        //     DeviceBatteryView.ins.labelTemperature.text = (TwoByteToFloat(bytes[5], bytes[6]) / 100).ToString("#0.00") + "℃";
+        // }
         if (bytes[7] == 0 && bytes[8] == 0 && bytes[9] == 0 && bytes[10] == 0 && bytes[11] == 0 && bytes[12] == 0)
             return;
         if (bytes[19] == 0 && bytes[20] == 0 && bytes[21] == 0 && bytes[22] == 0 && bytes[23] == 0 && bytes[24] == 0)
@@ -280,7 +280,7 @@ public class AimHandler : MonoBehaviour
         if (ban9AxisCalculate) //如果箭射出后禁止九轴计算，就缓存最新几帧九轴数据
         {
             
-            if (cached9AxisFrames.Count < 11)
+            if (cached9AxisFrames.Count < 2)
             {
                 cached9AxisFrames.Enqueue(bytes);
             }
@@ -435,23 +435,23 @@ public class AimHandler : MonoBehaviour
         ban9AxisCalculate = ban;
         if (!ban) {
             msOld = default;
-            try
-            {
-                if (StatesBackDebug.ins) _9Axis.DeleteStatesFromTail(StatesBackDebug.ins.val);
-            }
-            catch (Exception) { }
+            // try
+            // {
+            //     if (StatesBackDebug.ins) _9Axis.DeleteStatesFromTail(StatesBackDebug.ins.val);
+            // }
+            // catch (Exception) { }
             //恢复九轴计算时，把缓存的最新几帧计算了
-            bool isFirstFrame = true;
+            // bool isFirstFrame = true;
             while (cached9AxisFrames.Count > 0)
             {
                 try
                 {
                     OnDataReceived(cached9AxisFrames.Dequeue());
                     //最初的一帧是用来顶掉msOld的，不会进state数组，因此不需要设置方差
-                    if (!isFirstFrame) {
-                        _9Axis.SetAccMagVariance(10000);
-                    }
-                    isFirstFrame = false;
+                    // if (!isFirstFrame) {
+                    //     _9Axis.SetAccMagVariance(10000);
+                    // }
+                    // isFirstFrame = false;
                 }
                 catch (Exception) { }
             }

Assets/BowArrow/Scripts/Bluetooth/BluetoothHolder.cs

@@ -35,7 +35,7 @@ public class BluetoothHolder : MonoBehaviour
             this.transform.Find("CanvasForOpenDebug").gameObject.SetActive(false);
         }
         CloseDebug();
-        transform.Find("Objects").gameObject.SetActive(true);
+        // transform.Find("Objects").gameObject.SetActive(true);
     } 
 
     public void ShowMagEllipse(Object viewer)

Assets/BowArrow/Scripts/Bluetooth/o09AxisCS.cs

@@ -3,6 +3,7 @@ using System.Collections.Generic;
 using System.Linq;
 using UnityEngine;
 
+//旧版算法
 public class o09AxisCS
 {
     static public Vector3 AccIdentity = new Vector3(0, -1, 0);
@@ -12,19 +13,14 @@ public class o09AxisCS
         public long TimeGap;
         public Vector3 Acc = AccIdentity;
         public Vector3 AccSmooth = AccIdentity;
+        public double AccVariance = 1;
 
         public Vector3 Gyr;
         public Vector3 Mag = MagIdentity;
         public Vector3 MagSmooth = MagIdentity;
         public Quaternion Qua = Quaternion.identity;
         public Quaternion QuaSmooth = Quaternion.identity;
-        public double GyrVariance = 1;
-        public double AccVariance = 1;
-        public double MagVariance = 1;
-        public Quaternion QuaAccMag = Quaternion.identity;
-        public int QuaAccMagCount = 0;
-        public double AccMagVariance = 1;
-        public double TotalVariance = 1;
+        public double Variance = 1;
     }
 
     o0Project.Variance HardwareVarianceGyr = new o0Project.Variance(1000);
@@ -42,32 +38,37 @@ public class o09AxisCS
     public float z;
 
     long TimeGapOld;
-    o0Aien.o0WeightedAverageFilterVector3 AccFilter = new o0Aien.o0WeightedAverageFilterVector3(1);
-    o0Aien.o0WeightedAverageFilterVector3 MagFilter = new o0Aien.o0WeightedAverageFilterVector3(1);
-
-
-
-    public UnityEngine.Quaternion update(Vector3 Acc, Vector3 Gyr, Vector3 Mag, long TimeGap)
+    o0Aien.o0WeightedAverageFilterVector3 AccFilter = new o0Aien.o0WeightedAverageFilterVector3(5);
+    o0Aien.o0WeightedAverageFilterVector3 MagFilter = new o0Aien.o0WeightedAverageFilterVector3(10);
+         
+    public Quaternion update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
     {
-
         o0UIRawImageTester.UpdateAllOffset();
 
+        var Acc = this.AccOld;
+        var Gyr = (this.GyrOld + GyrOld)/2;
+        var Mag = this.MagOld;
+        float TimeGap = this.TimeGapOld;
+        this.AccOld = AccOld;
+        this.GyrOld = GyrOld;
+        this.MagOld = MagOld;
+        this.TimeGapOld = TimeGapOld;
         var Last = States.LastOrDefault() ?? new State();
+        if (this.TimeGapOld <= 0)
+            return Last.Qua;
         States.Add(new State());
-        if (States.Count > 500)
+        if (States.Count > 200)
             States.RemoveAt(0);
         var state = States.Last();
         state.Acc = Acc;
 
         state.AccSmooth = AccFilter.Update(Acc);
-
-        //Debug.Log(Gyr.magnitude);
-
+        
         state.Gyr = Gyr;
         state.Mag = Mag;/**/
         state.MagSmooth = MagFilter.Update(Mag);
-        if (States.Count <= 1)
-            return UnityEngine.Quaternion.identity;
+        if (States.Count <=1)
+            return Quaternion.identity;
 
         HardwareVarianceGyr.Update((Gyr).magnitude);//每毫秒方差2.331017E-09 度左右   0.00000002331017
         HardwareVarianceAcc.Update(Vector3.Angle(state.Acc, Last.Acc));//方差0.0012度左右
@@ -75,88 +76,31 @@ public class o09AxisCS
 
         var LastQuaternion = Last.Qua;
 
+        var newQua = new Quaternion();
+        newQua.eulerAngles = Gyr * TimeGap;
+        var quaGyr = LastQuaternion * newQua;
 
+        double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
+        double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差
 
-        var GyrOperator = new UnityEngine.Quaternion();
-        GyrOperator.eulerAngles = Gyr * TimeGap;
-        var quaGyr = LastQuaternion * GyrOperator;
-
-
-        //TestVector.Update9AxisRotation(GyrOperator, 1);
-        //TestVector.SetAcc(Acc / 10, 1);
-        //TestVector.SetMag(Mag, 1);
-        var accTest = new UnityEngine.Quaternion();
-        accTest.SetFromToRotation(Last.Acc, Acc);
-        accTest.w *= -1;
-        // TestVector.Update9AxisRotation(accTest, 2);
-        var magTest = new UnityEngine.Quaternion();
-        magTest.SetFromToRotation(Last.Mag, Mag);
-        magTest.w *= -1;
-        // TestVector.Update9AxisRotation(magTest, 3);
-        //TestVector.Set9AxisRotation(Last.Qua, 3);
+        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 MagVariance = Math.Max((Gyr * TimeGap).magnitude, Vector3.Angle(state.MagSmooth, Last.MagSmooth)) * 1 + Math.Pow(Math.Abs(state.MagSmooth.magnitude - 1) / 1 * MagLengthToAngle,4);/**/
 
-        double AccLengthToAngle = 360;//1倍引力差相当于多少度方差
-        double MagLengthToAngle = 360;//1倍磁力差相当于多少度方差
 
-        //double GyrVariance = 1 + Math.Pow((Gyr * TimeGap).magnitude, 0.5);//陀螺仪移动向量
-        double GyrVariance = Math.Pow((Gyr * TimeGap).magnitude + 0.225, 0.5);//陀螺仪移动向量
-        var modelVariance = Math.Pow(0.001 + (Gyr * TimeGap).magnitude * 0.3, 2);
-        double AccVariance = Math.Max(Math.Pow((Gyr * TimeGap).magnitude, 1), 1)
-            + (Math.Abs(state.AccSmooth.magnitude - 1) > 0.1 ? Math.Pow(Math.Abs(state.AccSmooth.magnitude - 1) / 1 * AccLengthToAngle, 4) : 0);
-        double MagVariance = 9
-            + (Math.Abs(state.MagSmooth.magnitude - 1) > 0.1 ? Math.Pow(Math.Abs(state.MagSmooth.magnitude - 1) * MagLengthToAngle, 4) : 0);//指数4其中2是引力2是方差/**/
-        /*
-        double GyrVariance = Math.Pow((Gyr * TimeGap).magnitude + 0.225, 0.5);//陀螺仪移动向量
-        double AccVariance = Math.Pow(Math.Max((Gyr * TimeGap).magnitude, Vector3.Angle(state.AccSmooth, Last.AccSmooth)), 0.5)
-            + (Math.Abs(state.AccSmooth.magnitude - 1) > 0.1 ? Math.Pow(Math.Abs(state.AccSmooth.magnitude - 1) / 1 * AccLengthToAngle, 4) : 0);
-        double MagVariance = Math.Pow(3, 1)
-            + (Math.Abs(state.MagSmooth.magnitude - 1) > 0.1 ? Math.Pow(Math.Abs(state.MagSmooth.magnitude - 1) * MagLengthToAngle, 4) : 0);//指数4其中2是引力2是方差/**/
+        state.Variance = GyrVariance;
+        state.Variance = state.Variance * (AccVariance+ MagVariance) / (state.Variance + (AccVariance + MagVariance));
 
-        //double AccGyrVariance = Last.AccVariance + GyrVariance;
-        //double MagGyrVariance = Last.MagVariance + GyrVariance;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
-
-        //Debug.Log((Gyr * TimeGap).magnitude * 0.1);
-
-
-        //state.MagVariance = MagGyrVariance;
-        //state.MagVariance = state.MagVariance * MagVariance / (state.MagVariance + MagVariance);
-        //state.AccVariance = AccGyrVariance;
-        //state.AccVariance = state.AccVariance * AccVariance / (state.AccVariance + AccVariance);
-
-        //if (double.IsNaN(MagGyrVariance))
-        //    MagGyrVariance = double.MinValue;
         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.GyrVariance))
-            state.GyrVariance = double.MinValue;
-        if (double.IsNaN(state.AccVariance))
-            state.AccVariance = double.MinValue;
-        if (double.IsNaN(state.MagVariance))
-            state.MagVariance = double.MinValue;
-
-        /////////////////////////////////////////////////临时代码
-
-
-        //var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance)));
-
-        //var tempQuaGyr = Quaternion.Slerp(Last.QuaSmooth, state.Qua, 0.5f);
-
-        var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, 1);
-        /*
-        if (state.QuaAccMagCount == 0)
-            state.QuaAccMag = quaAccMag;
-        else
-            state.QuaAccMag = Quaternion.Slerp(state.QuaAccMag, quaAccMag, state.QuaAccMagCount / (state.QuaAccMagCount + 1));
-        ++state.QuaAccMagCount;/**/
+        if (double.IsNaN(state.Variance))
+            state.Variance = double.MinValue;
 
-        ///////////////////////////////////////////////////
-        //var quaMagRate = MagGyrVariance / (MagGyrVariance + MagVariance);
-        //var quaAccRate = MagGyrVariance / (MagGyrVariance + 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));
@@ -167,125 +111,39 @@ public class o09AxisCS
         var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
 
         state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, state.AccSmooth, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, state.MagSmooth, (float)quaSecondRate);
-        /**/
-        var AccMagVariance = AccVariance + MagVariance;
-        if (GyrVariance > 2f)
-        {
-            state.AccMagVariance = 25;
-            state.QuaAccMag = quaGyr;
-        }
-        else
-        {
-            state.AccMagVariance = Math.Min(Last.AccMagVariance + Math.Max(GyrVariance - 1.0, 0), 10000);
-            state.QuaAccMag = UnityEngine.Quaternion.Slerp(Last.QuaAccMag, quaAccMag, (float)(state.AccMagVariance / (state.AccMagVariance + AccMagVariance)));
-            state.AccMagVariance = state.AccMagVariance * AccMagVariance / (state.AccMagVariance + AccMagVariance);
-        }
-
-
-
-        //var modelVariance = Math.Max(GyrVariance, 0) * 0.05;
-
-        state.GyrVariance = Math.Min(Last.GyrVariance + GyrVariance, 10000);
-        //quaGyr = Quaternion.Slerp(Last.QuaSmooth, quaGyr, (float)(state.GyrVariance / (state.GyrVariance + GyrVariance)));
-        state.GyrVariance = state.GyrVariance * GyrVariance / (state.GyrVariance + GyrVariance);
-
-        //Debug.Log(GyrVariance);
-        /*
-        if (state.AccMagVariance < GyrVariance && 1f > Last.GyrVariance)
-        {
-            Debug.Log("o0 static");
-            quaGyr = Quaternion.Slerp(Last.QuaSmooth, quaGyr, (float)(state.AccMagVariance / (state.AccMagVariance + GyrVariance)));
-        }/**/
-
-        state.MagVariance = Math.Min(Last.MagVariance + modelVariance, 10000);
-        state.Qua = o0Project.o0.FormQuaternion(quaGyr, MagIdentity, state.MagSmooth, (float)(state.MagVariance / (state.MagVariance + MagVariance)));
-        state.MagVariance = state.MagVariance * MagVariance / (state.MagVariance + MagVariance);
-
-
-        state.AccVariance = Math.Min(Last.AccVariance + modelVariance, 10000);
-        state.Qua = o0Project.o0.FormQuaternion(state.Qua, AccIdentity, state.AccSmooth, (float)(state.AccVariance / (state.AccVariance + AccVariance)));
-        state.AccVariance = state.AccVariance * AccVariance / (state.AccVariance + AccVariance);
-        //state.Qua = UnityEngine.Quaternion.Slerp(state.Qua, quaAccMag, (float)(state.AccVariance / (state.AccVariance + AccMagVariance)));
-        //state.AccVariance = state.AccVariance * AccMagVariance / (state.AccVariance + AccMagVariance);
-
-        ///////////////////////////////////////////////////////////////
-        ///
-        //var AccMagVariance = Math.Sqrt(AccVariance * AccVariance + MagVariance * MagVariance);
-        //state.Qua.eu
-
-        /*
-        state.TotalVariance = Math.Min(Last.TotalVariance + modelVariance * 0.3, 10000);
-        if (state.TotalVariance > 25)
-            state.TotalVariance = 25;
-        if (state.AccMagVariance < 4)
-        {
-            state.Qua = Quaternion.Slerp(state.Qua, state.QuaAccMag, (float)(state.TotalVariance / (state.TotalVariance + state.AccMagVariance)));
-            state.TotalVariance = state.TotalVariance * state.AccMagVariance / (state.TotalVariance + state.AccMagVariance);
-        }/**/
-
-
-        ///////////////////////////////////////////////////////////////
-
+        
         if (float.IsNaN(state.Qua.w))
             state.Qua = Last.Qua;/**/
-        state.QuaSmooth = UnityEngine.Quaternion.Slerp(Last.QuaSmooth, state.Qua, 0.3f);//Last.QuaSmooth - state.Qua    0 - 1
+        state.QuaSmooth = Quaternion.Slerp(Last.QuaSmooth, state.Qua, 0.3f);
 
         var frontV = Last.Qua * Vector3.forward;
         var upV = Last.Qua * Vector3.up;
 
-        x = (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90 : 270));
-        y = (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180 + (frontV.x < 0 ? 90 : 270));
-        z = (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90 : 270));
-
+        x = (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90:270));
+        y = (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180+(frontV.x < 0 ? 90:270));
+        z = (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90:270));
 
-        //QuaTest[0] = o0Project.o0.FormQuaternion(QuaTest[0] * GyrOperator, AccIdentity, state.AccSmooth, 1);
-        // QuaTest[0] = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance)));
-        // QuaTest[1] = state.QuaAccMag;
-        //QuaTest[1] = o0Project.o0.FormQuaternion(QuaTest[1] * GyrOperator, MagIdentity, state.MagSmooth, 1);
-        //Debug.Log(o09AxisCS.QuaTest[0]);
-        return state.QuaSmooth;
+        return state.Qua;
     }
 
     public void SetIdentity()
     {
-        //Quaternion qua = default;
-
-        int averageCount = Math.Max(States.Count, 10);
-        Vector3 aveAcc = Vector3.zero;
-        Vector3 aveMag = Vector3.zero;
-        for (var i = 0; i < averageCount; ++i)
-        {
-            aveAcc += States[i].Acc;
-            aveMag += States[i].Mag;
-        }
-        aveAcc /= averageCount;
-        aveMag /= averageCount;
-        //AccIdentity = AccOld;
-        //MagIdentity = MagOld;
-        AccIdentity = aveAcc;
-        MagIdentity = aveMag;
-        //qua = o0Project.o0.FormQuaternion(Quaternion.identity, Vector3.down,AccIdentity, 1);
-        //AccIdentity=qua*AccIdentity;
-        //MagIdentity = qua*MagIdentity;
+        Quaternion qua = default;
+        AccIdentity = AccOld;
+        MagIdentity = MagOld;
+        qua = o0Project.o0.FormQuaternion(Quaternion.identity, Vector3.up,AccIdentity, 1);
+        AccIdentity=qua*AccIdentity;
+        MagIdentity = qua*MagIdentity;
         States.Last().Qua = Quaternion.identity;
-        States.Last().QuaSmooth = Quaternion.identity;
-        //States.Last().Qua = qua*States.Last().Qua;//Quaternion.identity;
-        States.Last().AccVariance = 0.0000001;
-        States.Last().GyrVariance = 0.0000001;
-        States.Last().MagVariance = 0.0000001;
-        States.Last().QuaAccMag = Quaternion.identity;
-        States.Last().QuaAccMagCount = 0;
-        States.Last().AccMagVariance = 0.0000001;
-        States.Last().TotalVariance = 0.0000001;
+        // States.Last().Qua = qua*States.Last().Qua;//Quaternion.identity;
+        States.Last().Variance = 0.0000001;
     }
 
-    public State getLastState()
-    {
+    public State getLastState() {
         return this.States.Last();
     }
 
-    public Vector3 getGyrOld()
-    {
+    public Vector3 getGyrOld() {
         return GyrOld;
     }
 
@@ -293,9 +151,9 @@ public class o09AxisCS
         if (count <= 0 || States.Count == 0) return;
         States = States.GetRange(0, States.Count - count <= 0 ? 1 :  States.Count - count);
     }
-    
+
     public void SetAccMagVariance(double value) {
         States.Last().AccVariance = value;
-        States.Last().MagVariance = value;
+        States.Last().Variance = value;
     }
 }

Assets/BowArrow/Scripts/Manager/GameSceneManager.cs

@@ -10,7 +10,7 @@ public class GameSceneManager : MonoBehaviour
         //开启最高抗锯齿值
         antiAliasing = QualitySettings.antiAliasing;
         QualitySettings.antiAliasing = 8;
-        GameObject.Instantiate(Resources.Load<GameObject>("StatesBackDebug"));
+        // GameObject.Instantiate(Resources.Load<GameObject>("StatesBackDebug"));
     }
 
     void OnDestroy()

代码备份/新版算法-2022-8-24/o09AxisCS.cs

@@ -0,0 +1,301 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+
+public class o09AxisCS
+{
+    static public Vector3 AccIdentity = new Vector3(0, -1, 0);
+    static public Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
+    public class State
+    {
+        public long TimeGap;
+        public Vector3 Acc = AccIdentity;
+        public Vector3 AccSmooth = AccIdentity;
+
+        public Vector3 Gyr;
+        public Vector3 Mag = MagIdentity;
+        public Vector3 MagSmooth = MagIdentity;
+        public Quaternion Qua = Quaternion.identity;
+        public Quaternion QuaSmooth = Quaternion.identity;
+        public double GyrVariance = 1;
+        public double AccVariance = 1;
+        public double MagVariance = 1;
+        public Quaternion QuaAccMag = Quaternion.identity;
+        public int QuaAccMagCount = 0;
+        public double AccMagVariance = 1;
+        public double TotalVariance = 1;
+    }
+
+    o0Project.Variance HardwareVarianceGyr = new o0Project.Variance(1000);
+    o0Project.Variance HardwareVarianceAcc = new o0Project.Variance(1000);
+    o0Project.Variance HardwareVarianceMag = new o0Project.Variance(1000);
+
+    public List<State> States = new List<State>();
+
+    public Vector3 AccOld;
+    public Vector3 GyrOld;
+    public Vector3 MagOld;
+
+    public float x;
+    public float y;
+    public float z;
+
+    long TimeGapOld;
+    o0Aien.o0WeightedAverageFilterVector3 AccFilter = new o0Aien.o0WeightedAverageFilterVector3(1);
+    o0Aien.o0WeightedAverageFilterVector3 MagFilter = new o0Aien.o0WeightedAverageFilterVector3(1);
+
+
+
+    public UnityEngine.Quaternion update(Vector3 Acc, Vector3 Gyr, Vector3 Mag, long TimeGap)
+    {
+
+        o0UIRawImageTester.UpdateAllOffset();
+
+        var Last = States.LastOrDefault() ?? new State();
+        States.Add(new State());
+        if (States.Count > 500)
+            States.RemoveAt(0);
+        var state = States.Last();
+        state.Acc = Acc;
+
+        state.AccSmooth = AccFilter.Update(Acc);
+
+        //Debug.Log(Gyr.magnitude);
+
+        state.Gyr = Gyr;
+        state.Mag = Mag;/**/
+        state.MagSmooth = MagFilter.Update(Mag);
+        if (States.Count <= 1)
+            return UnityEngine.Quaternion.identity;
+
+        HardwareVarianceGyr.Update((Gyr).magnitude);//每毫秒方差2.331017E-09 度左右   0.00000002331017
+        HardwareVarianceAcc.Update(Vector3.Angle(state.Acc, Last.Acc));//方差0.0012度左右
+        HardwareVarianceMag.Update(Vector3.Angle(state.Mag, Last.Mag));//方差3.5度左右
+
+        var LastQuaternion = Last.Qua;
+
+
+
+        var GyrOperator = new UnityEngine.Quaternion();
+        GyrOperator.eulerAngles = Gyr * TimeGap;
+        var quaGyr = LastQuaternion * GyrOperator;
+
+
+        //TestVector.Update9AxisRotation(GyrOperator, 1);
+        //TestVector.SetAcc(Acc / 10, 1);
+        //TestVector.SetMag(Mag, 1);
+        var accTest = new UnityEngine.Quaternion();
+        accTest.SetFromToRotation(Last.Acc, Acc);
+        accTest.w *= -1;
+        // TestVector.Update9AxisRotation(accTest, 2);
+        var magTest = new UnityEngine.Quaternion();
+        magTest.SetFromToRotation(Last.Mag, Mag);
+        magTest.w *= -1;
+        // TestVector.Update9AxisRotation(magTest, 3);
+        //TestVector.Set9AxisRotation(Last.Qua, 3);
+
+        double AccLengthToAngle = 360;//1倍引力差相当于多少度方差
+        double MagLengthToAngle = 360;//1倍磁力差相当于多少度方差
+
+        //double GyrVariance = 1 + Math.Pow((Gyr * TimeGap).magnitude, 0.5);//陀螺仪移动向量
+        double GyrVariance = Math.Pow((Gyr * TimeGap).magnitude + 0.225, 0.5);//陀螺仪移动向量
+        var modelVariance = Math.Pow(0.001 + (Gyr * TimeGap).magnitude * 0.3, 2);
+        double AccVariance = Math.Max(Math.Pow((Gyr * TimeGap).magnitude, 1), 1)
+            + (Math.Abs(state.AccSmooth.magnitude - 1) > 0.1 ? Math.Pow(Math.Abs(state.AccSmooth.magnitude - 1) / 1 * AccLengthToAngle, 4) : 0);
+        double MagVariance = 9
+            + (Math.Abs(state.MagSmooth.magnitude - 1) > 0.1 ? Math.Pow(Math.Abs(state.MagSmooth.magnitude - 1) * MagLengthToAngle, 4) : 0);//指数4其中2是引力2是方差/**/
+        /*
+        double GyrVariance = Math.Pow((Gyr * TimeGap).magnitude + 0.225, 0.5);//陀螺仪移动向量
+        double AccVariance = Math.Pow(Math.Max((Gyr * TimeGap).magnitude, Vector3.Angle(state.AccSmooth, Last.AccSmooth)), 0.5)
+            + (Math.Abs(state.AccSmooth.magnitude - 1) > 0.1 ? Math.Pow(Math.Abs(state.AccSmooth.magnitude - 1) / 1 * AccLengthToAngle, 4) : 0);
+        double MagVariance = Math.Pow(3, 1)
+            + (Math.Abs(state.MagSmooth.magnitude - 1) > 0.1 ? Math.Pow(Math.Abs(state.MagSmooth.magnitude - 1) * MagLengthToAngle, 4) : 0);//指数4其中2是引力2是方差/**/
+
+        //double AccGyrVariance = Last.AccVariance + GyrVariance;
+        //double MagGyrVariance = Last.MagVariance + GyrVariance;// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
+
+        //Debug.Log((Gyr * TimeGap).magnitude * 0.1);
+
+
+        //state.MagVariance = MagGyrVariance;
+        //state.MagVariance = state.MagVariance * MagVariance / (state.MagVariance + MagVariance);
+        //state.AccVariance = AccGyrVariance;
+        //state.AccVariance = state.AccVariance * AccVariance / (state.AccVariance + AccVariance);
+
+        //if (double.IsNaN(MagGyrVariance))
+        //    MagGyrVariance = double.MinValue;
+        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.GyrVariance))
+            state.GyrVariance = double.MinValue;
+        if (double.IsNaN(state.AccVariance))
+            state.AccVariance = double.MinValue;
+        if (double.IsNaN(state.MagVariance))
+            state.MagVariance = double.MinValue;
+
+        /////////////////////////////////////////////////临时代码
+
+
+        //var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance)));
+
+        //var tempQuaGyr = Quaternion.Slerp(Last.QuaSmooth, state.Qua, 0.5f);
+
+        var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, 1);
+        /*
+        if (state.QuaAccMagCount == 0)
+            state.QuaAccMag = quaAccMag;
+        else
+            state.QuaAccMag = Quaternion.Slerp(state.QuaAccMag, quaAccMag, state.QuaAccMagCount / (state.QuaAccMagCount + 1));
+        ++state.QuaAccMagCount;/**/
+
+        ///////////////////////////////////////////////////
+        //var quaMagRate = MagGyrVariance / (MagGyrVariance + MagVariance);
+        //var quaAccRate = MagGyrVariance / (MagGyrVariance + MagVariance);
+        /*
+
+        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;
+
+        var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
+
+        state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, state.AccSmooth, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, state.MagSmooth, (float)quaSecondRate);
+        /**/
+        var AccMagVariance = AccVariance + MagVariance;
+        if (GyrVariance > 2f)
+        {
+            state.AccMagVariance = 25;
+            state.QuaAccMag = quaGyr;
+        }
+        else
+        {
+            state.AccMagVariance = Math.Min(Last.AccMagVariance + Math.Max(GyrVariance - 1.0, 0), 10000);
+            state.QuaAccMag = UnityEngine.Quaternion.Slerp(Last.QuaAccMag, quaAccMag, (float)(state.AccMagVariance / (state.AccMagVariance + AccMagVariance)));
+            state.AccMagVariance = state.AccMagVariance * AccMagVariance / (state.AccMagVariance + AccMagVariance);
+        }
+
+
+
+        //var modelVariance = Math.Max(GyrVariance, 0) * 0.05;
+
+        state.GyrVariance = Math.Min(Last.GyrVariance + GyrVariance, 10000);
+        //quaGyr = Quaternion.Slerp(Last.QuaSmooth, quaGyr, (float)(state.GyrVariance / (state.GyrVariance + GyrVariance)));
+        state.GyrVariance = state.GyrVariance * GyrVariance / (state.GyrVariance + GyrVariance);
+
+        //Debug.Log(GyrVariance);
+        /*
+        if (state.AccMagVariance < GyrVariance && 1f > Last.GyrVariance)
+        {
+            Debug.Log("o0 static");
+            quaGyr = Quaternion.Slerp(Last.QuaSmooth, quaGyr, (float)(state.AccMagVariance / (state.AccMagVariance + GyrVariance)));
+        }/**/
+
+        state.MagVariance = Math.Min(Last.MagVariance + modelVariance, 10000);
+        state.Qua = o0Project.o0.FormQuaternion(quaGyr, MagIdentity, state.MagSmooth, (float)(state.MagVariance / (state.MagVariance + MagVariance)));
+        state.MagVariance = state.MagVariance * MagVariance / (state.MagVariance + MagVariance);
+
+
+        state.AccVariance = Math.Min(Last.AccVariance + modelVariance, 10000);
+        state.Qua = o0Project.o0.FormQuaternion(state.Qua, AccIdentity, state.AccSmooth, (float)(state.AccVariance / (state.AccVariance + AccVariance)));
+        state.AccVariance = state.AccVariance * AccVariance / (state.AccVariance + AccVariance);
+        //state.Qua = UnityEngine.Quaternion.Slerp(state.Qua, quaAccMag, (float)(state.AccVariance / (state.AccVariance + AccMagVariance)));
+        //state.AccVariance = state.AccVariance * AccMagVariance / (state.AccVariance + AccMagVariance);
+
+        ///////////////////////////////////////////////////////////////
+        ///
+        //var AccMagVariance = Math.Sqrt(AccVariance * AccVariance + MagVariance * MagVariance);
+        //state.Qua.eu
+
+        /*
+        state.TotalVariance = Math.Min(Last.TotalVariance + modelVariance * 0.3, 10000);
+        if (state.TotalVariance > 25)
+            state.TotalVariance = 25;
+        if (state.AccMagVariance < 4)
+        {
+            state.Qua = Quaternion.Slerp(state.Qua, state.QuaAccMag, (float)(state.TotalVariance / (state.TotalVariance + state.AccMagVariance)));
+            state.TotalVariance = state.TotalVariance * state.AccMagVariance / (state.TotalVariance + state.AccMagVariance);
+        }/**/
+
+
+        ///////////////////////////////////////////////////////////////
+
+        if (float.IsNaN(state.Qua.w))
+            state.Qua = Last.Qua;/**/
+        state.QuaSmooth = UnityEngine.Quaternion.Slerp(Last.QuaSmooth, state.Qua, 0.3f);//Last.QuaSmooth - state.Qua    0 - 1
+
+        var frontV = Last.Qua * Vector3.forward;
+        var upV = Last.Qua * Vector3.up;
+
+        x = (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90 : 270));
+        y = (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180 + (frontV.x < 0 ? 90 : 270));
+        z = (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90 : 270));
+
+
+        //QuaTest[0] = o0Project.o0.FormQuaternion(QuaTest[0] * GyrOperator, AccIdentity, state.AccSmooth, 1);
+        // QuaTest[0] = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance)));
+        // QuaTest[1] = state.QuaAccMag;
+        //QuaTest[1] = o0Project.o0.FormQuaternion(QuaTest[1] * GyrOperator, MagIdentity, state.MagSmooth, 1);
+        //Debug.Log(o09AxisCS.QuaTest[0]);
+        return state.QuaSmooth;
+    }
+
+    public void SetIdentity()
+    {
+        //Quaternion qua = default;
+
+        int averageCount = Math.Max(States.Count, 10);
+        Vector3 aveAcc = Vector3.zero;
+        Vector3 aveMag = Vector3.zero;
+        for (var i = 0; i < averageCount; ++i)
+        {
+            aveAcc += States[i].Acc;
+            aveMag += States[i].Mag;
+        }
+        aveAcc /= averageCount;
+        aveMag /= averageCount;
+        //AccIdentity = AccOld;
+        //MagIdentity = MagOld;
+        AccIdentity = aveAcc;
+        MagIdentity = aveMag;
+        //qua = o0Project.o0.FormQuaternion(Quaternion.identity, Vector3.down,AccIdentity, 1);
+        //AccIdentity=qua*AccIdentity;
+        //MagIdentity = qua*MagIdentity;
+        States.Last().Qua = Quaternion.identity;
+        States.Last().QuaSmooth = Quaternion.identity;
+        //States.Last().Qua = qua*States.Last().Qua;//Quaternion.identity;
+        States.Last().AccVariance = 0.0000001;
+        States.Last().GyrVariance = 0.0000001;
+        States.Last().MagVariance = 0.0000001;
+        States.Last().QuaAccMag = Quaternion.identity;
+        States.Last().QuaAccMagCount = 0;
+        States.Last().AccMagVariance = 0.0000001;
+        States.Last().TotalVariance = 0.0000001;
+    }
+
+    public State getLastState()
+    {
+        return this.States.Last();
+    }
+
+    public Vector3 getGyrOld()
+    {
+        return GyrOld;
+    }
+
+    public void DeleteStatesFromTail(int count) {
+        if (count <= 0 || States.Count == 0) return;
+        States = States.GetRange(0, States.Count - count <= 0 ? 1 :  States.Count - count);
+    }
+    
+    public void SetAccMagVariance(double value) {
+        States.Last().AccVariance = value;
+        States.Last().MagVariance = value;
+    }
+}

代码备份/旧版算法-2022-8-19/o09AxisCS.cs

@@ -154,6 +154,6 @@ public class o09AxisCS
 
     public void SetAccMagVariance(double value) {
         States.Last().AccVariance = value;
-        States.Last().MagVariance = value;
+        States.Last().Variance = value;
     }
 }