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

AP_NavEKF: use exact matrix for trim rotation

Browse Source
This commit is contained in:
Jonathan Challinger 2016-08-09 11:33:21 -07:00 committed by Andrew Tridgell
parent 5ff78b8e47
commit 3eba985afd
1 changed files with 1 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF/AP_NavEKF_core.cpp
View File

@ -4345,9 +4345,8 @@ void NavEKF_core::setWindVelStates()
// return the transformation matrix from XYZ (body) to NED axes // return the transformation matrix from XYZ (body) to NED axes
void NavEKF_core::getRotationBodyToNED(Matrix3f &mat) const void NavEKF_core::getRotationBodyToNED(Matrix3f &mat) const
{ {
Vector3f trim = _ahrs->get_trim();
state.quat.rotation_matrix(mat); state.quat.rotation_matrix(mat);
mat.rotateXYinv(trim); mat = mat * _ahrs->get_rotation_vehicle_body_to_autopilot_body();
} }
// return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements // return the innovations for the NED Pos, NED Vel, XYZ Mag and Vtas measurements