|
|
@@ -148,20 +148,20 @@ namespace o0.Bow
|
|
|
|
|
|
|
|
|
if (double.IsInfinity(GyrVariance))
|
|
|
- GyrVariance = double.MinValue;
|
|
|
+ GyrVariance = 0.0000001;
|
|
|
if (double.IsNaN(GyrVariance))
|
|
|
- GyrVariance = double.MinValue;
|
|
|
+ GyrVariance = 0.0000001;
|
|
|
if (double.IsNaN(AccVariance))
|
|
|
- AccVariance = double.MinValue;
|
|
|
+ AccVariance = 0.0000001;
|
|
|
if (double.IsNaN(MagVariance))
|
|
|
- MagVariance = double.MinValue;
|
|
|
+ MagVariance = 0.0000001;
|
|
|
|
|
|
state.Variance = GyrVariance;
|
|
|
state.Variance = state.Variance * (AccVariance + MagVariance) / (state.Variance + (AccVariance + MagVariance));
|
|
|
|
|
|
|
|
|
if (double.IsNaN(state.Variance))
|
|
|
- state.Variance = double.MinValue;
|
|
|
+ state.Variance = 0.0000001;
|
|
|
|
|
|
var quaAccMag = Geometry.Quaternion.FormQuaternion(AccIdentity, MagIdentity, state.Acc, state.Mag, (float)(AccVariance / (AccVariance + MagVariance)));
|
|
|
var quaMinRate = GyrVariance / (GyrVariance + Math.Max(AccVariance, MagVariance));
|