zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF: use exact matrix for trim rotation

mission-4.1.18
Jonathan Challinger 9 years ago committed by Andrew Tridgell
parent
5ff78b8e47
commit
3eba985afd
1 changed files with 1 additions and 2 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      libraries/AP_NavEKF/AP_NavEKF_core.cpp

3
libraries/AP_NavEKF/AP_NavEKF_core.cpp
Unescape 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

Write Preview
Loading…
Cancel
Save