九轴算法切回集成库

Assets/BowArrow/Scripts/Bluetooth/o09Axis.cs

@@ -654,164 +654,8 @@ public class o0GyrCalibrater
         return v;
     }
 }
-// public class o09Axis
-// {
-//     public List<o0UIRawImageTester> Tester = new List<o0UIRawImageTester>();
-//     public List<Text> TextTester = new List<Text>();
-
-//     public GameObject AccMesh;
-//     public GameObject GryMesh;
-//     public GameObject MagMesh;
-
-//     public struct State 
-//     {
-//         public Int32 TimeGap;
-//         public Vector3 Acc;
-//         public Vector3 AccSmooth;
-//         public float AccVariance;
-//         public Vector3 Gyr;
-//         public Vector3 Mag;
-//         public Vector3 MagSmooth;
-//         public Quaternion Qua;
-//         public Quaternion QuaSmooth;
-//         public float Variance;
-
-//     }
-
-//     public void SetIdentityAndSave()
-//     {
-//         setIdentity();
-//         TextTester[0].text = "AccIdentity:" + getAccIdentity();
-//         AccMesh.transform.localRotation = default;
-//         MagMesh.transform.localRotation = default;
-//         GryMesh.transform.localRotation = default;
-//         SaveIdentity();
-//     }
-
-//     public void LoadIdentity() 
-//     {
-//         try {
-//             string magIdentityStr = PlayerPrefs.GetString("MagIdentity", "");
-//             if (magIdentityStr.Length > 0) {
-//                 float[] arr = JsonConvert.DeserializeObject<float[]>(magIdentityStr);
-//                 setMagIdentity(new Vector3(arr[0], arr[1], arr[2]));
-//             }
-//             string accIdentityStr = PlayerPrefs.GetString("AccIdentity", "");
-//             if (accIdentityStr.Length > 0) {
-//                 float[] arr = JsonConvert.DeserializeObject<float[]>(accIdentityStr);
-//                 setAccIdentity(new Vector3(arr[0], arr[1], arr[2]));
-//             }
-//         }
-//         catch (System.Exception e) { Debug.LogError(e.Message); }
-//     }
-    
-//     private void SaveIdentity() {
-//         Vector3 m = getMagIdentity();
-//         Vector3 a = getAccIdentity();
-//         PlayerPrefs.SetString("MagIdentity",JsonConvert.SerializeObject(new float[]{
-//             m.x, m.y, m.z
-//         }));
-//         PlayerPrefs.SetString("AccIdentity", JsonConvert.SerializeObject(new float[]{
-//             a.x, a.y, a.z
-//         }));
-//     }
-
-//     int platformID = -1;
-
-//     void SetPlatformID() 
-//     {
-//         if (Application.platform == RuntimePlatform.WindowsEditor) platformID = 1;
-//         else platformID = 2;
-//     }
-
-//     bool IsWindows() 
-//     {
-//         if (platformID == -1) SetPlatformID();
-//         return platformID == 1;
-//     }
-
-//     public Quaternion update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld) {
-//         if (IsWindows()) return Update_f(AccOld, GyrOld, MagOld, TimeGapOld);
-//         else return SO_Update_f(AccOld, GyrOld, MagOld, TimeGapOld);
-        
-//     }
-
-//     private void setIdentity() {
-//         if (IsWindows()) SetIdentity();
-//         else SO_SetIdentity();
-//     }
-
-//     public Vector3 getGyrOld() {
-//         if (IsWindows()) return GetGyrOld_f();
-//         else return SO_GetGyrOld_f();
-//     }
-
-//     public State getLastState() {
-//         if (IsWindows()) return GetLastState_f();
-//         else return SO_GetLastState_f();
-//     }
-
-//     private Vector3 getAccIdentity() {
-//         if (IsWindows()) return GetAccIdentity_f();
-//         else return SO_GetAccIdentity_f();
-//     }
-
-//     private Vector3 getMagIdentity() {
-//         if (IsWindows()) return GetMagIdentity_f();
-//         else return SO_GetMagIdentity_f();
-//     }
-
-//     private void setAccIdentity(Vector3 value) {
-//         if (IsWindows()) SetAccIdentity(value);
-//         else SO_SetAccIdentity(value);
-//     }
-
-//     private  void setMagIdentity(Vector3 value) {
-//         if (IsWindows()) SetMagIdentity(value);
-//         else SO_SetMagIdentity(value);
-//     }
-
-
-//     [DllImport("o09Axis")]
-//     extern static Quaternion Update_f(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld);
-//     [DllImport("o09Axis")]
-//     extern static void SetIdentity();
-//     [DllImport("o09Axis")]
-//     extern static Vector3 GetGyrOld_f();
-//     [DllImport("o09Axis")]
-//     extern static State GetLastState_f();
-//     [DllImport("o09Axis")]
-//     extern static Vector3 GetAccIdentity_f();
-//     [DllImport("o09Axis")]
-//     extern static Vector3 GetMagIdentity_f();
-//     [DllImport("o09Axis")]
-//     extern static void SetAccIdentity(Vector3 value);
-//     [DllImport("o09Axis")]
-//     extern static void SetMagIdentity(Vector3 value);
-
-
-//     [DllImport("libSharedObjectAxis")]
-//     extern static Quaternion SO_Update_f(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld);
-//     [DllImport("libSharedObjectAxis")]
-//     extern static void SO_SetIdentity();
-//     [DllImport("libSharedObjectAxis")]
-//     extern static Vector3 SO_GetGyrOld_f();
-//     [DllImport("libSharedObjectAxis")]
-//     extern static State SO_GetLastState_f();
-//     [DllImport("libSharedObjectAxis")]
-//     extern static Vector3 SO_GetAccIdentity_f();
-//     [DllImport("libSharedObjectAxis")]
-//     extern static Vector3 SO_GetMagIdentity_f();
-//     [DllImport("libSharedObjectAxis")]
-//     extern static void SO_SetAccIdentity(Vector3 value);
-//     [DllImport("libSharedObjectAxis")]
-//     extern static void SO_SetMagIdentity(Vector3 value);
-// }
-
 public class o09Axis
 {
-    // public static List<o0UIRawImageTester> Tester = new List<o0UIRawImageTester>();
-    // public static List<Text> TextTester = new List<Text>();
     public List<o0UIRawImageTester> Tester = new List<o0UIRawImageTester>();
     public List<Text> TextTester = new List<Text>();
 
@@ -819,336 +663,147 @@ public class o09Axis
     public GameObject GryMesh;
     public GameObject MagMesh;
 
-    static public Vector3 AccIdentity = new Vector3(0, -1, 0);
-    static public Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
-    public class State
+    public struct State 
     {
-        public long TimeGap;
-        public Vector3 Acc = AccIdentity;
-        public Vector3 AccSmooth = AccIdentity;
-        public double AccVariance = 1;
-
+        public Int32 TimeGap;
+        public Vector3 Acc;
+        public Vector3 AccSmooth;
+        public float AccVariance;
         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 Vector3 Mag;
+        public Vector3 MagSmooth;
+        public Quaternion Qua;
+        public Quaternion QuaSmooth;
+        public float Variance;
 
     }
 
-    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.o0SigmoidIntegrationFilterVector3 AccFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(0.2f, 1);
-    //o0Aien.o0SigmoidIntegrationFilterVector3 MagFilter = new o0Aien.o0SigmoidIntegrationFilterVector3(0.2f,1);
-    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         
-    public Quaternion update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
-    {
-        o0UIRawImageTester.UpdateAllOffset();
-
-        //Debug.Log(TimeGapOld);
-        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.AccSmooth = Vector3.Slerp(Last.AccSmooth, Acc, 0.2f);
-        
-        state.Gyr = Gyr;
-        state.Mag = Mag;/**/
-        state.MagSmooth = MagFilter.Update(Mag);
-        if (States.Count <=1)
-            return Quaternion.identity;
-
-
-        /*
-        state.Acc = Last.Acc;
-        AccFilter.Update(ref state.Acc, Acc);
-        state.Gyr = Gyr;
-        state.Mag = Last.Mag;
-        MagFilter.Update(ref state.Mag, Mag);/**/
-
-        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度左右
-        //Tester?[7].DrawLine(HardwareVarianceAcc.Value, new Color(0, 0, 0));//0.0012左右
-        //Tester?[8].DrawLine(HardwareVarianceMag.Value, new Color(0, 0, 0));//3.5左右
-        //Debug.Log(HardwareVarianceMag.Value);
-
-
-        //var Accwit = GameObject.Find("Accwit");
-        //var Gyrwit = GameObject.Find("Gyrwit");
-        //var Magwit = GameObject.Find("Magwit");
-        var LastQuaternion = Last.Qua;
-        //var LastQuaternion = Gyrwit.transform.localRotation;
-
-        var newQua = new Quaternion();
-        newQua.eulerAngles = Gyr * TimeGap;
-        var quaGyr = LastQuaternion * newQua;
-
-        AccMesh.transform.localRotation = o0Project.o0.FormQuaternion(AccMesh.transform.localRotation, AccIdentity, Acc, 1);
-        MagMesh.transform.localRotation = o0Project.o0.FormQuaternion(MagMesh.transform.localRotation, MagIdentity, Mag, 1);
-        GryMesh.transform.localRotation = GryMesh.transform.localRotation * newQua;
-
-        //Tester?[3].DrawLine(Vector3.Angle(Acc, Last.Acc) / 1, new Color(1, 0, 0));
-        //Tester?[4].DrawLine(Quaternion.Angle(LastQuaternion, quaGyr) / 45, new Color(1, 0, 0));
-        //Tester?[5].DrawLine(Vector3.Angle(Mag, Last.Mag) / 5, new Color(1, 0, 0));
-
-
-        double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
-        double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差
-
-
-        /*
-        float GyrVariance = Last.Variance + (Gyr * TimeGap).magnitude * 0.05f;
-        float AccVariance = Mathf.Max(TimeGap / 1, Mathf.Sqrt(Mathf.Pow((Acc.magnitude - 9.8f) / 9.8f * AccLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Acc, Last.Acc) * 0.8f, 2)));
-        //Debug.Log(AccVariance);
-        float MagVariance = Mathf.Max(TimeGap / 0.2f, Mathf.Sqrt(Mathf.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.05f, 2)));
-        state.Variance = state.Variance * AccVariance / (state.Variance + AccVariance);
-        state.Variance = state.Variance * MagVariance / (state.Variance + MagVariance);/**/
-        //测试效果不错但没迭代的版本
-
-
-        /*
-         * 
-        float GyrVariance = state.Variance + TimeGap/100 + (Gyr * TimeGap).magnitude * 0.05f;
-        float AccVariance = TimeGap / 30 + Mathf.Sqrt(Mathf.Pow((Acc.magnitude - 9.8f) / 9.8f * AccLengthToAngle, 2)+ Mathf.Pow(Vector3.Angle(Acc,Last.Acc) * 0.5f, 2));
-        //Debug.Log(AccVariance);
-        float MagVariance = TimeGap / 1 + Mathf.Sqrt(Mathf.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.1f, 2));
-        /**/
-
-
-        //Tester?[1].DrawLine(TimeGap / 100f, new Color(0, 0, 1));
-        //Tester?[2].DrawLine((int)(Last.Variance / 90), new Color(0, 0, 0));
-        // double GyrVariance = Last.Variance + 0.00000002331017 * TimeGap + Math.Pow((Gyr * TimeGap).magnitude * 0.03, 2);// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
-        // //Debug.Log(Math.Max(0.00000002331017 * TimeGap, Math.Pow((Gyr * TimeGap).magnitude * 0.001f, 2)));
-        // //Tester?[6].DrawLine((Gyr * TimeGap).magnitude * 0.05f / 90, new Color(0, 0, 0));
-        // 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) * 20, 2));
-        // //double AccVariance = Math.Max(0.01, Math.Pow((Acc.magnitude - 9.8) / 9.8 * AccLengthToAngle, 4) + Math.Pow(Vector3.Angle(Acc, Last.Acc) * 2, 2));
-        // //Debug.Log(Vector3.Angle(Mag, Last.Mag));
-        // double MagVariance = Math.Max(3.5, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 4) + Math.Pow(Vector3.Angle(Mag, Last.Mag) * 0.07, 2));
-
-        /*
-        double GyrVariance = Last.Variance + 0.00000002331017 * TimeGap + Math.Pow((Gyr * TimeGap).magnitude, 2) * 0.04;// 指数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) * 400);
-        //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 + 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.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);/**/
-
-
-
-
-        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;
-        TextTester[3].text = "MothonGyrVariance:" + Math.Pow((Gyr * TimeGap).magnitude * 0.07, 2);
-        TextTester[4].text = "GyrSpeed:" + Gyr.magnitude;
-        TextTester[5].text = "AccVariance:" + AccVariance;
-        TextTester[6].text = "AccLengthVariance:" + Math.Pow((Acc.magnitude - 9.8) / 9.8 * AccLengthToAngle, 4);
-        TextTester[7].text = "AccRotate:" + Math.Pow(Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc)/TimeGap)* 20, 2);
-        TextTester[9].text = "AccLength:" + Acc.magnitude;
-        TextTester[10].text = "Gyr*1000,000:" + (Gyr * 1000000).ToString();
-        TextTester[11].text = "AngleBetweenIdentity*1000:" + Quaternion.Angle(Last.Qua, Quaternion.identity) * 1000;
-        // TextTester[12].text = "Qua.eulerAngles.x:" + Last.Qua.eulerAngles.x;
-        // TextTester[13].text = "Qua.eulerAngles.y:" + Last.Qua.eulerAngles.y;
-        // TextTester[14].text = "Qua.eulerAngles.z:" + Last.Qua.eulerAngles.z;
-        /*if (Gyr != Vector3.zero)
-        {
-            Debug.Log(Gyr);
-        }/**/
-
-        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));
-        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);
-
-        // 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);
-        //Tester?[0].DrawLine(TimeGap / 200, new Color(1, 0, 0));
-        //Image1.DrawLine();
-        //Debug.Log((Gyr * TimeGap).magnitude);
-        //Debug.Log(Quaternion.Angle(state.Qua, Last.Qua));
-        //TextTester[8].text = "AngleRotated:" + Quaternion.Angle(state.Qua, Last.Qua);
-
-        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));
-//         TextTester[18].text = "x轴角度:" + (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90:270));
-//         TextTester[19].text = "y轴角度:" + (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180+(frontV.x < 0 ? 90:270));
-//         TextTester[20].text = "z轴角度" + (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90:270));
-
-//         TextTester[12].text = "x轴角度:\n" + (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90:270));
-//         TextTester[13].text = "y轴角度:\n" + (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180+(frontV.x < 0 ? 90:270));
-//         TextTester[14].text = "z轴角度:\n" + (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90:270));
-
-
-        //TextTester[12].text = "加速计:" + Acc;
-        //TextTester[13].text = "陀螺仪:" + Gyr;
-        //TextTester[14].text = "地磁计:" + Mag;
-
-        TextTester[12].text = "x:" +x;
-        TextTester[13].text = "y:" +y;
-        TextTester[14].text = "z:" +z;
- 
-     /*   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.Qua;
-    }
-
     public void SetIdentityAndSave()
     {
-        Quaternion qua = default;
-        
-        AccIdentity = AccOld;
-        MagIdentity = MagOld;
-        qua = o0Project.o0.FormQuaternion(Quaternion.identity, Vector3.down,AccIdentity, 1);
-
-        AccIdentity=qua*AccIdentity;
-        MagIdentity = qua*MagIdentity;
-        TextTester[0].text = "AccIdentity:"+AccIdentity;
-        States.Last().Qua = Quaternion.identity;
-        States.Last().Qua = qua*States.Last().Qua;//Quaternion.identity;
-        
-        States.Last().Variance = 0.0000001;
-
+        setIdentity();
+        TextTester[0].text = "AccIdentity:" + getAccIdentity();
         AccMesh.transform.localRotation = default;
         MagMesh.transform.localRotation = default;
         GryMesh.transform.localRotation = default;
-
         SaveIdentity();
     }
-    public void SetIdentityAccordingToRecords()
-    {
-        AccIdentity = Vector3.zero;
-        foreach (var i in States)
-            AccIdentity += i.Acc;
-        AccIdentity /= States.Count;
-        MagIdentity = Vector3.zero;
-        foreach (var i in States)
-            MagIdentity += i.Mag;
-        MagIdentity /= States.Count;
-        States.Last().Qua = Quaternion.identity;
-        States.Last().Variance = 0.0000001;
-
-        Vector3.Angle(Vector3.up, States.Last().Mag);  
-    }
+
     public void LoadIdentity() 
     {
-        try
-        {
+        try {
             string magIdentityStr = PlayerPrefs.GetString("MagIdentity", "");
             if (magIdentityStr.Length > 0) {
                 float[] arr = JsonConvert.DeserializeObject<float[]>(magIdentityStr);
-                MagIdentity = new Vector3(arr[0], arr[1], arr[2]);
+                setMagIdentity(new Vector3(arr[0], arr[1], arr[2]));
             }
             string accIdentityStr = PlayerPrefs.GetString("AccIdentity", "");
             if (accIdentityStr.Length > 0) {
                 float[] arr = JsonConvert.DeserializeObject<float[]>(accIdentityStr);
-                AccIdentity = new Vector3(arr[0], arr[1], arr[2]);
+                setAccIdentity(new Vector3(arr[0], arr[1], arr[2]));
             }
         }
         catch (System.Exception e) { Debug.LogError(e.Message); }
     }
-    void SaveIdentity() {
+    
+    private void SaveIdentity() {
+        Vector3 m = getMagIdentity();
+        Vector3 a = getAccIdentity();
         PlayerPrefs.SetString("MagIdentity",JsonConvert.SerializeObject(new float[]{
-                MagIdentity.x, MagIdentity.y, MagIdentity.z
+            m.x, m.y, m.z
         }));
         PlayerPrefs.SetString("AccIdentity", JsonConvert.SerializeObject(new float[]{
-            AccIdentity.x, AccIdentity.y, AccIdentity.z
+            a.x, a.y, a.z
         }));
     }
 
-    public State getLastState() {
-        return this.States.Last();
+    int platformID = -1;
+
+    void SetPlatformID() 
+    {
+        if (Application.platform == RuntimePlatform.WindowsEditor) platformID = 1;
+        else platformID = 2;
+    }
+
+    bool IsWindows() 
+    {
+        if (platformID == -1) SetPlatformID();
+        return platformID == 1;
+    }
+
+    public Quaternion update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld) {
+        if (IsWindows()) return Update_f(AccOld, GyrOld, MagOld, TimeGapOld);
+        else return SO_Update_f(AccOld, GyrOld, MagOld, TimeGapOld);
+        
+    }
+
+    private void setIdentity() {
+        if (IsWindows()) SetIdentity();
+        else SO_SetIdentity();
     }
 
     public Vector3 getGyrOld() {
-        return GyrOld;
+        if (IsWindows()) return GetGyrOld_f();
+        else return SO_GetGyrOld_f();
     }
+
+    public State getLastState() {
+        if (IsWindows()) return GetLastState_f();
+        else return SO_GetLastState_f();
+    }
+
+    private Vector3 getAccIdentity() {
+        if (IsWindows()) return GetAccIdentity_f();
+        else return SO_GetAccIdentity_f();
+    }
+
+    private Vector3 getMagIdentity() {
+        if (IsWindows()) return GetMagIdentity_f();
+        else return SO_GetMagIdentity_f();
+    }
+
+    private void setAccIdentity(Vector3 value) {
+        if (IsWindows()) SetAccIdentity(value);
+        else SO_SetAccIdentity(value);
+    }
+
+    private  void setMagIdentity(Vector3 value) {
+        if (IsWindows()) SetMagIdentity(value);
+        else SO_SetMagIdentity(value);
+    }
+
+
+    [DllImport("o09Axis")]
+    extern static Quaternion Update_f(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld);
+    [DllImport("o09Axis")]
+    extern static void SetIdentity();
+    [DllImport("o09Axis")]
+    extern static Vector3 GetGyrOld_f();
+    [DllImport("o09Axis")]
+    extern static State GetLastState_f();
+    [DllImport("o09Axis")]
+    extern static Vector3 GetAccIdentity_f();
+    [DllImport("o09Axis")]
+    extern static Vector3 GetMagIdentity_f();
+    [DllImport("o09Axis")]
+    extern static void SetAccIdentity(Vector3 value);
+    [DllImport("o09Axis")]
+    extern static void SetMagIdentity(Vector3 value);
+
+
+    [DllImport("libSharedObjectAxis")]
+    extern static Quaternion SO_Update_f(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld);
+    [DllImport("libSharedObjectAxis")]
+    extern static void SO_SetIdentity();
+    [DllImport("libSharedObjectAxis")]
+    extern static Vector3 SO_GetGyrOld_f();
+    [DllImport("libSharedObjectAxis")]
+    extern static State SO_GetLastState_f();
+    [DllImport("libSharedObjectAxis")]
+    extern static Vector3 SO_GetAccIdentity_f();
+    [DllImport("libSharedObjectAxis")]
+    extern static Vector3 SO_GetMagIdentity_f();
+    [DllImport("libSharedObjectAxis")]
+    extern static void SO_SetAccIdentity(Vector3 value);
+    [DllImport("libSharedObjectAxis")]
+    extern static void SO_SetMagIdentity(Vector3 value);
 }