yichael_smartbow/Assets/BowArrow/Scripts/Bluetooth/o09AxisCS.cs

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 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;
    // }

    // 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(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 > 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;

    //     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 newQua = new 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);/**/

    //     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;

    //     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;

    //     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);
        
    //     if (float.IsNaN(state.Qua.w))
    //         state.Qua = Last.Qua;/**/
    //     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));

    //     return state.Qua;
    // }

    // public void SetIdentity()
    // {
    //     Quaternion qua = default;
    //     AccIdentity = AccOld;
    //     MagIdentity = MagOld;
    //     qua = o0Project.o0.FormQuaternion(Quaternion.identity, Vector3.down,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;
    // }

    // public State getLastState() {
    //     return this.States.Last();
    // }

    // public Vector3 getGyrOld() {
    //     return GyrOld;
    // }
}