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

using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
using MathNet.Numerics.LinearAlgebra;

public class o0Vector3Filter
{
    Vector3 state = default;
    float Variance = 1;
    public Vector3 Update(Vector3 v)
    {
        if (state == default)
            return state = v;
        Variance += 10;

        float mVariance = 1;
        state = Vector3.Lerp(state, v, mVariance/ (Variance + mVariance));

        Variance = Variance * mVariance / (Variance + mVariance);

        return state;
    }
}
public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境下，有磁干扰则无法保证效果
{
    [JsonIgnore]
    public Vector3 _Center = Vector3.zero;
    [JsonIgnore]
    Matrix<double> _CorrectMatrix = null;

    public float[] Center
    {
        get
        {
            return new float[]{_Center.x, _Center.y, _Center.z};
        }
        set
        {
            _Center = new Vector3(value[0], value[1], value[2]);
        }
    }
    public double[] CorrectMatrix
    {
        get
        {
            if (_CorrectMatrix == null)
                return default;
            var m = new double[9];
            for (var i = 0; i < 3; ++i)
                for (var j = 0; j < 3; ++j)
                    m[j + i * 3] = _CorrectMatrix[i,j];
            return m;
        }
        set
        {
            _CorrectMatrix = CreateMatrix.Dense<double>(3,3);
            for (var i = 0; i < 3; ++i)
                for (var j = 0; j < 3; ++j)
                    _CorrectMatrix[i, j] = value[j + i * 3];
        }
    }

    public o0MagneticCalibraterEllipsoidFitting()
    {
        //Calibration = true;
    }
    
    // public o0MagneticCalibraterEllipsoidFitting(o0Project.Vector3f Center, double[] CorrectMatrix)
    public o0MagneticCalibraterEllipsoidFitting(float[] Center, double[] CorrectMatrix)
    {
        this.Center = Center;
        this.CorrectMatrix = CorrectMatrix;
    }

    [JsonIgnore]
    List<Vector3> records = null;
    [JsonIgnore]
    public Vector3 _Radius = default;
    public float[] Radius
    {
        get
        {
            return new float[]{_Radius.x, _Radius.y, _Radius.z};
        }
        set
        {
            _Radius = new Vector3(value[0], value[1], value[2]);
        }
    }
    [JsonIgnore]
    List<Vector3> BadRecords = null;
    [JsonIgnore]
    public bool Calibration
    {
        get
        {
            return records != null;
        }
        set
        {
            if (value == true)
            {
                records = new List<Vector3>();
            }
            else
            {
                int mag_data_counter = records.Count;    //mag数据数量

                double mag_x, mag_y, mag_z;

                var mat_D = CreateMatrix.Dense<double>(mag_data_counter, 9);

                //读取mag
                for (int i = 0; i < mag_data_counter; i++)
                {
                    //mag_x_y_z赋值
                    mag_x = records[i].x;
                    mag_y = records[i].y;
                    mag_z = records[i].z;

                    mat_D[i, 0] = mag_x * mag_x;
                    mat_D[i, 1] = mag_y * mag_y;
                    mat_D[i, 2] = mag_z * mag_z;
                    mat_D[i, 3] = 2 * mag_x * mag_y;
                    mat_D[i, 4] = 2 * mag_x * mag_z;
                    mat_D[i, 5] = 2 * mag_y * mag_z;
                    mat_D[i, 6] = 2 * mag_x;
                    mat_D[i, 7] = 2 * mag_y;
                    mat_D[i, 8] = 2 * mag_z;

                }

                var mat_DT = mat_D.Transpose();

                var mat_Ones = CreateMatrix.Dense<double>(mag_data_counter, 1, 1.0);

                var mat_Result = (mat_DT * mat_D).Inverse() * (mat_DT * mat_Ones);

                var mat_A_4x4 = CreateMatrix.Dense<double>(4, 4);

                mat_A_4x4[0, 0] = mat_Result[0, 0];
                mat_A_4x4[0, 1] = mat_Result[3, 0];
                mat_A_4x4[0, 2] = mat_Result[4, 0];
                mat_A_4x4[0, 3] = mat_Result[6, 0];

                mat_A_4x4[1, 0] = mat_Result[3, 0];
                mat_A_4x4[1, 1] = mat_Result[1, 0];
                mat_A_4x4[1, 2] = mat_Result[5, 0];
                mat_A_4x4[1, 3] = mat_Result[7, 0];

                mat_A_4x4[2, 0] = mat_Result[4, 0];
                mat_A_4x4[2, 1] = mat_Result[5, 0];
                mat_A_4x4[2, 2] = mat_Result[2, 0];
                mat_A_4x4[2, 3] = mat_Result[8, 0];

                mat_A_4x4[3, 0] = mat_Result[6, 0];
                mat_A_4x4[3, 1] = mat_Result[7, 0];
                mat_A_4x4[3, 2] = mat_Result[8, 0];
                mat_A_4x4[3, 3] = -1.0;


                var mat_Center = -((mat_A_4x4.SubMatrix(0, 3, 0, 3)).Inverse() * mat_Result.SubMatrix(6, 3, 0, 1));
                //椭球圆心					//分块，从0,0开始的3*3的矩阵

                var mat_T_4x4 = CreateMatrix.DenseIdentity<double>(4, 4);
                mat_T_4x4.SetSubMatrix(3, 1, 0, 3, mat_Center.Transpose());

                var mat_R = mat_T_4x4 * mat_A_4x4 * mat_T_4x4.Transpose();

                var evd = mat_R.SubMatrix(0, 3, 0, 3) / -mat_R[3, 3];
                var eig = evd.Evd();

                var mat_Eigval = CreateVector.Dense<double>(3);
                var mat_Evecs = eig.EigenVectors;


                mat_Eigval[0] = eig.EigenValues[0].Real;   //特征值的实部
                mat_Eigval[1] = eig.EigenValues[1].Real;
                mat_Eigval[2] = eig.EigenValues[2].Real;


                var mat_Radii = mat_Eigval.Map(delegate (double x)
                {
                    return 1.0 / Math.Sqrt(Math.Abs(x));
                });    //椭球半径，特征值倒数后开方

                var mat_Scale = CreateMatrix.DenseIdentity<double>(3, 3);
                mat_Scale[0, 0] = mat_Radii[0];
                mat_Scale[1, 1] = mat_Radii[1];
                mat_Scale[2, 2] = mat_Radii[2];

                //double min_Radii = mat_Radii.Minimum();           //返回最小的元素

                mat_Scale = mat_Scale.Inverse();// * min_Radii;
                var mat_Correct = mat_Evecs * mat_Scale * mat_Evecs.Transpose();

                //_Center = new Vector3((float)mat_Center[0], (float)mat_Center[1], (float)mat_Center[2]);

                Debug.Log("The Ellipsoid center is:" + mat_Center.ToString());
                Debug.Log("The Ellipsoid radii is:" + mat_Radii.ToString());
                Debug.Log("The scale matrix is:" + mat_Scale.ToString());
                Debug.Log("The correct matrix is:" + mat_Correct.ToString());

                _Center = new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
                _Radius = new Vector3((float)mat_Radii[0], (float)mat_Radii[1], (float)mat_Radii[2]);
                this._CorrectMatrix = mat_Correct;

                {
                    BadRecords = new List<Vector3>();
                    var AverageDistance = 0f;
                    foreach (var i in records)
                    {
                        var v = i - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
                        var MathNetV = CreateVector.Dense<double>(3);
                        MathNetV[0] = v.x;
                        MathNetV[1] = v.y;
                        MathNetV[2] = v.z;
                        //MathNetV = (MathNetV * mat_Scale) * mat_Correct;
                        MathNetV = (MathNetV) * mat_Correct;
                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
                        AverageDistance += v.magnitude;
                    }
                    AverageDistance /= records.Count;

                    foreach (var i in records)
                    {
                        var v = i - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
                        var MathNetV = CreateVector.Dense<double>(3);
                        MathNetV[0] = v.x;
                        MathNetV[1] = v.y;
                        MathNetV[2] = v.z;
                        //MathNetV = (MathNetV * mat_Scale) * mat_Correct;
                        MathNetV = (MathNetV) * mat_Correct;
                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
                        if(Math.Abs(v.magnitude - AverageDistance) > 0.1* AverageDistance) {
                            BadRecords.Add(i);
                        }
                    }
                    Debug.Log("BadRecords: "+ BadRecords.Count);
                }
                /*
                {
                    var textV = new o0Project.Variance(records.Count);
                    foreach (var i in records)
                    {
                        var v = i - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
                        var MathNetV = CreateVector.Dense<double>(3);
                        MathNetV[0] = v.x;
                        MathNetV[1] = v.y;
                        MathNetV[2] = v.z;
                        //MathNetV = (MathNetV * mat_Scale) * mat_Correct;
                        MathNetV = (MathNetV) * mat_Correct;
                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
                        textV.Update(v.magnitude);
                    }
                    Debug.Log(textV.Value);
                }
                {
                    var textV = new o0Project.Variance(records.Count);
                    foreach (var i in records)
                    {
                        var v = i;
                        var MathNetV = CreateVector.Dense<double>(3);
                        MathNetV[0] = v.x;
                        MathNetV[1] = v.y;
                        MathNetV[2] = v.z;
                        //MathNetV = (MathNetV * mat_Scale) * mat_Correct;
                        MathNetV = (MathNetV) * mat_Correct;
                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]) - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
                        textV.Update(v.magnitude);
                    }
                    Debug.Log(textV.Value);
                }
                {
                    var textV = new o0Project.Variance(records.Count);
                    foreach (var i in records)
                    {
                        var v = i - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
                        var MathNetV = CreateVector.Dense<double>(3);
                        MathNetV[0] = v.x;
                        MathNetV[1] = v.y;
                        MathNetV[2] = v.z;
                        MathNetV = (MathNetV * mat_Scale) * mat_Correct;
                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
                        textV.Update(v.magnitude);
                    }
                    Debug.Log(textV.Value);
                }
                {
                    var textV = new o0Project.Variance(records.Count);
                    foreach (var i in records)
                    {
                        var v = i - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
                        var MathNetV = CreateVector.Dense<double>(3);
                        MathNetV[0] = v.x;
                        MathNetV[1] = v.y;
                        MathNetV[2] = v.z;
                        MathNetV = (MathNetV * mat_Correct) * mat_Scale;
                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
                        textV.Update(v.magnitude);
                    }
                    Debug.Log(textV.Value);
                }
                {
                    var textV = new o0Project.Variance(records.Count);
                    foreach (var i in records)
                    {
                        var v = i - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
                        var MathNetV = CreateVector.Dense<double>(3);
                        MathNetV[0] = v.x;
                        MathNetV[1] = v.y;
                        MathNetV[2] = v.z;
                        MathNetV = MathNetV * mat_Scale;
                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
                        textV.Update(v.magnitude);
                    }
                    Debug.Log(textV.Value);
                }
                {
                    var textV = new o0Project.Variance(records.Count);
                    foreach (var i in records)
                    {
                        var v = i - new Vector3((float)mat_Center[0, 0]/ (float)mat_Radii[0], (float)mat_Center[1, 0]/(float)mat_Radii[1], (float)mat_Center[2, 0]/(float)mat_Radii[2]);
                        textV.Update(v.magnitude);
                    }
                    Debug.Log(textV.Value);
                }
                {
                    var textV = new o0Project.Variance(records.Count);
                    foreach (var i in records)
                    {
                        var v = i -new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
                        textV.Update(v.magnitude);
                    }
                    Debug.Log(textV.Value);
                }/**/

                records = null;
            }
        }
    }
    public Vector3 Update(Vector3 v)
    {
        if (v.magnitude > 30)
            Debug.Log(v);
        if (Calibration)
        {
            records.Add(v);
            return v;
        }
        if(_CorrectMatrix != null)
        {
            v -= _Center;

            var MathNetV = CreateVector.Dense<double>(3);
            MathNetV[0] = v.x;
            MathNetV[1] = v.y;
            MathNetV[2] = v.z;
            //MathNetV = (MathNetV * mat_Scale) * mat_Correct;
            MathNetV = (MathNetV) * _CorrectMatrix;

            v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
            //Debug.Log(v.magnitude);
            return v;
        }
        return v;
    }
    public float CalibratCompletionPercentage()
    {
        return 0;
    }
}
public class o0MagneticCalibraterSimple//默认在无磁干扰环境下，有磁干扰则无法保证效果
{

