yichael_smartbow/代码备份/新版算法-2022-8-24/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 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;
    }
}