Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Changed sqrt method to sqrtf method.

Browse Source 
AP_NavEKF2: Changed sqrt method to sqrtf method.
This commit is contained in:
murata 2016-12-05 20:23:07 +09:00 committed by Francisco Ferreira
parent 05d9455f87
commit 25e47cce6b
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp
View File

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