From f270573acc2c0755cbf5966110a7ce7df48a46d8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 23 Sep 2015 13:15:14 +1000 Subject: [PATCH] AP_NavEKF2: Use library functions for quaternion corrections --- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 34 +++++------------------- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 3 --- 2 files changed, 6 insertions(+), 31 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index b60b0b07ce..55bf754087 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -1566,7 +1566,7 @@ void NavEKF2_core::FuseVelPosNED() // the first 3 states represent the angular misalignment vector. This is // is used to correct the estimated quaternion - correctQuatStates(stateStruct.angErr); + stateStruct.quat.rotate(stateStruct.angErr); // sum the attitude error from velocity and position fusion only // used as a metric for convergence monitoring @@ -1915,7 +1915,7 @@ void NavEKF2_core::FuseMagnetometer() // the first 3 states represent the angular misalignment vector. This is // is used to correct the estimated quaternion on the current time step - correctQuatStates(stateStruct.angErr); + stateStruct.quat.rotate(stateStruct.angErr); // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the @@ -2084,7 +2084,7 @@ void NavEKF2_core::FuseAirspeed() // the first 3 states represent the angular misalignment vector. This is // is used to correct the estimated quaternion on the current time step - correctQuatStates(stateStruct.angErr); + stateStruct.quat.rotate(stateStruct.angErr); // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the @@ -2272,7 +2272,7 @@ void NavEKF2_core::FuseSideslip() // the first 3 states represent the angular misalignment vector. This is // is used to correct the estimated quaternion on the current time step - correctQuatStates(stateStruct.angErr); + stateStruct.quat.rotate(stateStruct.angErr); // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the @@ -2695,7 +2695,7 @@ void NavEKF2_core::FuseOptFlow() // the first 3 states represent the angular misalignment vector. This is // is used to correct the estimated quaternion on the current time step - correctQuatStates(stateStruct.angErr); + stateStruct.quat.rotate(stateStruct.angErr); // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in KH to reduce the @@ -4849,28 +4849,6 @@ bool NavEKF2_core::getLastYawResetAngle(float &yawAng) } } -// correct the quaternion using an attitude error vector -void NavEKF2_core::correctQuatStates(Vector3f &errVec) -{ - // Convert the error rotation vector to its equivalent quaternion where - // truth = estimate + error - float rotationMag = errVec.length(); - if (rotationMag > 1e-6f) { - Quaternion deltaQuat; - float temp = sinf(0.5f*rotationMag) / rotationMag; - deltaQuat[0] = cosf(0.5f*rotationMag); - deltaQuat[1] = errVec.x*temp; - deltaQuat[2] = errVec.y*temp; - deltaQuat[3] = errVec.z*temp; - - // Update the quaternion states by rotating from the previous attitude through the error quaternion - stateStruct.quat *= deltaQuat; - - // re-normalise the quaternion - stateStruct.quat.normalize(); - } -} - // Fuse compass measurements usinga simple declination observation model that doesn't use magnetic field states void NavEKF2_core::fuseCompass() { @@ -4962,7 +4940,7 @@ void NavEKF2_core::fuseCompass() // the first 3 states represent the angular misalignment vector. This is // is used to correct the estimated quaternion on the current time step - correctQuatStates(stateStruct.angErr); + stateStruct.quat.rotate(stateStruct.angErr); // correct the covariance using P = P - K*H*P taking advantage of the fact that only the first 3 elements in H are non zero float HP[24]; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index c0aa15f6d1..66a32f8567 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -546,9 +546,6 @@ private: // align the NE earth magnetic field states with the published declination void alignMagStateDeclination(); - // correct the quaternion using an attitude error vector - void correctQuatStates(Vector3f &errVec); - // Fuse compass measurements using a simple declination observation (doesn't require magnetic field states) void fuseCompass();