|
|
@@ -141,10 +141,11 @@ namespace o0.Bow
|
|
|
// TestVector.SetAcc(state.AccSmooth.ToUnityVector(), 1);
|
|
|
// TestVector.SetMag(state.MagSmooth.ToUnityVector(), 1);
|
|
|
}
|
|
|
+ public static double vari = 0.001;
|
|
|
public double CalGyrVariance(State Last, State state)
|
|
|
{
|
|
|
//return Last.Variance + (state.Gyr * state.TimeGap).Length * 0.3 + 0.001;
|
|
|
- return Last.Variance + (state.Gyr * state.TimeGap).Length * 0.3 + 0.001 * state.TimeGap;//0.00000002331017
|
|
|
+ return Last.Variance + (state.Gyr * state.TimeGap).Length * 0.3 + vari * state.TimeGap;//0.00000002331017
|
|
|
}
|
|
|
public double CalAccVariance(State Last, State state)
|
|
|
{
|
|
|
@@ -184,7 +185,7 @@ namespace o0.Bow
|
|
|
|| double.IsNaN(quaternion.x) || double.IsNaN(quaternion.y) || double.IsNaN(quaternion.z) || double.IsNaN(quaternion.w))
|
|
|
quaternion = backUp;
|
|
|
}
|
|
|
-
|
|
|
+ // public static string debugInfo = "";
|
|
|
public Geometry.Quaternion Process9Axis(State Last, State state)
|
|
|
{
|
|
|
MagSmooth(Last, state);
|
|
|
@@ -198,6 +199,15 @@ namespace o0.Bow
|
|
|
o0UIRawImageTester.UpdateAllOffset();
|
|
|
|
|
|
//Debug.Log(Gyr.Length);
|
|
|
+ // debugInfo = $"Acc:{Acc}\n";
|
|
|
+ // debugInfo += $"AccLen:{Acc.Length}\n";
|
|
|
+ // debugInfo += $"Gyr:{Gyr}\n";
|
|
|
+ // debugInfo += $"GyrLen:{Gyr.Length}\n";
|
|
|
+ // debugInfo += $"Mag:{Mag}\n";
|
|
|
+ // debugInfo += $"MagLen:{Mag.Length}\n";
|
|
|
+ // debugInfo += $"TimeGap:{TimeGap}\n";
|
|
|
+ // debugInfo += $"GyrMeanLen:{Attitude.GyrCalibrater.Mean.Length}\n";
|
|
|
+ // debugInfo += $"MagVariance:{Attitude.MagCalibrater.Variance}\n";
|
|
|
|
|
|
var LastQuaternion = Last.QuaSmooth;
|
|
|
|