|
|
|
@ -83,7 +83,7 @@ void NavEKF2_core::FuseRngBcn()
@@ -83,7 +83,7 @@ void NavEKF2_core::FuseRngBcn()
|
|
|
|
|
float t6 = t3*t3; |
|
|
|
|
float t7 = t4*t4; |
|
|
|
|
float t8 = t5+t6+t7; |
|
|
|
|
float t9 = 1.0f/sqrt(t8); |
|
|
|
|
float t9 = 1.0f/sqrtf(t8); |
|
|
|
|
H_BCN[6] = -t4*t9; |
|
|
|
|
H_BCN[7] = -t3*t9; |
|
|
|
|
H_BCN[8] = -t2*t9; |
|
|
|
@ -369,7 +369,7 @@ void NavEKF2_core::FuseRngBcnStatic()
@@ -369,7 +369,7 @@ void NavEKF2_core::FuseRngBcnStatic()
|
|
|
|
|
// calculation will be badly conditioned
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
float t9 = 1.0f/sqrt(t8); |
|
|
|
|
float t9 = 1.0f/sqrtf(t8); |
|
|
|
|
float t10 = rngBcnDataDelayed.beacon_posNED.x*2.0f; |
|
|
|
|
float t15 = receiverPos.x*2.0f; |
|
|
|
|
float t11 = t10-t15; |
|
|
|
@ -498,11 +498,11 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
@@ -498,11 +498,11 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
|
|
|
|
|
float t8 = t5+t6+t7; |
|
|
|
|
float t9; |
|
|
|
|
if (t8 > 0.1f) { |
|
|
|
|
t9 = 1.0f/sqrt(t8); |
|
|
|
|
t9 = 1.0f/sqrtf(t8); |
|
|
|
|
obsDeriv = t2*t9; |
|
|
|
|
|
|
|
|
|
// Calculate innovation
|
|
|
|
|
innov = sqrt(t8) - rngBcnDataDelayed.rng; |
|
|
|
|
innov = sqrtf(t8) - rngBcnDataDelayed.rng; |
|
|
|
|
|
|
|
|
|
// calculate a filtered innovation magnitude to be used to select between the high or low offset
|
|
|
|
|
OffsetMaxInnovFilt = (1.0f - filtAlpha) * bcnPosOffsetMaxVar + filtAlpha * fabsf(innov); |
|
|
|
@ -535,11 +535,11 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
@@ -535,11 +535,11 @@ void NavEKF2_core::CalcRangeBeaconPosDownOffset(float obsVar, Vector3f &vehicleP
|
|
|
|
|
t5 = t2*t2; |
|
|
|
|
t8 = t5+t6+t7; |
|
|
|
|
if (t8 > 0.1f) { |
|
|
|
|
t9 = 1.0f/sqrt(t8); |
|
|
|
|
t9 = 1.0f/sqrtf(t8); |
|
|
|
|
obsDeriv = t2*t9; |
|
|
|
|
|
|
|
|
|
// Calculate innovation
|
|
|
|
|
innov = sqrt(t8) - rngBcnDataDelayed.rng; |
|
|
|
|
innov = sqrtf(t8) - rngBcnDataDelayed.rng; |
|
|
|
|
|
|
|
|
|
// calculate a filtered innovation magnitude to be used to select between the high or low offset
|
|
|
|
|
OffsetMinInnovFilt = (1.0f - filtAlpha) * OffsetMinInnovFilt + filtAlpha * fabsf(innov); |
|
|
|
|