|
|
@@ -3,6 +3,7 @@ using System;
|
|
|
using System.Collections.Generic;
|
|
|
using System.Linq;
|
|
|
using UnityEngine;
|
|
|
+using UnityEngine.UI;
|
|
|
|
|
|
public class o0Vector3Filter
|
|
|
{
|
|
|
@@ -22,11 +23,105 @@ public class o0Vector3Filter
|
|
|
return state;
|
|
|
}
|
|
|
}
|
|
|
+public class o0MagneticCalibraterSimple//默认在无磁干扰环境下,有磁干扰则无法保证效果
|
|
|
+{
|
|
|
+
|
|
|
+ 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 o0MagneticCalibraterSimple()
|
|
|
+ {
|
|
|
+ //Calibration = true;
|
|
|
+ }
|
|
|
+ public o0MagneticCalibraterSimple(o0Project.Vector3f Center, o0Project.Vector3f Radius)
|
|
|
+ {
|
|
|
+ this.Center = Center;
|
|
|
+ this.Radius = Radius;
|
|
|
+ }
|
|
|
+
|
|
|
+ Vector3 Min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
|
|
|
+ Vector3 Max = new Vector3(float.MinValue, float.MinValue, float.MinValue);
|
|
|
+ public bool Calibration
|
|
|
+ {
|
|
|
+ get
|
|
|
+ {
|
|
|
+ return Min != default && Max != default;
|
|
|
+ }
|
|
|
+ set
|
|
|
+ {
|
|
|
+ if (value == true)
|
|
|
+ {
|
|
|
+ Min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
|
|
|
+ Max = new Vector3(float.MinValue, float.MinValue, float.MinValue);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ Min = default;
|
|
|
+ Max = default;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ public System.Random r = new System.Random();
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ 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//默认在无磁干扰环境下,有磁干扰则无法保证效果
|
|
|
{
|
|
|
+
|
|
|
public Vector3 _Center = Vector3.zero;
|
|
|
//Vector3 Center = new Vector3(0,0,0);
|
|
|
- public Vector3 _Radius = new Vector3(2,2,2);
|
|
|
+ public Vector3 _Radius = new Vector3(2, 2, 2);
|
|
|
|
|
|
public o0Project.Vector3f Center
|
|
|
{
|
|
|
@@ -55,7 +150,6 @@ public class o0MagneticCalibrater//默认在无磁干扰环境下,有磁干扰
|
|
|
{
|
|
|
//Calibration = true;
|
|
|
}
|
|
|
-
|
|
|
public o0MagneticCalibrater(o0Project.Vector3f Center, o0Project.Vector3f Radius)
|
|
|
{
|
|
|
this.Center = Center;
|
|
|
@@ -90,7 +184,8 @@ public class o0MagneticCalibrater//默认在无磁干扰环境下,有磁干扰
|
|
|
{
|
|
|
t += Distance[(i, v)];
|
|
|
continue;
|
|
|
- }else if (Distance.ContainsKey((v, i)))
|
|
|
+ }
|
|
|
+ else if (Distance.ContainsKey((v, i)))
|
|
|
{
|
|
|
t += Distance[(v, i)];
|
|
|
continue;
|
|
|
@@ -132,16 +227,17 @@ public class o0MagneticCalibrater//默认在无磁干扰环境下,有磁干扰
|
|
|
if (max.z < i.z)
|
|
|
max.z = i.z;
|
|
|
}
|
|
|
- return (max - min)/2;
|
|
|
+ return (max - min) / 2;
|
|
|
}
|
|
|
- public bool Calibration {
|
|
|
+ public bool Calibration
|
|
|
+ {
|
|
|
get
|
|
|
{
|
|
|
return Distance != null;
|
|
|
}
|
|
|
set
|
|
|
{
|
|
|
- if(value == true)
|
|
|
+ if (value == true)
|
|
|
{
|
|
|
Point = new HashSet<Vector3>();
|
|
|
Distance = new Dictionary<(Vector3, Vector3), float>();
|
|
|
@@ -155,12 +251,12 @@ public class o0MagneticCalibrater//默认在无磁干扰环境下,有磁干扰
|
|
|
public System.Random r = new System.Random();
|
|
|
public Vector3 Update(Vector3 v)
|
|
|
{
|
|
|
- if(v.magnitude > 30)
|
|
|
+ if (v.magnitude > 30)
|
|
|
Debug.Log(v);
|
|
|
if (Calibration)
|
|
|
{
|
|
|
AddPoint(v);
|
|
|
- if (Point.Count > 60)
|
|
|
+ if (Point.Count > 100)
|
|
|
{
|
|
|
RemovePoint(MinDistancePoint());
|
|
|
_Radius = RadiusScale();
|
|
|
@@ -201,7 +297,7 @@ public class o0MagneticCalibrater//默认在无磁干扰环境下,有磁干扰
|
|
|
var v = i - _Center;
|
|
|
ScaleDistance.Add(new Vector3(v.x / _Radius.x, v.y / _Radius.y, v.z / _Radius.z).magnitude);
|
|
|
}
|
|
|
- while (ScaleDistance.Count < 60)
|
|
|
+ while (ScaleDistance.Count < 100)
|
|
|
ScaleDistance.Add(0);
|
|
|
float average = 0;
|
|
|
foreach (var i in ScaleDistance)
|
|
|
@@ -211,13 +307,12 @@ public class o0MagneticCalibrater//默认在无磁干扰环境下,有磁干扰
|
|
|
foreach (var i in ScaleDistance)
|
|
|
variance += Mathf.Pow(average - i, 2);
|
|
|
variance /= ScaleDistance.Count;
|
|
|
- return Mathf.Pow((1 - variance / average),10) * 100;
|
|
|
+ return Mathf.Pow((1 - variance / average), 10) * 100;
|
|
|
//return variance;
|
|
|
}
|
|
|
}
|
|
|
-public class o09DOF
|
|
|
+public class o09Axis
|
|
|
{
|
|
|
-
|
|
|
static Vector3 AccIdentity = new Vector3(0, -1, 0);
|
|
|
static Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
|
|
|
public class State
|
|
|
@@ -227,9 +322,13 @@ public class o09DOF
|
|
|
public Vector3 Gyr;
|
|
|
public Vector3 Mag = MagIdentity;
|
|
|
public Quaternion Qua;
|
|
|
- public float Variance = 1;
|
|
|
+ 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);
|
|
|
+
|
|
|
List<State> States = new List<State>();
|
|
|
|
|
|
Vector3 AccOld;
|
|
|
@@ -237,9 +336,8 @@ public class o09DOF
|
|
|
Vector3 MagOld;
|
|
|
long TimeGapOld;
|
|
|
/////////////////////g degree/ms
|
|
|
- public Quaternion o06DOFUpdate(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
|
|
|
+ public Quaternion Update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
|
|
|
{
|
|
|
- //Debug.Log(TimeGapOld);
|
|
|
var Acc = this.AccOld;
|
|
|
var Gyr = this.GyrOld;
|
|
|
var Mag = this.MagOld;
|
|
|
@@ -248,61 +346,54 @@ public class o09DOF
|
|
|
this.GyrOld = GyrOld;
|
|
|
this.MagOld = MagOld;
|
|
|
this.TimeGapOld = TimeGapOld;
|
|
|
- if (this.TimeGapOld <= 0)
|
|
|
- return Quaternion.identity;
|
|
|
var Last = States.LastOrDefault() ?? new State();
|
|
|
+ if (this.TimeGapOld <= 0)
|
|
|
+ return Last.Qua;
|
|
|
States.Add(new State());
|
|
|
- if (States.Count > 10)
|
|
|
+ if (States.Count > 2)
|
|
|
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");/**/
|
|
|
+ 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度左右
|
|
|
+
|
|
|
var LastQuaternion = Last.Qua;
|
|
|
- //var LastQuaternion = Gyrwit.transform.localRotation;
|
|
|
|
|
|
var newQua = new Quaternion();
|
|
|
newQua.eulerAngles = Gyr * TimeGap;
|
|
|
var quaGyr = LastQuaternion * newQua;
|
|
|
|
|
|
+ double AccLengthToAngle = 5;//1倍引力差相当于多少度方差
|
|
|
+ double MagLengthToAngle = 5;//1倍磁力差相当于多少度方差
|
|
|
|
|
|
+ double GyrVariance = Last.Variance + 0.00000002331017 * TimeGap + Math.Pow((Gyr * TimeGap).magnitude * 0.03, 2);// 指数4 = 方差2 * 欧拉角旋转误差2 移动导致累计误差
|
|
|
+ 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 MagVariance = Math.Max(3.5, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 4) + Math.Pow(Vector3.Angle(Mag, Last.Mag) * 0.07, 2));
|
|
|
|
|
|
- float AccLengthToAngle = 180;//1倍引力差相当于多少度方差
|
|
|
- float MagLengthToAngle = 180;//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 / 1000 + (Gyr * TimeGap).magnitude * 0.05f;
|
|
|
- float AccVariance = Mathf.Max(TimeGap / 10, 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 / 1, 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);
|
|
|
-
|
|
|
- var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, AccVariance / (AccVariance + MagVariance));
|
|
|
+ state.Variance = GyrVariance;
|
|
|
+ state.Variance = state.Variance * (AccVariance+ MagVariance) / (state.Variance + (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;
|
|
|
+ var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, (float)(AccVariance / (AccVariance + MagVariance)));
|
|
|
|
|
|
- float quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
|
|
|
- state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, Acc, quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, Mag, quaSecondRate);
|
|
|
+ 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);
|
|
|
|
|
|
+ state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, Acc, (float)quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, Mag, (float)quaSecondRate);
|
|
|
+
|
|
|
return state.Qua;
|
|
|
- //Image1.DrawLine();
|
|
|
+ }
|
|
|
+
|
|
|
+ public void SetIdentity()
|
|
|
+ {
|
|
|
+ AccIdentity = AccOld;
|
|
|
+ MagIdentity = MagOld;
|
|
|
+ States.Last().Qua = Quaternion.identity;
|
|
|
}
|
|
|
}
|