    [JsonIgnore]
    public Vector3 _Center = Vector3.zero;
    //Vector3 Center = new Vector3(0,0,0);
    [JsonIgnore]
    public Vector3 _Radius = new Vector3(2, 2, 2);

    public o0Project.Vector3f Center
    {
        get
        {
            return new o0Project.Vector3f(_Center.x, _Center.y, _Center.z);
        }
        set
        {
            _Center = new Vector3(value.x, value.y, value.z);
        }
    }
    public o0Project.Vector3f Radius
    {
        get
        {
            return new o0Project.Vector3f(_Radius.x, _Radius.y, _Radius.z);
        }
        set
        {
            _Radius = new Vector3(value.x, value.y, value.z);
        }
    }

    public o0MagneticCalibraterSimple()
    {
        //Calibration = true;
    }
   
    public o0MagneticCalibraterSimple(o0Project.Vector3f Center, o0Project.Vector3f Radius)
    {
        this.Center = Center;
        this.Radius = Radius;
    }

    [JsonIgnore]
    Vector3 Min = new Vector3(float.MinValue, float.MinValue, float.MinValue);
    [JsonIgnore]
    Vector3 Max = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
    [JsonIgnore]
    public bool Calibration
    {
        get
        {
            return !(Min == new Vector3(float.MinValue, float.MinValue, float.MinValue) && Max == new Vector3(float.MaxValue, float.MaxValue, float.MaxValue));
        }
        set
        {
            if (value == true)
            {
                Min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
                Max = new Vector3(float.MinValue, float.MinValue, float.MinValue);
            }
            else
            {
                Min = new Vector3(float.MinValue, float.MinValue, float.MinValue);
                Max = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
            }
        }
    }
    public Vector3 Update(Vector3 v)
    {
        if (v.magnitude > 30)
            Debug.Log(v);
        if (Calibration)
        {
            if (Min.x > v.x)
                Min.x = v.x;
            if (Min.y > v.y)
                Min.y = v.y;
            if (Min.z > v.z)
                Min.z = v.z;
            if (Max.x < v.x)
                Max.x = v.x;
            if (Max.y < v.y)
                Max.y = v.y;
            if (Max.z < v.z)
                Max.z = v.z;
            _Center = (Max + Min) / 2;
            _Radius = (Max - Min) / 2;
            return v;
        }
        v -= _Center;
        v = new Vector3(v.x / _Radius.x, v.y / _Radius.y, v.z / _Radius.z);
        return v;
    }
    public float CalibratCompletionPercentage()
    {
        return 0;
    }
}
public class o0MagneticCalibrater//默认在无磁干扰环境下，有磁干扰则无法保证效果
{

