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

AP_NavEKF2: Use library functions for quaternion corrections

Browse Source
This commit is contained in:
Paul Riseborough 2015-09-23 13:15:14 +10:00 committed by Andrew Tridgell
parent 2470cf0e76
commit f270573acc
2 changed files with 6 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -1566,7 +1566,7 @@ void NavEKF2_core::FuseVelPosNED()
// the first 3 states represent the angular misalignment vector. This is // the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion // 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 // sum the attitude error from velocity and position fusion only
// used as a metric for convergence monitoring // 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 // the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step // 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 // correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the // 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 // the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step // 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 // correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the // 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 // the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step // 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 // correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the // 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 // the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step // 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 // correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the // 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 // Fuse compass measurements usinga simple declination observation model that doesn't use magnetic field states
void NavEKF2_core::fuseCompass() void NavEKF2_core::fuseCompass()
{ {
@ -4962,7 +4940,7 @@ void NavEKF2_core::fuseCompass()
// the first 3 states represent the angular misalignment vector. This is // the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion on the current time step // 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 // 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]; float HP[24];

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -546,9 +546,6 @@ private:
// align the NE earth magnetic field states with the published declination // align the NE earth magnetic field states with the published declination
void alignMagStateDeclination(); 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) // Fuse compass measurements using a simple declination observation (doesn't require magnetic field states)
void fuseCompass(); void fuseCompass();