fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 00:53:34 +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/optflow_fusion.cpp
+1 -2
View File
@@ -68,8 +68,7 @@ void Ekf::fuseOptFlow()
float Kfusion[24][2] = {}; // Optical flow Kalman gains
// get rotation matrix from earth to body
Dcmf earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose();
Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
// calculate the sensor position relative to the IMU
Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;