    [JsonIgnore]
    public Vector3 _Center = Vector3.zero;
    //Vector3 Center = new Vector3(0,0,0);
    [JsonIgnore]
    public Vector3 _Radius = new Vector3(2, 2, 2);

    public o0Project.Vector3f Center
    {
        get
        {
            return new o0Project.Vector3f(_Center.x, _Center.y, _Center.z);
        }
        set
        {
            _Center = new Vector3(value.x, value.y, value.z);
        }
    }
    public o0Project.Vector3f Radius
    {
        get
        {
            return new o0Project.Vector3f(_Radius.x, _Radius.y, _Radius.z);
        }
        set
        {
            _Radius = new Vector3(value.x, value.y, value.z);
        }
    }

    public o0MagneticCalibrater()
    {
        //Calibration = true;
    }
    
    public o0MagneticCalibrater(o0Project.Vector3f Center, o0Project.Vector3f Radius)
    {
        this.Center = Center;
        this.Radius = Radius;
    }

    [JsonIgnore]
    HashSet<Vector3> Point = default;
    [JsonIgnore]
    int PointMaxCount = 50;
    [JsonIgnore]
    Dictionary<(Vector3, Vector3), float> Distance = default;
    public void AddPoint(Vector3 v)
    {
        if (Point.Contains(v))
            return;
        foreach (var i in Point)
            Distance.Add((i, v), Vector3.Distance(v, i));
        Point.Add(v);
    }
    public void RemovePoint(Vector3 v)
    {
        Point.Remove(v);
        foreach (var i in Point)
        {
            Distance.Remove((v, i));
            Distance.Remove((i, v));
        }
    }
    public float TotalDistance(Vector3 v)
    {
        float t = 0;
        foreach (var i in Point)
        {
            if (Distance.ContainsKey((i, v)))
            {
                t += Distance[(i, v)];
                continue;
            }
            else if (Distance.ContainsKey((v, i)))
            {
                t += Distance[(v, i)];
                continue;
            }
        }
        return t;
    }
    public Vector3 MinDistancePoint()
    {
        Vector3 minV = default;
        float minD = float.MaxValue;
        foreach (var i in Point)
        {
            float d = TotalDistance(i);
            if (minV == default || minD > d)
            {
                minD = d;
                minV = i;
            }
        }
        return minV;
    }
    public Vector3 RadiusScale()
    {
        Vector3 min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
        Vector3 max = new Vector3(float.MinValue, float.MinValue, float.MinValue);
        foreach (var i in Point)
        {
            if (min.x > i.x)
                min.x = i.x;
            if (min.y > i.y)
                min.y = i.y;
            if (min.z > i.z)
                min.z = i.z;
            if (max.x < i.x)
                max.x = i.x;
            if (max.y < i.y)
                max.y = i.y;
            if (max.z < i.z)
                max.z = i.z;
        }
        return (max - min) / 2;
    }
    [JsonIgnore]
    public bool Calibration
    {
        get
        {
            return Distance != null;
        }
        set
        {
            if (value == true)
            {
                Point = new HashSet<Vector3>();
                Distance = new Dictionary<(Vector3, Vector3), float>();
            }
            else
            {
                Distance = null;
            }
        }
    }
    [JsonIgnore]
    public System.Random r = new System.Random();
    public Vector3 Update(Vector3 v)
    {
        if (v.magnitude > 30)
            Debug.Log(v);
        if (Calibration)
        {
            AddPoint(v);
            if (Point.Count > PointMaxCount)
            {
                RemovePoint(MinDistancePoint());
                _Radius = RadiusScale();
            }


            Vector3 randomV = Point.ElementAt(r.Next(Point.Count));
            var scaledCenter = new Vector3(_Center.x / _Radius.x, _Center.y / _Radius.y, _Center.z / _Radius.z);
            var scaledV = new Vector3(randomV.x / _Radius.x, randomV.y / _Radius.y, randomV.z / _Radius.z);
            float diff = Vector3.Distance(scaledCenter, scaledV) - 1;
            scaledCenter += (scaledV - scaledCenter).normalized * diff * 0.1f;
            _Center = new Vector3(scaledCenter.x * _Radius.x, scaledCenter.y * _Radius.y, scaledCenter.z * _Radius.z);
        }

        /*
        if (diff > 0)
        {
            Center -= v * diff;
        }
        else
        {

        }/**/

        //Point.Add(v);
        //Debug.Log(v.magnitude);
        v -= _Center;
        v = new Vector3(v.x / _Radius.x, v.y / _Radius.y, v.z / _Radius.z);
        return v;
    }
    public float CalibratCompletionPercentage()
    {
        if (Point == null)
            return 0;
        List<float> ScaleDistance = new List<float>();
        foreach (var i in Point)
        {
            var v = i - _Center;
            ScaleDistance.Add(new Vector3(v.x / _Radius.x, v.y / _Radius.y, v.z / _Radius.z).magnitude);
        }
        while (ScaleDistance.Count < PointMaxCount)
            ScaleDistance.Add(0);
        float average = 0;
        foreach (var i in ScaleDistance)
            average += i;
        average /= ScaleDistance.Count;
        float variance = 0;
        foreach (var i in ScaleDistance)
            variance += Mathf.Pow(average - i, 2);
        variance /= ScaleDistance.Count;
        return Mathf.Pow((1 - variance / average), 10) * 100;
        //return variance;
    }
}

