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

using ArduinoBluetoothAPI;
using System;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

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 o0MagneticCalibrater//默认在无磁干扰环境下，有磁干扰则无法保证效果
{
    public Vector3 _Center = Vector3.zero;
    //Vector3 Center = new Vector3(0,0,0);
    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;
    }

    HashSet<Vector3> Point = default;
    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;
    }
    public bool Calibration {
        get
        {
            return Distance != null;
        }
        set
        {
            if(value == true)
            {
                Point = new HashSet<Vector3>();
                Distance = new Dictionary<(Vector3, Vector3), float>();
            }
            else
            {
                Distance = null;
            }
        }
    }
    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 > 100)
            {
                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 < 100)
            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 o09DOF 
{

    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 float Variance = 1;
    }

    List<State> States = new List<State>();

    Vector3 AccOld;
    Vector3 GyrOld;
    Vector3 MagOld;
    long TimeGapOld;
    /////////////////////g       degree/ms         
    public Quaternion o06DOFUpdate(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
    {
        //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;
        if (this.TimeGapOld <= 0)
            return Quaternion.identity;
        var Last = States.LastOrDefault() ?? new State();
        States.Add(new State());
        if (States.Count > 10)
            States.RemoveAt(0);
        var state = States.Last();
        state.Acc = Acc;
        state.Gyr = Gyr;
        state.Mag = Mag;
        //Debug.Log(TimeGap);

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



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


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

        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 / 100 + Mathf.Sqrt(Mathf.Pow((Mag.magnitude - 500) / 500 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.1f, 2));
        state.Variance = state.Variance * AccVariance / (state.Variance + AccVariance);
        state.Variance = state.Variance * MagVariance / (state.Variance + MagVariance);

        var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, AccVariance / (AccVariance + MagVariance));

        var quaMinRate = GyrVariance / (GyrVariance + Mathf.Max(AccVariance, MagVariance));
        var quaMaxRate = GyrVariance / (GyrVariance + Mathf.Min(AccVariance, MagVariance));
        Quaternion quaFirst = Quaternion.Slerp(quaGyr, quaAccMag, quaMinRate).normalized;

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


        return state.Qua;
        //Image1.DrawLine();
    }
}