|
|
@@ -4,6 +4,8 @@ using System.Collections.Generic;
|
|
|
using System.Linq;
|
|
|
using UnityEngine;
|
|
|
using MathNet.Numerics.LinearAlgebra;
|
|
|
+using UnityEngine.UI;
|
|
|
+using MathNet.Numerics;
|
|
|
|
|
|
public class o0Vector3Filter
|
|
|
{
|
|
|
@@ -112,139 +114,146 @@ public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- int mag_data_counter = records.Count; //mag数据数量
|
|
|
+ try
|
|
|
+ {
|
|
|
+ int mag_data_counter = records.Count; //mag数据数量
|
|
|
|
|
|
- double mag_x, mag_y, mag_z;
|
|
|
+ double mag_x, mag_y, mag_z;
|
|
|
|
|
|
- var mat_D = CreateMatrix.Dense<double>(mag_data_counter, 9);
|
|
|
+ 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;
|
|
|
+ //读取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_DT = mat_D.Transpose();
|
|
|
|
|
|
- var mat_Ones = CreateMatrix.Dense<double>(mag_data_counter, 1, 1.0);
|
|
|
+ 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_Result = (mat_DT * mat_D).Inverse() * (mat_DT * mat_Ones);
|
|
|
|
|
|
- var mat_A_4x4 = CreateMatrix.Dense<double>(4, 4);
|
|
|
+ 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[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[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[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;
|
|
|
+ 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_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_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 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 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;
|
|
|
+ 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;
|
|
|
+ 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_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];
|
|
|
+ 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(); //返回最小的元素
|
|
|
+ //double min_Radii = mat_Radii.Minimum(); //返回最小的元素
|
|
|
|
|
|
- mat_Scale = mat_Scale.Inverse();// * min_Radii;
|
|
|
- var mat_Correct = mat_Evecs * mat_Scale * mat_Evecs.Transpose();
|
|
|
+ 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]);
|
|
|
+ //_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());
|
|
|
+ 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;
|
|
|
+ _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);
|
|
|
+ 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);
|
|
|
}
|
|
|
- Debug.Log("BadRecords: "+ BadRecords.Count);
|
|
|
+ }
|
|
|
+ catch(NonConvergenceException)
|
|
|
+ {
|
|
|
+ Debug.Log("数据错误无法拟合");
|
|
|
}
|
|
|
/*
|
|
|
{
|
|
|
@@ -738,6 +747,7 @@ public class o09Axis
|
|
|
{
|
|
|
// public static List<o0UIRawImageTester> Tester = new List<o0UIRawImageTester>();
|
|
|
// public static List<Text> TextTester = new List<Text>();
|
|
|
+ public List<Text> TextTester = new List<Text>();
|
|
|
|
|
|
static Vector3 AccIdentity = new Vector3(0, -1, 0);
|
|
|
static Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
|
|
|
@@ -760,6 +770,11 @@ public class o09Axis
|
|
|
public Vector3 AccOld;
|
|
|
public Vector3 GyrOld;
|
|
|
public Vector3 MagOld;
|
|
|
+
|
|
|
+ public float x;
|
|
|
+ public float y;
|
|
|
+ public float z;
|
|
|
+
|
|
|
long TimeGapOld;
|
|
|
/////////////////////g degree/ms
|
|
|
public Quaternion Update(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
|
|
|
@@ -838,13 +853,18 @@ public class o09Axis
|
|
|
|
|
|
//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));
|
|
|
+ // 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));
|
|
|
+
|
|
|
+ double GyrVariance = Last.Variance + 0.00000002331017 * TimeGap + Math.Pow((Gyr * TimeGap).magnitude , 2) * 0.04;// 指数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), 2) * 400);
|
|
|
+ //double AccVariance = Math.Max(0.01, Math.Pow(Math.Max(Gyr.magnitude, Vector3.Angle(Acc, Last.Acc) / TimeGap), 2) * 1000);
|
|
|
+ double MagVariance = Math.Max(3.5, Math.Pow((Mag.magnitude - 1) / 1 * MagLengthToAngle, 4) + Math.Pow(Vector3.Angle(Mag, Last.Mag), 2) * 0.005);
|
|
|
|
|
|
/*
|
|
|
Tester?[1].DrawLine(TimeGap / 100f, new Color(0, 0, 1));
|
|
|
@@ -863,20 +883,20 @@ public class o09Axis
|
|
|
//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;
|
|
|
+ // 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);
|
|
|
@@ -901,15 +921,36 @@ public class o09Axis
|
|
|
//Debug.Log(Quaternion.Angle(state.Qua, Last.Qua));
|
|
|
//TextTester[8].text = "AngleRotated:" + Quaternion.Angle(state.Qua, Last.Qua);
|
|
|
|
|
|
+ var frontV = Last.Qua * Vector3.forward;
|
|
|
+ var upV = Last.Qua * Vector3.up;
|
|
|
|
|
|
+ x = (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90:270));
|
|
|
+ y = (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180+(frontV.x < 0 ? 90:270));
|
|
|
+ z = (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90:270));
|
|
|
+// TextTester[18].text = "x轴角度:" + (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90:270));
|
|
|
+// TextTester[19].text = "y轴角度:" + (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180+(frontV.x < 0 ? 90:270));
|
|
|
+// TextTester[20].text = "z轴角度" + (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90:270));
|
|
|
+
|
|
|
+ TextTester[12].text = "x轴角度:\n" + (Mathf.Atan(upV.y / upV.z) / Mathf.PI * 180 + (upV.z < 0 ? 90:270));
|
|
|
+ TextTester[13].text = "y轴角度:\n" + (Mathf.Atan(frontV.z / frontV.x) / Mathf.PI * 180+(frontV.x < 0 ? 90:270));
|
|
|
+ TextTester[14].text = "z轴角度:\n" + (Mathf.Atan(upV.y / upV.x) / Mathf.PI * 180 + (upV.x < 0 ? 90:270));
|
|
|
return state.Qua;
|
|
|
}
|
|
|
|
|
|
public void SetIdentity()
|
|
|
{
|
|
|
+ Quaternion qua = default;
|
|
|
+
|
|
|
AccIdentity = AccOld;
|
|
|
MagIdentity = MagOld;
|
|
|
+ qua = o0Project.o0.FormQuaternion(Quaternion.identity, Vector3.down,AccIdentity, 1);
|
|
|
+
|
|
|
+ AccIdentity=qua*AccIdentity;
|
|
|
+ MagIdentity = qua*MagIdentity;
|
|
|
+ TextTester[0].text = "AccIdentity:"+AccIdentity;
|
|
|
States.Last().Qua = Quaternion.identity;
|
|
|
+ States.Last().Qua = qua*States.Last().Qua;//Quaternion.identity;
|
|
|
+
|
|
|
States.Last().Variance = 0.0000001;
|
|
|
}
|
|
|
public void SetIdentityAccordingToRecords()
|
|
|
@@ -925,7 +966,6 @@ public class o09Axis
|
|
|
States.Last().Qua = Quaternion.identity;
|
|
|
States.Last().Variance = 0.0000001;
|
|
|
|
|
|
- Vector3.Angle(Vector3.up, States.Last().Mag);
|
|
|
-
|
|
|
+ Vector3.Angle(Vector3.up, States.Last().Mag);
|
|
|
}
|
|
|
}
|