public class o0GyrCalibrater
{
    [JsonIgnore]
    public Vector3 _Average = Vector3.zero;
    [JsonIgnore]
    public long Count = -1;
    [JsonIgnore]
    public bool Calibration
    {
        get
        {
            return Count != -1;
        }
        set
        {
            if (value)
                Count = 0;
            else
                Count = -1;
        }
    }
     public float[] Average
    {
        get
        {
            return new float[]{_Average.x, _Average.y, _Average.z};
        }
        set
        {
            _Average = new Vector3(value[0], value[1], value[2]);
        }
    }
    public o0GyrCalibrater()
    {

    }
    //[JsonConstructor, o0.BinarySerialization.Constructor]
    // public o0GyrCalibrater(o0Project.Vector3f Average)
    public o0GyrCalibrater(float[] Average)
    {
        this.Average = Average;
    }
    public Vector3 Update(Vector3 v)
    {
        if (Calibration)
            _Average += (v - _Average) / ++Count;
        v -= _Average;
        if (v.magnitude < 0.0002)
            return Vector3.zero;
        return v;
    }
}
public class o09Axis
{
    // public static List<o0UIRawImageTester> Tester = new List<o0UIRawImageTester>();
    // public static List<Text> TextTester = new List<Text>();

    static Vector3 AccIdentity = new Vector3(0, -1, 0);
    static Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
    public class State
    {
        public long TimeGap;
        public Vector3 Acc = AccIdentity;
        public Vector3 Gyr;
        public Vector3 Mag = MagIdentity;
        public Quaternion Qua;
        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;
    long TimeGapOld;
    /////////////////////g       degree/ms         
    public Quaternion Update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
    {
        //o0UIRawImageTester.UpdateAllOffset();

        //Debug.Log(TimeGapOld);
        var Acc = this.AccOld;
        var Gyr = this.GyrOld;
        var Mag = this.MagOld;
        float TimeGap = (TimeGapOld + this.TimeGapOld) / 2;
        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.Gyr = Gyr;
        state.Mag = Mag;
        //Debug.Log(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度左右
        //Tester?[7].DrawLine(HardwareVarianceAcc.Value, new Color(0, 0, 0));//0.0012左右
        //Tester?[8].DrawLine(HardwareVarianceMag.Value, new Color(0, 0, 0));//3.5左右
        //Debug.Log(HardwareVarianceMag.Value);


        // var Accwit = GameObject.Find("Accwit");
        // var Gyrwit = GameObject.Find("Gyrwit");
        // var Magwit = GameObject.Find("Magwit");
        var LastQuaternion = Last.Qua;
        //var LastQuaternion = Gyrwit.transform.localRotation;

        var newQua = new Quaternion();
        newQua.eulerAngles = Gyr * TimeGap;
        var quaGyr = LastQuaternion * newQua;

        // Accwit.transform.localRotation = o0Project.o0.FormQuaternion(Accwit.transform.localRotation, AccIdentity, Acc, 1);
        // Magwit.transform.localRotation = o0Project.o0.FormQuaternion(Magwit.transform.localRotation, MagIdentity, Mag, 1);

        //Tester?[3].DrawLine(Vector3.Angle(Acc, Last.Acc) / 1, new Color(1, 0, 0));
        //Tester?[4].DrawLine(Quaternion.Angle(LastQuaternion, quaGyr) / 45, new Color(1, 0, 0));
        //Tester?[5].DrawLine(Vector3.Angle(Mag, Last.Mag) / 5, new Color(1, 0, 0));


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


        /*
        float GyrVariance = Last.Variance + (Gyr * TimeGap).magnitude * 0.05f;
        float AccVariance = Mathf.Max(TimeGap / 1, Mathf.Sqrt(Mathf.Pow((Acc.magnitude - 9.8f) / 9.8f * AccLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Acc, Last.Acc) * 0.8f, 2)));
        //Debug.Log(AccVariance);
        float MagVariance = Mathf.Max(TimeGap / 0.2f, Mathf.Sqrt(Mathf.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.05f, 2)));
        state.Variance = state.Variance * AccVariance / (state.Variance + AccVariance);
        state.Variance = state.Variance * MagVariance / (state.Variance + MagVariance);/**/
        //测试效果不错但没迭代的版本


        /*
         * 
        float GyrVariance = state.Variance + TimeGap/100 + (Gyr * TimeGap).magnitude * 0.05f;
        float AccVariance = TimeGap / 30 + Mathf.Sqrt(Mathf.Pow((Acc.magnitude - 9.8f) / 9.8f * AccLengthToAngle, 2)+ Mathf.Pow(Vector3.Angle(Acc,Last.Acc) * 0.5f, 2));
        //Debug.Log(AccVariance);
        float MagVariance = TimeGap / 1 + Mathf.Sqrt(Mathf.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.1f, 2));
        /**/


        //Tester?[1].DrawLine(TimeGap / 100f, new Color(0, 0, 1));
        //Tester?[2].DrawLine((int)(Last.Variance / 90), new Color(0, 0, 0));
        double GyrVariance = Last.Variance + 0.00000002331017 * TimeGap + Math.Pow((Gyr * TimeGap).magnitude * 0.03, 2);// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
        //Debug.Log(Math.Max(0.00000002331017 * TimeGap, Math.Pow((Gyr * TimeGap).magnitude * 0.001f, 2)));
        //Tester?[6].DrawLine((Gyr * TimeGap).magnitude * 0.05f / 90, new Color(0, 0, 0));
        double AccVariance = Math.Max(0.01, Math.Pow((Acc.magnitude - 9.8) / 9.8 * AccLengthToAngle, 4) + Math.Pow(Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc) / TimeGap) * 20, 2));
        //double AccVariance = Math.Max(0.01, Math.Pow((Acc.magnitude - 9.8) / 9.8 * AccLengthToAngle, 4) + Math.Pow(Vector3.Angle(Acc, Last.Acc) * 2, 2));
        //Debug.Log(Vector3.Angle(Mag, Last.Mag));
        double MagVariance = Math.Max(3.5, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 4) + Math.Pow(Vector3.Angle(Mag, Last.Mag) * 0.07, 2));

        /*
        Tester?[1].DrawLine(TimeGap / 100f, new Color(0, 0, 1));
        Tester?[2].DrawLine((int)(Last.Variance / 90), new Color(0, 0, 0));
        double GyrVariance = Last.Variance + Math.Max(0.00000002331017 * TimeGap, Math.Pow((Gyr * TimeGap).magnitude * 0.001f, 2));// 指数4 = 方差2 * 欧拉角旋转误差2     移动导致累计误差
        //Debug.Log(Math.Max(0.00000002331017 * TimeGap, Math.Pow((Gyr * TimeGap).magnitude * 0.001f, 2)));
        Tester?[6].DrawLine((Gyr * TimeGap).magnitude * 0.05f / 90, new Color(0, 0, 0));
        double AccVariance = Vector3.Angle(Acc, Last.Acc) < 0.01 ? 0.0012f : 99999;
        //double AccVariance = Math.Max(0.0012f, Math.Pow((Acc.magnitude - 9.8) / 9.8 * AccLengthToAngle, 2) + Math.Pow(Vector3.Angle(Acc, Last.Acc) * 0.8, 2));
        //Debug.Log(Vector3.Angle(Mag, Last.Mag));
        double MagVariance = Vector3.Angle(Mag, Last.Mag) < 5 && Vector3.Angle(Mag, Last.Mag) != 0 ? 3.5 : 99999;/**/
        //double MagVariance = Math.Max(3.5f, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 2) + Math.Pow(Vector3.Angle(Mag, Last.Mag) * 0.05, 2));
        //Debug.Log(MagVariance);
        state.Variance = GyrVariance;
        state.Variance = state.Variance * (AccVariance+ MagVariance) / (state.Variance + (AccVariance + MagVariance));
        //state.Variance = state.Variance * MagVariance / (state.Variance + MagVariance);
        //Debug.Log(state.Variance);
        //Debug.Log(TextTester[0]);
        //TextTester[0].text = "Variance:" + state.Variance;
        //TextTester[1].text = "GyrVariance:" + GyrVariance;
        //TextTester[2].text = "StaticGyrVariance:" + 0.00000002331017 * TimeGap;
        //TextTester[3].text = "MothonGyrVariance:" + Math.Pow((Gyr * TimeGap).magnitude * 0.07, 2);
        //TextTester[4].text = "GyrSpeed:" + Gyr.magnitude;
        //TextTester[5].text = "AccVariance:" + AccVariance;
        //TextTester[6].text = "AccLengthVariance:" + Math.Pow((Acc.magnitude - 9.8) / 9.8 * AccLengthToAngle, 4);
        //TextTester[7].text = "AccRotate:" + Math.Pow(Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc)/TimeGap)* 20, 2);
        //TextTester[9].text = "AccLength:" + Acc.magnitude;
        //TextTester[10].text = "Gyr*1000,000:" + (Gyr * 1000000).ToString();
        //TextTester[11].text = "AngleBetweenIdentity*1000:" + Quaternion.Angle(Last.Qua, Quaternion.identity) * 1000;
        //TextTester[12].text = "Qua.eulerAngles.x:" + Last.Qua.eulerAngles.x;
        //TextTester[13].text = "Qua.eulerAngles.y:" + Last.Qua.eulerAngles.y;
        //TextTester[14].text = "Qua.eulerAngles.z:" + Last.Qua.eulerAngles.z;
        /*if (Gyr != Vector3.zero)
        {
            Debug.Log(Gyr);
        }/**/

        var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, (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;

        var quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
        // Gyrwit.transform.localRotation = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, Acc, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, Mag, (float)quaSecondRate);

        // state.Qua = Gyrwit.transform.localRotation;

        state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, Acc, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, Mag, (float)quaSecondRate);

        //Tester?[0].DrawLine(TimeGap / 200, new Color(1, 0, 0));
        //Image1.DrawLine();
        //Debug.Log((Gyr * TimeGap).magnitude);
        //Debug.Log(Quaternion.Angle(state.Qua, Last.Qua));
        //TextTester[8].text = "AngleRotated:" + Quaternion.Angle(state.Qua, Last.Qua);


        return state.Qua;
    }

    public void SetIdentity()
    {
        AccIdentity = AccOld;
        MagIdentity = MagOld;
        States.Last().Qua = Quaternion.identity;
        States.Last().Variance = 0.0000001;
    }
    public void SetIdentityAccordingToRecords()
    {
        AccIdentity = Vector3.zero;
        foreach (var i in States)
            AccIdentity += i.Acc;
        AccIdentity /= States.Count;
        MagIdentity = Vector3.zero;
        foreach (var i in States)
            MagIdentity += i.Mag;
        MagIdentity /= States.Count;
        States.Last().Qua = Quaternion.identity;
        States.Last().Variance = 0.0000001;

        Vector3.Angle(Vector3.up, States.Last().Mag);
        
    }
}