From 3eba985afdc3b08dc793f9d5784db63a22a09ce7 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Tue, 9 Aug 2016 11:33:21 -0700
Subject: [PATCH] AP_NavEKF: use exact matrix for trim rotation

---
 libraries/AP_NavEKF/AP_NavEKF_core.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF_core.cpp b/libraries/AP_NavEKF/AP_NavEKF_core.cpp
index e2b9146f24..8a7978c1cc 100644
--- a/libraries/AP_NavEKF/AP_NavEKF_core.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF_core.cpp
@@ -4345,9 +4345,8 @@ void NavEKF_core::setWindVelStates()
 // return the transformation matrix from XYZ (body) to NED axes
 void NavEKF_core::getRotationBodyToNED(Matrix3f &mat) const
 {
-    Vector3f trim = _ahrs->get_trim();
     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