算法更新

Assets/BowArrow/Scripts/Bluetooth/AimHandler.cs

@@ -37,7 +37,7 @@ public class AimHandler : MonoBehaviour
     Vector3 Acc = default;
     Vector3 Gyr = default;
     Vector3 Mag = default;
-    o09Axis _9Axis = new o09Axis();
+    public o09Axis _9Axis = new o09Axis();
 
     Vector3 cMaxVector = new Vector3(0,0,0);
     Vector3 cMinVector = new Vector3(0, 0, 0);
@@ -47,7 +47,7 @@ public class AimHandler : MonoBehaviour
 
     //陀螺仪校准进度记录
     [NonSerialized] public int gyrCalibrateCompleteCount = 0;
-    [NonSerialized] public int gyrCalibrateTotalCount = 300;
+    [NonSerialized] public int gyrCalibrateTotalCount = 2000;
 
     [NonSerialized] public long msOld = 0;
 
@@ -219,7 +219,7 @@ public class AimHandler : MonoBehaviour
                     }
                     varianceV /= listArray.Length;
                     isCalibrateMagPerfect = varianceV < 0.001;
-                    // PopupMgr.ins.ShowTip(TextAutoLanguage2.GetTextByKey("tip_mag-calibrate-variance-equal") + varianceV);
+                    PopupMgr.ins.ShowTip(TextAutoLanguage2.GetTextByKey("tip_mag-calibrate-variance-equal") + varianceV);
                 }
                 #endregion
             }
@@ -277,20 +277,20 @@ public class AimHandler : MonoBehaviour
         if (bytes[19] == 0 && bytes[20] == 0 && bytes[21] == 0 && bytes[22] == 0 && bytes[23] == 0 && bytes[24] == 0)
             return;
 
-        if (ban9AxisCalculate) //如果箭射出后禁止九轴计算，就缓存最新几帧九轴数据
-        {
-            
-            if (cached9AxisFrames.Count < 2)
-            {
-                cached9AxisFrames.Enqueue(bytes);
-            }
-            else
-            {
-                cached9AxisFrames.Dequeue();
-                cached9AxisFrames.Enqueue(bytes);
-            }
-            return;
-        }
+       //if (ban9AxisCalculate) //如果箭射出后禁止九轴计算，就缓存最新几帧九轴数据
+       //{
+       //    
+       //    if (cached9AxisFrames.Count < 2)
+       //    {
+       //        cached9AxisFrames.Enqueue(bytes);
+       //    }
+       //    else
+       //    {
+       //        cached9AxisFrames.Dequeue();
+       //        cached9AxisFrames.Enqueue(bytes);
+       //    }
+       //    return;
+       //}
 
         float ax = TwoByteToFloat(bytes[7], bytes[8]);
         float ay = TwoByteToFloat(bytes[9], bytes[10]);
@@ -391,7 +391,7 @@ public class AimHandler : MonoBehaviour
         GyrOperator.eulerAngles = Gyr * TimeGap;
         GryMesh.transform.localRotation *= GyrOperator;
 
-        AMesh.transform.localRotation = newRotation = _9Axis.update(Acc * 10, Gyr, Mag, TimeGap);
+        AMesh.transform.localRotation = newRotation = _9Axis.update(Acc, Gyr, Mag, TimeGap);
 
         if (BowQuatDebug.ins) BowQuatDebug.ins.ShowModuleQuat(newRotation.eulerAngles);
 

Assets/BowArrow/Scripts/Bluetooth/ShootCheck.cs

@@ -357,7 +357,11 @@ public class ShootCheck : MonoBehaviour
             ShootCheck.ins.webSocket.Send(logTxt);
         } catch (Exception) {}
         //调用游戏中的射箭接口
-        if (ArmBow.ins) ArmBow.ins.ADS_fire();
+        if (ArmBow.ins) 
+        {
+            AimHandler.ins._9Axis.axisCSBridge.o09AxisCS.OnShot(100, 100, 100000);
+            ArmBow.ins.ADS_fire();
+        }
     }
 
     void Dolock() 

Assets/BowArrow/Scripts/Bluetooth/o09Axis.cs

@@ -730,7 +730,7 @@ public class o09Axis
         return platformID == 1;
     }
 
-    o09AxisCSBridge axisCSBridge = new o09AxisCSBridge();
+    public o09AxisCSBridge axisCSBridge = new o09AxisCSBridge();
 
     public Quaternion update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld) {
         if (axisCSBridge != null) {
@@ -765,19 +765,6 @@ public class o09Axis
         else return SO_GetLastState_f();
     }
 
-    public void DeleteStatesFromTail(int count) {
-        if (axisCSBridge != null) {
-            axisCSBridge.DeleteStatesFromTail(count);
-            return;
-        }
-    }
-
-    public void SetAccMagVariance(double value) {
-        if (axisCSBridge != null) {
-            axisCSBridge.SetAccMagVariance(value);
-            return;
-        }
-    }
 
     private Vector3 getAccIdentity() {
         if (axisCSBridge != null) {

Assets/BowArrow/Scripts/Bluetooth/o09AxisCS.cs

@@ -1,74 +1,132 @@
+using o0.Geometry;
 using System;
 using System.Collections.Generic;
 using System.Linq;
 using UnityEngine;
 
-//旧版算法
-public class o09AxisCS
+
+namespace o0.Bow
 {
-    static public Vector3 AccIdentity = new Vector3(0, -1, 0);
-    static public Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
-    public class State
+    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 Variance = 1;
-    }
+        public static List<UnityEngine.Quaternion> QuaTest = new List<UnityEngine.Quaternion>();
+        public o09AxisCS()
+        {
+            QuaTest.Add(UnityEngine.Quaternion.identity);
+            QuaTest.Add(UnityEngine.Quaternion.identity);
+        }
+
+        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 double GapMS;
+
+            public UnityEngine.Quaternion Qua = UnityEngine.Quaternion.identity;
+            public UnityEngine.Quaternion QuaSmooth = UnityEngine.Quaternion.identity;
+            public double Variance = 1;
+            public double GyrVariance = 1;
+            public double AccVariance = 1;
+            public double MagVariance = 1;
+            public UnityEngine.Quaternion QuaAccMag = UnityEngine.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 float x;
+        public float y;
+        public float z;
+
+        long TimeGapOld;
+        o0Aien.o0WeightedAverageFilterVector3 AccFilter = new o0Aien.o0WeightedAverageFilterVector3(1);
+        o0Aien.o0WeightedAverageFilterVector3 MagFilter = new o0Aien.o0WeightedAverageFilterVector3(1);
+
+
+        //Geometry.Quaternion Qua = Geometry.Quaternion.Identity;
+        public Geometry.Quaternion Update(Vector<double> Gyr, Vector<double> Acc, Vector<double> Mag, double GapMS)
+        {
+
+            //var GyrOperator = Geometry.Quaternion.Euler(Gyr * GapMS);
+            var GyrOperator = Geometry.Quaternion.Euler(Gyr * GapMS);
+            //Debug.Log(Acc +" | "+ Acc.Length);
+            //var GyrOperator = new UnityEngine.Quaternion();
+            //GyrOperator.eulerAngles = (Gyr * GapMS).ToUnityVector();
+            //GyrOperator.eulerAngles = (Gyr).ToUnityVector();
+            //Qua = Qua * GyrOperator;
+            //TestVector.Set9AxisRotation(Qua);
+            //TestVector.Set9AxisRotation(Qua.ToUnityQuaternion(), 0);
+
+            Geometry.Quaternion quaternion = Update(Acc, Gyr, Mag, (long)GapMS);
+
+            return quaternion;
+        }
+
+        public UnityEngine.Quaternion Update(Vector3 Acc, Vector3 Gyr, Vector3 Mag, long TimeGap)
+        {
+            var Last = States.LastOrDefault() ?? new State();
+            States.Add(new State());
+            if (States.Count > 200)
+                States.RemoveAt(0);
+            var state = States.Last();
+            state.Acc = Acc;
+
+            state.AccSmooth = AccFilter.Update(Acc);
 
-    o0Project.Variance HardwareVarianceGyr = new o0Project.Variance(1000);
-    o0Project.Variance HardwareVarianceAcc = new o0Project.Variance(1000);
-    o0Project.Variance HardwareVarianceMag = new o0Project.Variance(1000);
+            //Debug.Log(Gyr.magnitude);
 
-    public List<State> States = new List<State>();
+            state.Gyr = Gyr;
+            state.Mag = Mag;/**/
+            state.MagSmooth = MagFilter.Update(Mag);
+            state.TimeGap = TimeGap;
+            if (States.Count <= 1)
+                return UnityEngine.Quaternion.identity;
+            return Process9Axis(Last, state);
+        }/**/
 
-    public Vector3 AccOld;
-    public Vector3 GyrOld;
-    public Vector3 MagOld;
 
-    public float x;
-    public float y;
-    public float z;
+        int ShakeFrame;
+        int AccVarianceInput;
+        /// //////////////////向前追溯多少帧//向后多少帧计算抖动
+        public void OnShot(int TrackBack, int ShakeFrame, int AccVarianceInput)
+        {
+            this.AccVarianceInput = AccVarianceInput;
+            TrackBack = Math.Min(TrackBack, States.Count);
+            var startI = States.Count - TrackBack;
+            State Last = default;
+            foreach (var i in TrackBack.Range())
+            {
+                var state = States[startI + i];
+                
+                if (Last != default)
+                    Process9Axis(Last, state);
+                Last = state;
+            }
 
-    long TimeGapOld;
-    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)
+            this.ShakeFrame = ShakeFrame;
+            Debug.Log("OnShot");/**/
+        }
+        public double diff = 0.001;
+        public UnityEngine.Quaternion Process9Axis(State Last, State state)
     {
-        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 > 200)
-            States.RemoveAt(0);
-        var state = States.Last();
-        state.Acc = Acc;
-
-        state.AccSmooth = AccFilter.Update(Acc);
-        
-        state.Gyr = Gyr;
-        state.Mag = Mag;/**/
-        state.MagSmooth = MagFilter.Update(Mag);
-        if (States.Count <=1)
-            return Quaternion.identity;
+            Vector3 Acc = state.Acc;
+            Vector3 Gyr = state.Gyr;
+            Vector3 Mag = state.Mag;
+            long TimeGap = state.TimeGap;
 
         HardwareVarianceGyr.Update((Gyr).magnitude);//每毫秒方差2.331017E-09 度左右   0.00000002331017
         HardwareVarianceAcc.Update(Vector3.Angle(state.Acc, Last.Acc));//方差0.0012度左右
@@ -76,19 +134,23 @@ public class o09AxisCS
 
         var LastQuaternion = Last.Qua;
 
-        var newQua = new Quaternion();
+        var newQua = new UnityEngine.Quaternion();
         newQua.eulerAngles = Gyr * TimeGap;
         var quaGyr = LastQuaternion * newQua;
 
         double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
         double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差
 
-        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 GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).magnitude * 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 AccVariance = Math.Max((Gyr * TimeGap).magnitude, Vector3.Angle(state.AccSmooth, Last.AccSmooth)) * 1 + Math.Pow(Math.Abs(state.AccSmooth.magnitude - 1) / 1 * AccLengthToAngle,4);
+        double MagVariance = 10 + Math.Pow(Math.Abs(state.MagSmooth.magnitude - 1) / 1 * MagLengthToAngle, 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);/**/
 
 
-        state.Variance = GyrVariance;
+            state.Variance = GyrVariance;
         state.Variance = state.Variance * (AccVariance+ MagVariance) / (state.Variance + (AccVariance + MagVariance));
 
         if (double.IsNaN(GyrVariance))
@@ -104,7 +166,7 @@ public class o09AxisCS
 
         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;
+        UnityEngine.Quaternion quaFirst = UnityEngine.Quaternion.Slerp(quaGyr, quaAccMag, (float)quaMinRate).normalized;
         if (float.IsNaN(quaFirst.w))
             quaFirst = Last.Qua;
 
@@ -114,7 +176,7 @@ public class o09AxisCS
         
         if (float.IsNaN(state.Qua.w))
             state.Qua = Last.Qua;/**/
-        state.QuaSmooth = Quaternion.Slerp(Last.QuaSmooth, state.Qua, 0.3f);
+        state.QuaSmooth = UnityEngine.Quaternion.Slerp(Last.QuaSmooth, state.Qua, 0.3f);
 
         var frontV = Last.Qua * Vector3.forward;
         var upV = Last.Qua * Vector3.up;
@@ -125,35 +187,249 @@ public class o09AxisCS
 
         return state.Qua;
     }
