smart-bow-infrared/Assets/BowArrow/Scripts/Bluetooth/o09AxisCS.cs

using o0.Geometry;
using System;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;


namespace o0.Bow
{
    public class o09AxisCS
    {
        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).To<float>());
            //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);

            //Debug.Log(Gyr.magnitude);

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


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

            this.ShakeFrame = ShakeFrame;
            Debug.Log("OnShot");/**/
        }
        public double diff = 0.001;
        public UnityEngine.Quaternion Process9Axis(State Last, State state)
    {
            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度左右
        HardwareVarianceMag.Update(Vector3.Angle(state.Mag, Last.Mag));//方差3.5度左右

        var LastQuaternion = Last.Qua;

        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)+ 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 = 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));
        UnityEngine.Quaternion quaFirst = UnityEngine.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 = UnityEngine.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 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;

            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;
            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();
        }

    }

}