From 5cfe1ad5dc78fb366a4a50f87a3a3d7f51192f1f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 17 Feb 2012 16:15:27 +1100 Subject: [PATCH] DCM: remove the taylor expansion optimisation for renormalisation The sqrt() costs about 44usec on a 2560, which is small enough for us not to worry about the speed. This also changes the range of values where we declare a blowup to much less likely, which means we can cope with larger delta_t glitches --- libraries/AP_DCM/AP_DCM.cpp | 45 +++++++++++++++++++++++++---------- libraries/AP_DCM/AP_DCM.h | 2 +- libraries/AP_DCM/AP_DCM_HIL.h | 2 +- 3 files changed, 35 insertions(+), 14 deletions(-) diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_DCM/AP_DCM.cpp index 765e9d106e..433e3721ab 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_DCM/AP_DCM.cpp @@ -260,21 +260,42 @@ AP_DCM::renorm(Vector3f const &a, int &problem) { float renorm_val; - renorm_val = a * a; + // numerical errors will slowly build up over time in DCM, + // causing inaccuracies. We can keep ahead of those errors + // using the renormalization technique from the DCM IMU paper + // (see equations 18 to 21). - if (renorm_val < 1.5625f && renorm_val > 0.64f) { // Check if we are OK to use Taylor expansion - renorm_val = 0.5 * (3 - renorm_val); // eq.21 - } else if (renorm_val < 100.0f && renorm_val > 0.01f) { - renorm_val = 1.0 / sqrt(renorm_val); - renorm_sqrt_count++; - } else { - problem = 1; - SITL_debug("ERROR: DCM renormalisation error. renorm_val=%f\n", - renorm_val); - renorm_blowup_count++; + // For APM we don't bother with the taylor expansion + // optimisation from the paper as on our 2560 CPU the cost of + // the sqrt() is 44 microseconds, and the small time saving of + // the taylor expansion is not worth the potential of + // additional error buildup. + + // Note that we can get significant renormalisation values + // when we have a larger delta_t due to a glitch eleswhere in + // APM, such as a I2c timeout or a set of EEPROM writes. While + // we would like to avoid these if possible, if it does happen + // we don't want to compound the error by making DCM less + // accurate. + + renorm_val = 1.0 / sqrt(a * a); + + if (renorm_val > 2.0 || renorm_val < 0.5) { + // this is larger than it should get - log it as a warning + renorm_range_count++; + if (renorm_val > 1.0e6 || renorm_val < 1.0e-6) { + // we are getting values which are way out of + // range, we will reset the matrix and hope we + // can recover our attitude using drift + // correction before we hit the ground! + problem = 1; + SITL_debug("ERROR: DCM renormalisation error. renorm_val=%f\n", + renorm_val); + renorm_blowup_count++; + } } - return(a * renorm_val); + return (a * renorm_val); } /**************************************************/ diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h index bf1a539479..1523c80ea9 100644 --- a/libraries/AP_DCM/AP_DCM.h +++ b/libraries/AP_DCM/AP_DCM.h @@ -68,7 +68,7 @@ public: float yaw; // Radians uint8_t gyro_sat_count; - uint8_t renorm_sqrt_count; + uint8_t renorm_range_count; uint8_t renorm_blowup_count; float kp_roll_pitch() { return _kp_roll_pitch; } diff --git a/libraries/AP_DCM/AP_DCM_HIL.h b/libraries/AP_DCM/AP_DCM_HIL.h index ad59081f61..6ebdf13893 100644 --- a/libraries/AP_DCM/AP_DCM_HIL.h +++ b/libraries/AP_DCM/AP_DCM_HIL.h @@ -49,7 +49,7 @@ public: float yaw; // Radians uint8_t gyro_sat_count; - uint8_t renorm_sqrt_count; + uint8_t renorm_range_count; uint8_t renorm_blowup_count;