fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 05:23: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/mag_fusion.cpp
+1 -2
View File
@@ -73,8 +73,7 @@ void Ekf::fuseMag()
SH_MAG[8] = 2.0f*magE*q3;
// rotate magnetometer earth field state into body frame
Dcmf R_to_body(_state.quat_nominal);
R_to_body = R_to_body.transpose();
Dcmf R_to_body = quat_to_invrotmat(_state.quat_nominal);
Vector3f mag_I_rot = R_to_body * _state.mag_I;