smart-bow-infrared/Assets/BowArrow/Scripts/Bluetooth/New/o0663Axis.cs

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

namespace o0.Bow
{
    public class o0663Axis
    {
        public IMU._663AxisPreProcessor Attitude;

        public static List<UnityEngine.Quaternion> QuaTest = new List<UnityEngine.Quaternion>();
        public o0663Axis(Vector<int> Gyr1ByteIndex = default, Vector<int> Gyr2ByteIndex = default, Vector<int> Acc1ByteIndex = default, Vector<int> Acc2ByteIndex = default, Vector<int> MagByteIndex = default)
        {
            Attitude = new IMU._663AxisPreProcessor(Gyr1ByteIndex, Gyr2ByteIndex, Acc1ByteIndex, Acc2ByteIndex, MagByteIndex);

            QuaTest.Add(UnityEngine.Quaternion.identity);
            QuaTest.Add(UnityEngine.Quaternion.identity);
        }

        static public Vector<double> AccIdentity = new Vector<double>(0, -1, 0);
        static public Vector<double> MagIdentity = new Vector<double>(-1, 2, 0).Normalized;
        public class State
        {
            public double TimeGap;
            public Vector<double> Acc = AccIdentity;
            public Vector<double> AccSmooth = AccIdentity;

            public Vector<double> Gyr;
            public Vector<double> Mag = MagIdentity;
            public Vector<double> MagSmooth = MagIdentity;

            public double GapMS;

            public Geometry.Quaternion Qua = Geometry.Quaternion.Identity;
            public Geometry.Quaternion QuaSmooth = Geometry.Quaternion.Identity;
            public double Variance = 1;
            public double GyrVariance = 1;
            public double AccVariance = 1;
            public double MagVariance = 1;
            public Geometry.Quaternion QuaAccMag = Geometry.Quaternion.Identity;
            public int QuaAccMagCount = 0;
            public double AccMagVariance = 1;
            public double TotalVariance = 1;
        }


        public List<State> States = new List<State>();
        public int StatesMaxCount = 1000;

        public float x;
        public float y;
        public float z;

        long TimeGapOld;

        double lastv = default;
        public Geometry.Quaternion Update(IEnumerable<byte> gyr1Byte, IEnumerable<byte> gyr2Byte, IEnumerable<byte> acc1Byte, IEnumerable<byte> acc2Byte, IEnumerable<byte> magByte,
            byte min, byte sec, byte ms1, byte ms2)
        {
            if(lastv != Attitude.MagCalibrater?.Variance)
            {
                lastv = Attitude.MagCalibrater?.Variance ?? default;
                Debug.Log(lastv);
            }
            var (Gyr, Acc, Mag, TimeGap) = Attitude.Update(gyr1Byte, gyr2Byte, acc1Byte, acc2Byte, magByte, min, sec, ms1, ms2);


            var GyrOperator = Geometry.Quaternion.Euler((Gyr * TimeGap).To<float>());


            var Last = States.LastOrDefault() ?? new State();
            States.Add(new State());
            if (States.Count > StatesMaxCount)
                States.RemoveAt(0);
            var state = States.Last();
            state.Acc = Acc;


            //Debug.Log(Gyr.magnitude);

            state.Gyr = Gyr;
            state.Mag = Mag;/**/
            state.TimeGap = TimeGap;
            if (States.Count <= 1)
                return Geometry.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 Geometry.Quaternion Process9Axis(State Last, State state)
        {
            var Acc = state.Acc;
            var Gyr = state.Gyr;
            var Mag = state.Mag;
            double TimeGap = state.TimeGap;

            o0UIRawImageTester.UpdateAllOffset();


            var LastQuaternion = Last.Qua;



            var GyrOperator = Geometry.Quaternion.Euler((Gyr * TimeGap).To<float>());
            var quaGyr = LastQuaternion * GyrOperator;


            //TestVector.Update9AxisRotation(GyrOperator, 1);
            //TestVector.SetAcc(Acc / 10, 1);
            //TestVector.SetMag(Mag, 1);
            var accTest = Geometry.Quaternion.FromToRotation(Last.Acc.To<float>(), Acc.To<float>()).Inversed;
            var magTest = Geometry.Quaternion.FromToRotation(Last.Mag.To<float>(), Mag.To<float>()).Inversed;
            //TestVector.Set9AxisRotation(Last.Qua, 3);

            double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
            double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差

            double GyrVariance = Last.Variance + Math.Pow((Gyr * TimeGap).Length * 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 = AccVarianceInput != default ? AccVarianceInput : Math.Max((Gyr * TimeGap).Length, state.Acc.Angle(Last.Acc)) * 1 + Math.Pow(Math.Abs(state.Acc.Length - 1) / 1 * AccLengthToAngle, 4);
            double MagVariance = 10 + Math.Pow(Math.Abs(state.Mag.Length - 1) / 1 * MagLengthToAngle, 4);/**/



            if (double.IsInfinity(GyrVariance))
                GyrVariance = 0.0000001;
            if (double.IsNaN(GyrVariance))
                GyrVariance = 0.0000001;
            if (double.IsNaN(AccVariance))
                AccVariance = 0.0000001;
            if (double.IsNaN(MagVariance))
                MagVariance = 0.0000001;

            state.Variance = GyrVariance;
            state.Variance = state.Variance * (AccVariance + MagVariance) / (state.Variance + (AccVariance + MagVariance));


            if (double.IsNaN(state.Variance))
                state.Variance = 0.0000001;

            var quaAccMag = Geometry.Quaternion.FormQuaternion(AccIdentity.To<float>(), MagIdentity.To<float>(), state.Acc.To<float>(), state.Mag.To<float>(), (float)(AccVariance / (AccVariance + MagVariance)));
            var quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));
            var quaMaxRate = GyrVariance / (GyrVariance + Math.Min(AccVariance, MagVariance));
            Geometry.Quaternion quaFirst = Geometry.Quaternion.SLerp(quaGyr, quaAccMag, (float)quaMinRate);
            if (double.IsNaN(quaFirst.w))
                quaFirst = Last.Qua;

            var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);

            state.Qua = AccVariance < MagVariance ? Geometry.Quaternion.FormQuaternion(quaFirst, AccIdentity.To<float>(), state.Acc.To<float>(), (float)quaSecondRate) : Geometry.Quaternion.FormQuaternion(quaFirst, MagIdentity.To<float>(), state.Mag.To<float>(), (float)quaSecondRate);




            ///////////////////////////////////////////////////////////////

            if (double.IsNaN(state.Qua.w))
                state.Qua = Last.Qua;/**/
            state.QuaSmooth = Geometry.Quaternion.SLerp(Last.QuaSmooth, state.Qua, 0.3f);//Last.QuaSmooth - state.Qua    0 - 1


            //QuaTest[0] = o0Project.o0.FormQuaternion(QuaTest[0] * GyrOperator, AccIdentity, state.AccSmooth, 1);
            // QuaTest[0] = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.AccSmooth, state.MagSmooth, (float)(AccVariance / (AccVariance + MagVariance))).ToUnityQuaternion();
            // QuaTest[1] = state.QuaAccMag.ToUnityQuaternion();
            //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().AccVariance = 1000;
            States.Last().GyrVariance = 1000;
            States.Last().MagVariance = 1000;
            States.Last().AccMagVariance = 1000;
            States.Last().TotalVariance = 1000;
        }
        public Geometry.Quaternion SetIdentity()
        {
            //UnityEngine.Quaternion qua = default;

            //int averageCount = Math.Min(States.Count, 5);
            int averageCount = Math.Min(States.Count, 50);
            Vector<double> aveAcc = Vector<double>.Zero;
            Vector<double> aveMag = Vector<double>.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 = Geometry.Quaternion.Identity;
            States.Last().QuaSmooth = Geometry.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 = Geometry.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();
        }
    }

}