diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-29 11:55:44 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-29 11:55:44 +0200 |
commit | 092ede366a531ad68f7ccc2f372f83b8d2993242 (patch) | |
tree | 4ca3bd63fc6c655c5a7fc1320b072ffa451dd6ab | |
parent | cd2ef000ebba7da47a30fbdeb09635bef3a2684a (diff) | |
download | px4-firmware-092ede366a531ad68f7ccc2f372f83b8d2993242.tar.gz px4-firmware-092ede366a531ad68f7ccc2f372f83b8d2993242.tar.bz2 px4-firmware-092ede366a531ad68f7ccc2f372f83b8d2993242.zip |
Estimator: Clean up delta quat calculations, put them in a sweet spot between accuracy and runtime.
-rw-r--r-- | src/modules/ekf_att_pos_estimator/estimator_23states.cpp | 12 |
1 files changed, 8 insertions, 4 deletions
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 826419cda..df319a93a 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -71,7 +71,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float rotationMag; float qUpdated[4]; float quatMag; - double deltaQuat[4]; + float deltaQuat[4]; const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; // Remove sensor bias errors @@ -104,8 +104,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED() } else { - deltaQuat[0] = cos(0.5f*rotationMag); - double rotScaler = (sin(0.5f*rotationMag))/rotationMag; + // We are using double here as we are unsure how small + // the angle differences are and if we get into numeric + // issues with float. The runtime impact is not measurable + // for these quantities. + deltaQuat[0] = cos(0.5*(double)rotationMag); + float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag; deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; @@ -1659,7 +1663,7 @@ void AttPosEKF::FuseRangeFinder() // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data SH_RNG[4] = sin(rngFinderPitch); - cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch); + cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); if (useRangeFinder && cosRngTilt > 0.87f) { // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset |