保存校准数据和更改滤波算法

Assets/BowArrow/Scripts/Bluetooth/BluetoothAim.cs

@@ -5,6 +5,7 @@ using System.Collections.Generic;
 using System.Linq;
 using UnityEngine.UI;
 using Newtonsoft.Json;
+using o0Aien;
 
 public class BluetoothAim : MonoBehaviour
 {
@@ -177,6 +178,8 @@ class AimHandler
     Vector3 Gyr = default;
     Vector3 Mag = default;
     o09Axis _9Axis = new o09Axis();
+    o0SigmoidIntegrationFilterQuaternion filter = new o0SigmoidIntegrationFilterQuaternion(0.2f);
+
     //转换读取的数据，无符号->有符号
     float TwoByteToFloat(byte b1, byte b2) 
     {
@@ -279,15 +282,15 @@ class AimHandler
             });
         }
 
-        GyrCalibrater = new o0GyrCalibrater();
-        string gyrDataStr = PlayerPrefs.GetString("o0GyrCalibrater");
-        if (gyrDataStr.Length > 0)
+        try {
+            string gyrDataStr = PlayerPrefs.GetString("o0GyrCalibrater");
+            GyrCalibrater = JsonConvert.DeserializeObject<o0GyrCalibrater>(gyrDataStr);
+        } catch(Exception) {
+            GyrCalibrater = null;
+        }
+        if (GyrCalibrater == null) 
         {
-            string[] dataStrs = gyrDataStr.Split(',');
-            if (dataStrs.Length == 3)
-            {
-                GyrCalibrater._Average =  new Vector3(float.Parse(dataStrs[0]), float.Parse(dataStrs[1]), float.Parse(dataStrs[2]));
-            }
+            GyrCalibrater = new o0GyrCalibrater();
         }
         if (GyrCalibrationButton != null)
         {
@@ -296,12 +299,7 @@ class AimHandler
                 {
                     GyrCalibrater.Calibration = false;
                     GyrCalibrationButton.GetComponentInChildren<Text>().text = "开始陀螺仪校准";
-                    float[] dataFloats = new float[3];
-                    dataFloats[0] = GyrCalibrater._Average.x;
-                    dataFloats[1] = GyrCalibrater._Average.y;
-                    dataFloats[2] = GyrCalibrater._Average.z;
-                    string dataStr = String.Join(",", dataFloats);
-                    PlayerPrefs.SetString("o0GyrCalibrater", dataStr);
+                    PlayerPrefs.SetString("o0GyrCalibrater", JsonConvert.SerializeObject(GyrCalibrater));
                 }
                 else
                 {
@@ -359,7 +357,7 @@ class AimHandler
             Mag = MagCalibrater.Update(Mag);
             if (MagScaleText != null)
             {
-                MagScaleText.text = MagCalibrater._Radius.ToString() + MagCalibrater.CalibratCompletionPercentage() + "%";
+                MagScaleText.text = MagCalibrater._Radius.ToString();
             }
         }
 
@@ -372,18 +370,18 @@ class AimHandler
         TimeGap = ms - msOld;
         msOld = ms;
 
-        _9Axis.Update(Acc * 10, Gyr, Mag, TimeGap);
-        if(_9Axis.States.Count() < 10){
-            newRotation = _9Axis.States.Last().Qua;
-        }
-        else
-        {
-            int MuliteLerpCount = 10;
-            for (var i = 0;i< MuliteLerpCount;++i)
-            {
-                newRotation = Quaternion.Slerp(newRotation, _9Axis.States[_9Axis.States.Count() - MuliteLerpCount + i].Qua, 1/(i+1));
-            }
-        }
+        newRotation = _9Axis.Update(Acc * 10, Gyr, Mag, TimeGap);
+        // if(_9Axis.States.Count() < 10){
+        //     newRotation = _9Axis.States.Last().Qua;
+        // }
+        // else
+        // {
+        //     int MuliteLerpCount = 10;
+        //     for (var i = 0;i< MuliteLerpCount;++i)
+        //     {
+        //         newRotation = Quaternion.Slerp(newRotation, _9Axis.States[_9Axis.States.Count() - MuliteLerpCount + i].Qua, 1/(i+1));
+        //     }
+        // }
 
 
         receiveDataCount++;
@@ -406,7 +404,10 @@ class AimHandler
         if (hasAutoIdentity && controlObj != null)
         {
             // controlObj.localRotation = Quaternion.Lerp(controlObj.localRotation, newRotation, Time.deltaTime * 6);
-            controlObj.localRotation = newRotation;
+            // controlObj.localRotation = newRotation;
+            Quaternion nowRotation = controlObj.localRotation;
+            filter.Update(ref nowRotation, newRotation);
+            controlObj.localRotation = nowRotation;
             GameObject.Find("Canvas/RPY_LOG").GetComponent<Text>().text = 
                 "roll: " + controlObj.localEulerAngles.x +
                 "\npitch: " + controlObj.localEulerAngles.y +

Assets/BowArrow/Scripts/Bluetooth/o09Axis.cs

@@ -3,7 +3,6 @@ using System.Collections.Generic;
 using System.Linq;
 using UnityEngine;
 using Newtonsoft.Json;
-using MathNet;
 using MathNet.Numerics.LinearAlgebra;
 
 public class o0Vector3Filter
@@ -31,15 +30,15 @@ public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境
     [JsonIgnore]
     Matrix<double> _CorrectMatrix = null;
 
-    public o0Project.Vector3f Center
+    public float[] Center
     {
         get
         {
-            return new o0Project.Vector3f(_Center.x, _Center.y, _Center.z);
+            return new float[]{_Center.x, _Center.y, _Center.z};
         }
         set
         {
-            _Center = new Vector3(value.x, value.y, value.z);
+            _Center = new Vector3(value[0], value[1], value[2]);
         }
     }
     public double[] CorrectMatrix
@@ -67,15 +66,22 @@ public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境
     {
         //Calibration = true;
     }
-    public o0MagneticCalibraterEllipsoidFitting(o0Project.Vector3f 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]
     public bool Calibration
     {
@@ -619,8 +625,11 @@ public class o0MagneticCalibrater//默认在无磁干扰环境下，有磁干扰
 }
 public class o0GyrCalibrater
 {
+    [JsonIgnore]
     public Vector3 _Average = Vector3.zero;
+    [JsonIgnore]
     public long Count = -1;
+    [JsonIgnore]
     public bool Calibration
     {
         get
@@ -635,24 +644,20 @@ public class o0GyrCalibrater
                 Count = -1;
         }
     }
-    public o0Project.Vector3f Average
+    public float[] Average
     {
         get
         {
-            return new o0Project.Vector3f(_Average.x, _Average.y, _Average.z);
+            return new float[]{_Average.x, _Average.y, _Average.z};
         }
         set
         {
-            _Average = new Vector3(value.x, value.y, value.z);
+            _Average = new Vector3(value[0], value[1], value[2]);
         }
     }
     public o0GyrCalibrater()
     {
 
-    }
-    public o0GyrCalibrater(o0Project.Vector3f Average)
-    {
-        this.Average = Average;
     }
     public Vector3 Update(Vector3 v)
     {

Assets/BowArrow/Scripts/Bluetooth/o0Filter.cs

@@ -0,0 +1,434 @@
+using System;
+using UnityEngine;
+
+namespace o0Aien
+{
+    public interface IFilter<T>
+    {
+        void Update(ref T value, T measuredValue);
+    }/**/
+    public abstract class Filter<T>: IFilter<T>
+    {
+        abstract public void Update(ref T value, T measuredValue);
+    }
+    public class NullFilter<T> : Filter<T>
+    {
+        public override void Update(ref T value, T measuredValue)
+        {
+            value = measuredValue;
+        }
+    }
+    public class FilterVector2<TFilter> : Filter<Vector2> where TFilter : Filter<float>
+    {
+        public Filter<float> this[int index]
+        {
+            get
+            {
+                return filter[index];
+            }
+            set
+            {
+                filter[index] = value;
+            }
+        }
+        public Filter<float> X
+        {
+            get
+            {
+                return filter[0];
+            }
+            set
+            {
+                filter[0] = value;
+            }
+        }
+        public Filter<float> Y
+        {
+            get
+            {
+                return filter[1];
+            }
+            set
+            {
+                filter[1] = value;
+            }
+        }
+        protected Filter<float>[] filter;
+        public FilterVector2()
+        {
+        }
+        public FilterVector2(TFilter x, TFilter y)
+        {
+            filter = new Filter<float>[] { x, y };
+        }
+        public override void Update(ref Vector2 value, Vector2 measuredValue)
+        {
+            var v = new float[2];
+            for (var i = 0;i<2;++i)
+                filter[i].Update(ref v[i],measuredValue[i]);
+            value = new Vector2(v[0],v[1]);
+        }
+    }
+    public class FilterVector3<TFilter>: Filter<Vector3> where TFilter : Filter<float>
+    {
+        public Filter<float> this[int index]
+        {
+            get
+            {
+                return filter[index];
+            }
+            set
+            {
+                filter[index] = value;
+            }
+        }
+        public Filter<float> X
+        {
+            get
+            {
+                return filter[0];
+            }
+            set
+            {
+                filter[0] = value;
+            }
+        }
+        public Filter<float> Y
+        {
+            get
+            {
+                return filter[1];
+            }
+            set
+            {
+                filter[1] = value;
+            }
+        }
+        public Filter<float> Z
+        {
+            get
+            {
+                return filter[2];
+            }
+            set
+            {
+                filter[2] = value;
+            }
+        }
+        protected Filter<float>[] filter;
+        public FilterVector3()
+        {
+            filter = new Filter<float>[3];
+        }
+        public FilterVector3(TFilter x, TFilter y, TFilter z)
+        {
+            filter = new Filter<float>[] { x, y, z };
+        }
+        public override void Update(ref Vector3 value, Vector3 measuredValue)
+        {
+            var v = new float[2];
+            for (var i = 0; i < 3; ++i)
+                filter[i].Update(ref v[i], measuredValue[i]);
+            value = new Vector3(v[0], v[1], v[2]);
+        }
+    }
+    public class FilterStoreValue<T>: Filter<T>
+    {
+        protected Filter<T> filter;
+        protected T value = default;
+        public T Value { get
+            {
+                return value;
+            }
+        }
+        public FilterStoreValue(Filter<T> filter)
+        {
+            this.filter = filter;
+        }
+        public T Update(T measuredValue)
+        {
+            filter.Update(ref value, measuredValue);
+            return value;
+        }
+
+        public override void Update(ref T value, T measuredValue)
+        {
+            filter.Update(ref value, measuredValue);
+            this.value = value;
+        }
+    }
+
+
+
+
+
+
+
+    public class o0SigmoidIntegrationFilter : Filter<float>
+    {
+        static public float SigmoidIntegration(float to)//计算优化过的sigmoid算法
+        {
+            if (to > 700)
+                return to - 0.00671534848911797f;
+            return (float)Math.Log(1 + Math.Pow(Math.E, to)) - 0.00671534848911797f;
+        }
+        public o0SigmoidIntegrationFilter(float noise)//数据 的 噪音值大小
+        {
+            this.noise = Mathf.Abs(noise);
+        }
+        //public float value { get; protected set; } = default;
+        float noise = 10;
+        
+        public override void Update(ref float value, float measuredValue)
+        {
+            if (value == default)
+            {
+                value = measuredValue;
+                return;
+            }
+            var noiseBuffer = noise / 10;
+            float sigmoidResult =  SigmoidIntegration(Math.Abs(measuredValue - value) / noiseBuffer - 5) * noiseBuffer;//处理value过大时为无穷
+            if (measuredValue > value)
+                value += sigmoidResult;
+            else
+                value -= sigmoidResult;
+        }
+    }
+    public class o0SigmoidIntegrationFilterVector3 : Filter<Vector3>
+    {
+        public o0SigmoidIntegrationFilterVector3(float noise)
+        {
+            this.noise = Mathf.Abs(noise);
+        }
+        //public Vector3 value { get; protected set; } = default;
+        float noise = 10;
+
+        public override void Update(ref Vector3 value, Vector3 measuredValue)
+        {
+            if (value == default)
+            {
+                value = measuredValue;
+                return;
+            }
+            var noiseBuffer = noise / 10;
+            var differenceVector = measuredValue - value;
+            float sigmoidResult = o0SigmoidIntegrationFilter.SigmoidIntegration(differenceVector.magnitude / noiseBuffer - 5) * noiseBuffer;//处理value过大时为无穷
+            value += sigmoidResult * differenceVector.normalized;
+        }
+    }
+    public class o0SigmoidIntegrationFilterQuaternion : Filter<Quaternion>
+    {
+        public o0SigmoidIntegrationFilterQuaternion(float noise)
+        {
+            this.noise = Mathf.Abs(noise);
+        }
+        //public Vector3 value { get; protected set; } = default;
+        float noise = 10;
+
+        public override void Update(ref Quaternion value, Quaternion measuredValue)
+        {
+            if (value == default)
+            {
+                value = measuredValue;
+                return;
+            }
+            var noiseBuffer = noise / 10;
+            var difference = Quaternion.Angle(measuredValue, value);
+            //var differenceVector = measuredValue - value;
+            float sigmoidResult = o0SigmoidIntegrationFilter.SigmoidIntegration(difference / noiseBuffer - 5) * noiseBuffer;//处理value过大时为无穷
+            value = Quaternion.Slerp(value, measuredValue, sigmoidResult);
+            //value += sigmoidResult * differenceVector.normalized;
+        }
+    }
+
+    public class o0Filter3 : Filter<float>
+    {
+        public o0Filter3(float noise)
+        {
+            this.noise = Mathf.Abs(noise);
+        }
+        //public float value { get; protected set; } = float.MinValue;
+        float noise = 10;
+
+        float accelerationChangeLimit;
+        public override void Update(ref float value, float measuredValue)
+        {
+            if (Mathf.Abs(value - measuredValue) <= noise)
+            {
+                value += (measuredValue - value) * 0.1f;
+                return;
+            }
+            if (measuredValue > value)
+                value += (measuredValue - noise - value) * 0.7f + noise * 0.1f;
+            else
+                value += (measuredValue + noise - value) * 0.7f - noise * 0.1f;
+        }
+    }
+    public class o0Filter: Filter<float>
+    {
+        public o0Filter(float accelerationLimit)
+        {
+        }
+        //public float value { get; protected set; } = float.MinValue;
+        float noise = 6;
+
+        float accelerationChangeLimit;
+        public override void Update(ref float value, float measuredValue)
+        {
+            if (value == float.MinValue)
+            {
+                value = measuredValue;
+                return;
+            }
+            if (Mathf.Abs(measuredValue - value) > noise)
+            {
+                float targetValue = value;
+                if (value < measuredValue - noise)
+                {
+                    targetValue = measuredValue - noise;
+                }
+                else if (value > measuredValue + noise)
+                {
+                    targetValue = measuredValue + noise;
+                }/**/
+                value = value + (targetValue - value) * 0.9f;
+            }   
+            else
+            {
+                value = value + (measuredValue - value) * 0.1f;
+            }
+        }
+    }
+    public class o0AccelerationExFilter: Filter<float>
+    {
+        public o0AccelerationExFilter(float accelerationChangeLimit)//正数
+        {
+            this.accelerationChangeLimit = Mathf.Abs(accelerationChangeLimit);
+        }
+        //public float value { get; protected set; } = float.MinValue;
+        float speed = 0;
+        float acceleration = 0;
+        float accelerationChangeLimit;
+        public override void Update(ref float value, float measuredValue)
+        {
+            if (value == float.MinValue)
+            {
+                value = measuredValue;
+                return;
+            }
+            if (Mathf.Abs(value + speed - measuredValue) <= acceleration + accelerationChangeLimit)
+            {
+                if(Mathf.Abs(value + speed - measuredValue) >= acceleration - accelerationChangeLimit)
+                    acceleration = Mathf.Abs(value + speed - measuredValue);
+                else
+                    acceleration -= accelerationChangeLimit;
+                speed = measuredValue - value;
+                value = measuredValue;
+            }
+            else if (value + speed + acceleration + accelerationChangeLimit < measuredValue)
+            {
+                acceleration += accelerationChangeLimit;
+                speed += acceleration;
+                value += speed;
+            }
+            else
+            {
+                acceleration += accelerationChangeLimit;
+                speed -= acceleration;
+                value += speed;
+            }
+        }
+    }
+    public class o0AccelerationFilter: Filter<float>
+    {
+        public o0AccelerationFilter(float accelerationLimit)//正数
+        {
+            this.accelerationLimit = Mathf.Abs(accelerationLimit);
+        }
+        //public float value { get; protected set; } = float.MinValue;
+        float speed = 0;
+        float accelerationLimit;
+        public override void Update(ref float value, float measuredValue)
+        {
+            if (value == float.MinValue)
+            {
+                value = measuredValue;
+                return;
+            }
+            if (Mathf.Abs(value + speed - measuredValue) <= accelerationLimit)
+            {
+                speed = measuredValue - value;
+                value = measuredValue;
+            }
+            else if (value + speed + accelerationLimit < measuredValue)
+            {
+                speed += accelerationLimit;
+                value += speed;
+            }
+            else
+            {
+                speed -= accelerationLimit;
+                value += speed;
+            }
+        }
+    }
+    public class o0SpeedFilter: Filter<float>
+    {
+        public o0SpeedFilter(float speedLimit)//正数
+        {
+            this.speedLimit = Mathf.Abs(speedLimit);
+        }
+        //public float value { get; protected set; } = float.MinValue;
+        float speedLimit = 0;
+        public override void Update(ref float value, float measuredValue)
+        {
+            if (value == float.MinValue)
+            {
+                value = measuredValue;
+                return;
+            }
+            if (Mathf.Abs(value - measuredValue) <= speedLimit)
+            {
+                value = measuredValue;
+            }
+            else if (value + speedLimit < measuredValue)
+            {
+                value += speedLimit;
+            }
+            else
+            {
+                value -= speedLimit;
+            }
+        }
+    }
+    public class o0KalmanFilter: Filter<float>
+    {
+        float measuredValueDeviation;
+        float estimatedValueDeviation;
+        public o0KalmanFilter(float measuredValueDeviation)
+        {
+            this.measuredValueDeviation = measuredValueDeviation;
+            this.estimatedValueDeviation = measuredValueDeviation;
+        }
+        //public float value { get; protected set; } = float.MinValue;
+        public override void Update(ref float value, float measuredValue)
+        {
+            if (value == float.MinValue)
+            {
+                value = measuredValue;
+                return;
+            }
+            float measuredValueDeviationSquare = measuredValueDeviation * measuredValueDeviation;
+            float estimatedValueDeviationSquare = estimatedValueDeviation * estimatedValueDeviation;
+
+            float H = Mathf.Sqrt(estimatedValueDeviationSquare / (estimatedValueDeviationSquare + measuredValueDeviationSquare));
+            float OptimalValue = value + H * (measuredValue - value);
+            float OptimalValueDeviation = Mathf.Sqrt((1 - H) * estimatedValueDeviationSquare);
+            (value, estimatedValueDeviation) = this.nextEstimatedValue(OptimalValue, OptimalValueDeviation);
+        }
+        public virtual (float, float) nextEstimatedValue(float OptimalValue, float OptimalValueDeviation)
+        {
+            return (OptimalValue, OptimalValueDeviation);
+        }/**/
+    }
+}

Assets/BowArrow/Scripts/Bluetooth/o0Filter.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 33a47ac0a6677a846b87c3cc170c6de0
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: