From 092ede366a531ad68f7ccc2f372f83b8d2993242 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 29 Jun 2014 11:55:44 +0200 Subject: [PATCH] Estimator: Clean up delta quat calculations, put them in a sweet spot between accuracy and runtime. --- .../ekf_att_pos_estimator/estimator_23states.cpp | 12 ++++++++---- 1 file 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 826419cda1..df319a93a4 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