smart-bow-sdk-dll/SmartBowSDK/o09AxisAfterXiaMenFromDll_SDK.cs

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


namespace o0.Bow
{
    public class o09AxisAfterXiaMenFromDll_SDK//卡尔曼滤波优化地磁计向量版
    {
        public IMU._9AxisPreProcessor Attitude;
        public IMU.HardwareVariance GyrHardwareVariance;
        public IMU.HardwareVariance AccHardwareVariance;
        public IMU.HardwareVariance MagHardwareVariance;

        // public static List<UnityEngine.Quaternion> QuaTest = new List<UnityEngine.Quaternion>();
        public o09AxisAfterXiaMenFromDll_SDK(Vector<int> GyrByteIndex = default, Vector<int> AccByteIndex = default, Vector<int> MagByteIndex = default)
        {
            Attitude = new IMU._9AxisPreProcessor(GyrByteIndex, AccByteIndex, MagByteIndex);
            GyrHardwareVariance = new IMU.HardwareVariance();
            AccHardwareVariance = new IMU.HardwareVariance();
            MagHardwareVariance = new IMU.HardwareVariance();

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

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

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

            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 Geometry.Quaternion Update(IEnumerable<byte> gyrByte, IEnumerable<byte> accByte, IEnumerable<byte> magByte,
            byte min, byte sec, byte ms1, byte ms2)
        {
            var (Gyr, Acc, Mag, TimeGap) = Attitude.Update(gyrByte, accByte, magByte, min, sec, ms1, ms2);
            if ((Gyr, Acc, Mag, TimeGap) == default)
                return Geometry.Quaternion.Identity;


            Vector<double> Buffer = default;
            Buffer = GyrHardwareVariance.Update(Attitude.LastGyr);
            if (Buffer != default)
                Debug.Log("GyrHardwareVariance: " + Buffer.Length);
            Buffer = AccHardwareVariance.Update(Acc);
            if (Buffer != default)
                Debug.Log("AccHardwareVariance: " + Buffer.Length);
            Buffer = MagHardwareVariance.Update(Mag);
            if (Buffer != default)
                Debug.Log("MagHardwareVariance: " + Buffer.Length);

            // var GyrOperator = Geometry.Quaternion.Euler(Gyr * TimeGap);
            // TestVector.Update9AxisRotation(GyrOperator.ToUnityQuaternion(), 0);
            // TestVector.SetAcc(Acc.ToUnityVector(), 0);
            // TestVector.SetMag(Mag.ToUnityVector(), 0);


            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 double diff = 0.0001;

        public void MagSmooth(State Last, State state)
        {
            var Acc = state.Acc;
            var Gyr = state.Gyr;
            var Mag = state.Mag;
            double TimeGap = state.TimeGap;
            double GyrVariance = Last.MagVariance + Math.Pow((Gyr * TimeGap).Length, 2);
            double AccVariance = AccVarianceInput != default ? AccVarianceInput : Math.Pow((Gyr * TimeGap).Length * 1, 2) + Math.Pow(Math.Abs(Acc.Length - 1) / 1 * 30, 2);
            double MagVariance = 25;

            state.AccSmooth = Last.AccSmooth + (state.Acc - Last.AccSmooth) * GyrVariance / (AccVariance + GyrVariance);
            state.AccVariance = (GyrVariance + AccVariance) / GyrVariance / AccVariance;
            state.MagSmooth = Last.MagSmooth + (state.Mag - Last.MagSmooth) * GyrVariance / (MagVariance + GyrVariance);
            state.MagVariance = (GyrVariance + MagVariance) / GyrVariance / MagVariance;

            // TestVector.SetAcc(state.AccSmooth.ToUnityVector(), 1);
            // TestVector.SetMag(state.MagSmooth.ToUnityVector(), 1);
        }
        public static double vari = 0.001;
        public double CalGyrVariance(State Last, State state)
        {
            //return Last.Variance + (state.Gyr * state.TimeGap).Length * 0.3 + 0.001;
            return Last.Variance + (state.Gyr * state.TimeGap).Length * 0.3 + vari * state.TimeGap;//0.00000002331017
        }
        public double CalAccVariance(State Last, State state)
        {
            double AccLengthToAngle = 10000;//1倍引力差相当于多少度方差
            return AccVarianceInput != default ? AccVarianceInput :
                Math.Max(
                    (state.Gyr * state.TimeGap).Length * 1000,
                    Math.Abs(state.Acc.Length - 1) * AccLengthToAngle
                );
            //return AccVarianceInput != default ? AccVarianceInput : Math.Max((state.Gyr * state.TimeGap).Length, AccAngleDiff) * 1 + Math.Pow(Math.Abs(state.Acc.Length - 1) / 1 * AccLengthToAngle, 4);
        }
        public double CalMagVariance(State Last, State state)
        {
            return 100;
        }
        public (double, double, double) CalVariance(State Last, State state)
        {
            var GyrVariance = CalGyrVariance(Last, state);
            var AccVariance = CalAccVariance(Last, state);
            var MagVariance = CalMagVariance(Last, state);
            if (double.IsInfinity(GyrVariance) || double.IsNaN(GyrVariance))
                GyrVariance = InitVariance;
            if (double.IsInfinity(AccVariance) || double.IsNaN(AccVariance))
                AccVariance = InitVariance;
            if (double.IsInfinity(MagVariance) || double.IsNaN(MagVariance))
                MagVariance = InitVariance;
            var max = Math.Max(AccVariance, MagVariance);
            state.Variance = GyrVariance * max / (GyrVariance + max);
            if (double.IsInfinity(state.Variance) || double.IsNaN(state.Variance))
                state.Variance = InitVariance;
            return (GyrVariance, AccVariance, MagVariance);
        }

        public void QuaCheck(ref Geometry.Quaternion quaternion, Geometry.Quaternion backUp)
        {
            if (double.IsInfinity(quaternion.x) || double.IsInfinity(quaternion.y) || double.IsInfinity(quaternion.z) || double.IsInfinity(quaternion.w)
                || double.IsNaN(quaternion.x) || double.IsNaN(quaternion.y) || double.IsNaN(quaternion.z) || double.IsNaN(quaternion.w))
                quaternion = backUp;
        }
        // public static string debugInfo = "";
        public Geometry.Quaternion Process9Axis(State Last, State state)
        {
            MagSmooth(Last, state);
            //var Acc = state.AccSmooth;
            var Acc = state.Acc;
            var Gyr = state.Gyr;//误差0.0001-0.0006之间
            //var Gyr = Vector<double>.One.Normalized * 0.001;//误差0.0001-0.0006之间
            var Mag = state.Mag;
            double TimeGap = state.TimeGap;


            //Debug.Log(Gyr.Length);
            // debugInfo = $"Acc:{Acc}\n";
            // debugInfo += $"AccLen:{Acc.Length}\n";
            // debugInfo += $"Gyr:{Gyr}\n";
            // debugInfo += $"GyrLen:{Gyr.Length}\n";
            // debugInfo += $"Mag:{Mag}\n";
            // debugInfo += $"MagLen:{Mag.Length}\n";
            // debugInfo += $"TimeGap:{TimeGap}\n";
            // debugInfo += $"GyrMeanLen:{Attitude.GyrCalibrater.Mean.Length}\n";
            // debugInfo += $"MagVariance:{Attitude.MagCalibrater.Variance}\n";

            var LastQuaternion = Last.QuaSmooth;



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


            // var accTest = Geometry.Quaternion.FromToRotation(Last.Acc, Acc).Inversed;
            // TestVector.Set9AxisRotation(Last.QuaSmooth.ToUnityQuaternion(), 2);
            // var magTest = Geometry.Quaternion.FromToRotation(Last.Mag, Mag).Inversed;
            // TestVector.Update9AxisRotation(GyrOperator.ToUnityQuaternion(), 3);
            //TestVector.Set9AxisRotation(quaGyr.ToUnityQuaternion(), 3);
            //TestVector.Set9AxisRotation(Last.Qua, 3);


            var (GyrVariance, AccVariance, MagVariance) = CalVariance(Last, state);

            var quaAccMag = Geometry.Quaternion.FormQuaternion(AccIdentity.To<float>(), MagIdentity.To<float>(), Acc.To<float>(), Mag.To<float>(), (float)(AccVariance / (AccVariance + MagVariance)));
            var quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));//静止时约0.01效果最好
            var quaMaxRate = GyrVariance / (GyrVariance + Math.Min(AccVariance, MagVariance));
            Geometry.Quaternion quaFirst = Geometry.Quaternion.SLerp(quaGyr, quaAccMag, (float)quaMinRate);//运算到这里如果视角校准有漂移，说明需要重新校准陀螺仪//不是代码问题
            QuaCheck(ref quaFirst, LastQuaternion);//防止数值溢出

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

            // Debug.Log((quaMinRate, AccVariance, quaSecondRate));

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

            QuaCheck(ref state.Qua, LastQuaternion);//防止数值溢出
            state.QuaSmooth = state.Qua;

            if (ShakeFrame > 0)
            {
                --ShakeFrame;
                if (ShakeFrame == 0)
                    AccVarianceInput = default;
            }
            return state.QuaSmooth;
        }

        public double InitVariance
        {
            get
            {
                var minVariance = double.MaxValue;
                foreach (var i in States)
                    if (!double.IsInfinity(i.Variance)
                        && !double.IsNaN(i.Variance)
                        && minVariance > i.Variance)
                        minVariance = i.Variance;
                return minVariance;
            }
        }
        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].AccSmooth;
                aveMag += States[i].MagSmooth;
            }
            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 = InitVariance;
            States.Last().AccVariance = InitVariance;
            States.Last().GyrVariance = InitVariance;
            States.Last().MagVariance = InitVariance;
            States.Last().QuaAccMag = Geometry.Quaternion.Identity;
            States.Last().QuaAccMagCount = 0;
            States.Last().AccMagVariance = InitVariance;
            States.Last().TotalVariance = InitVariance;


            return States.Last().Qua;
        }

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

    }

}