+        /// ////////////////////////////////////////////上一帧////当前帧
+        public UnityEngine.Quaternion Process9Axis2(State Last, State state)
+        {
+            //o0UIRawImageTester.UpdateAllOffset();
+            Vector3 Acc = state.Acc;
+            Vector3 Gyr = state.Gyr;
+            Vector3 Mag = state.Mag;
+            long TimeGap = state.TimeGap;
 
-    public void SetIdentity()
-    {
-        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().Qua = qua*States.Last().Qua;//Quaternion.identity;
-        States.Last().Variance = 0.0000001;
-    }
+            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度左右
 
-    public State getLastState() {
-        return this.States.Last();
-    }
+            var LastQuaternion = Last.Qua;
 
-    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().Variance = value;
+            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;
+            var magTest = new UnityEngine.Quaternion();
+            magTest.SetFromToRotation(Last.Mag, Mag);
+            magTest.w *= -1;
+            //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);
+            //var modelVariance = Math.Pow(0.001 + (Mathf.Max(Gyr.magnitude, 1) * TimeGap) * 0.3, 2);
+            double AccVariance = AccVarianceInput != default ? AccVarianceInput : 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]);
+            if (ShakeFrame > 0)
+            {
+                --ShakeFrame;
+                if (ShakeFrame == 0)
+                    AccVarianceInput = default;
+            }
+            return state.QuaSmooth;
+        }
+
+        public void Init()
+        {
+            States.Last().Variance = 1000;
+            States.Last().AccVariance = 1000;
+            States.Last().GyrVariance = 1000;
+            States.Last().MagVariance = 1000;
+            States.Last().AccMagVariance = 1000;
+            States.Last().TotalVariance = 1000;
+        }
+        public UnityEngine.Quaternion SetIdentity()
+        {
+            //UnityEngine.Quaternion qua = default;
+
+            //int averageCount = Math.Min(States.Count, 5);
+            int averageCount = Math.Min(States.Count, 50);
+            Vector3 aveAcc = Vector3.zero;
+            Vector3 aveMag = Vector3.zero;
+            for (var i = States.Count - averageCount; i < States.Count; ++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 = UnityEngine.Quaternion.identity;
+            States.Last().QuaSmooth = UnityEngine.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().QuaAccMag = UnityEngine.Quaternion.identity;
+            States.Last().QuaAccMagCount = 0;
+            States.Last().AccMagVariance = 0.0000001;
+            States.Last().TotalVariance = 0.0000001;
+
+
+            return States.Last().Qua;
+        }
+
+        public State getLastState()
+        {
+            return this.States.Last();
+        }
+
     }
