fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 23:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Make use of inverse rotation function consistent with name

Browse Source 
The quaternion to inverse rotation matrix function has been updated so that the rotation it produces is the inverse to that produced by the matrix library and the the inverse of the quaternion is uses. This function is now used to directly calculate an inverse rotation matrix rather than calculating the forward rotation and then transposing it.
This commit is contained in:
Paul Riseborough
2019-10-23 14:44:08 +03:00
committed by Roman Bapst
parent 36de2b3dc1
commit 4d37065f1b
6 changed files with 12 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+7 -6
View File
@@ -1382,6 +1382,7 @@ Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3
}
// calculate the inverse rotation matrix from a quaternion rotation
// this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator
Matrix3f EstimatorInterface::quat_to_invrotmat(const Quatf &quat)
{
float q00 = quat(0) * quat(0);
@@ -1399,12 +1400,12 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quatf &quat)
dcm(0, 0) = q00 + q11 - q22 - q33;
dcm(1, 1) = q00 - q11 + q22 - q33;
dcm(2, 2) = q00 - q11 - q22 + q33;
dcm(0, 1) = 2.0f * (q12 - q03);
dcm(0, 2) = 2.0f * (q13 + q02);
dcm(1, 0) = 2.0f * (q12 + q03);
dcm(1, 2) = 2.0f * (q23 - q01);
dcm(2, 0) = 2.0f * (q13 - q02);
dcm(2, 1) = 2.0f * (q23 + q01);
dcm(1, 0) = 2.0f * (q12 - q03);
dcm(2, 0) = 2.0f * (q13 + q02);
dcm(0, 1) = 2.0f * (q12 + q03);
dcm(2, 1) = 2.0f * (q23 - q01);
dcm(0, 2) = 2.0f * (q13 - q02);
dcm(1, 2) = 2.0f * (q23 + q01);
return dcm;
}