|
|
@@ -1,8 +1,8 @@
|
|
|
-using System;
|
|
|
+using Newtonsoft.Json;
|
|
|
+using System;
|
|
|
using System.Collections.Generic;
|
|
|
using System.Linq;
|
|
|
using UnityEngine;
|
|
|
-using Newtonsoft.Json;
|
|
|
using MathNet.Numerics.LinearAlgebra;
|
|
|
|
|
|
public class o0Vector3Filter
|
|
|
@@ -66,6 +66,13 @@ public class 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;
|
|
|
@@ -83,6 +90,8 @@ public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境
|
|
|
}
|
|
|
}
|
|
|
[JsonIgnore]
|
|
|
+ List<Vector3> BadRecords = null;
|
|
|
+ [JsonIgnore]
|
|
|
public bool Calibration
|
|
|
{
|
|
|
get
|
|
|
@@ -97,9 +106,6 @@ public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
int mag_data_counter = records.Count; //mag数据数量
|
|
|
|
|
|
double mag_x, mag_y, mag_z;
|
|
|
@@ -109,7 +115,6 @@ public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境
|
|
|
//读取mag
|
|
|
for (int i = 0; i < mag_data_counter; i++)
|
|
|
{
|
|
|
-
|
|
|
//mag_x_y_z赋值
|
|
|
mag_x = records[i].x;
|
|
|
mag_y = records[i].y;
|
|
|
@@ -202,6 +207,39 @@ public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境
|
|
|
_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);
|
|
|
@@ -337,8 +375,10 @@ public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境
|
|
|
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
|
|
|
@@ -368,14 +408,18 @@ public class 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
|
|
|
@@ -430,8 +474,10 @@ public class o0MagneticCalibraterSimple//默认在无磁干扰环境下,有磁
|
|
|
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
|
|
|
@@ -461,14 +507,18 @@ public class 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)
|
|
|
{
|
|
|
@@ -541,6 +591,7 @@ public class o0MagneticCalibrater//默认在无磁干扰环境下,有磁干扰
|
|
|
}
|
|
|
return (max - min) / 2;
|
|
|
}
|
|
|
+ [JsonIgnore]
|
|
|
public bool Calibration
|
|
|
{
|
|
|
get
|
|
|
@@ -560,6 +611,7 @@ public class o0MagneticCalibrater//默认在无磁干扰环境下,有磁干扰
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
+ [JsonIgnore]
|
|
|
public System.Random r = new System.Random();
|
|
|
public Vector3 Update(Vector3 v)
|
|
|
{
|
|
|
@@ -623,6 +675,7 @@ public class o0MagneticCalibrater//默认在无磁干扰环境下,有磁干扰
|
|
|
//return variance;
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
public class o0GyrCalibrater
|
|
|
{
|
|
|
[JsonIgnore]
|
|
|
@@ -644,7 +697,7 @@ public class o0GyrCalibrater
|
|
|
Count = -1;
|
|
|
}
|
|
|
}
|
|
|
- public float[] Average
|
|
|
+ public float[] Average
|
|
|
{
|
|
|
get
|
|
|
{
|
|
|
@@ -658,6 +711,12 @@ public class o0GyrCalibrater
|
|
|
public o0GyrCalibrater()
|
|
|
{
|
|
|
|
|
|
+ }
|
|
|
+ //[JsonConstructor, o0.BinarySerialization.Constructor]
|
|
|
+ // public o0GyrCalibrater(o0Project.Vector3f Average)
|
|
|
+ public o0GyrCalibrater(float[] Average)
|
|
|
+ {
|
|
|
+ this.Average = Average;
|
|
|
}
|
|
|
public Vector3 Update(Vector3 v)
|
|
|
{
|
|
|
@@ -671,6 +730,9 @@ public class o0GyrCalibrater
|
|
|
}
|
|
|
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
|
|
|
@@ -689,13 +751,16 @@ public class o09Axis
|
|
|
|
|
|
public List<State> States = new List<State>();
|
|
|
|
|
|
- Vector3 AccOld;
|
|
|
- Vector3 GyrOld;
|
|
|
- Vector3 MagOld;
|
|
|
+ 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;
|
|
|
@@ -714,26 +779,102 @@ public class o09Axis
|
|
|
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)));
|
|
|
|
|
|
@@ -742,9 +883,19 @@ public class o09Axis
|
|
|
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;
|
|
|
}
|
|
|
|
|
|
@@ -755,7 +906,6 @@ public class o09Axis
|
|
|
States.Last().Qua = Quaternion.identity;
|
|
|
States.Last().Variance = 0.0000001;
|
|
|
}
|
|
|
-
|
|
|
public void SetIdentityAccordingToRecords()
|
|
|
{
|
|
|
AccIdentity = Vector3.zero;
|
|
|
@@ -768,5 +918,8 @@ public class o09Axis
|
|
|
MagIdentity /= States.Count;
|
|
|
States.Last().Qua = Quaternion.identity;
|
|
|
States.Last().Variance = 0.0000001;
|
|
|
+
|
|
|
+ Vector3.Angle(Vector3.up, States.Last().Mag);
|
|
|
+
|
|
|
}
|
|
|
}
|