-}
+
+}

Assets/BowArrow/Scripts/Bluetooth/o09AxisCSBridge.cs

@@ -1,15 +1,17 @@
 using UnityEngine;
+using o0.Bow;
+using System.Linq;
 
 public class o09AxisCSBridge {
-    o09AxisCS o09AxisCS = new o09AxisCS();
+    public o09AxisCS o09AxisCS = new o09AxisCS();
     public Quaternion Update_f(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld) {
-        return o09AxisCS.update(AccOld, GyrOld, MagOld, TimeGapOld);
+        return o09AxisCS.Update(AccOld, GyrOld, MagOld, TimeGapOld);
     }
     public void SetIdentity() {
         o09AxisCS.SetIdentity();
     }
     public Vector3 GetGyrOld_f() {
-        return o09AxisCS.getGyrOld();
+        return o09AxisCS.States.LastOrDefault().Gyr;
     }
     public o09Axis.State GetLastState_f() {
         o09AxisCS.State s = o09AxisCS.getLastState();
@@ -27,14 +29,7 @@ public class o09AxisCSBridge {
         return state;
     }
 
-    public void DeleteStatesFromTail(int count) {
-        o09AxisCS.DeleteStatesFromTail(count);
-    }
 
-    public void SetAccMagVariance(double value) {
-        o09AxisCS.SetAccMagVariance(value);
-    }
-    
     public Vector3 GetAccIdentity_f() {
         return o09AxisCS.AccIdentity;
     }