diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 4aa60860cc..32623cfe62 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -610,7 +610,7 @@ void AP_AHRS::update_EKF3(void) // use the primary IMU for accel earth frame Vector3f accel = _ins.get_accel(primary_accel); accel -= abias; - _accel_ef = _dcm_matrix * accel; + _accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel; nav_filter_status filt_state; EKF3.getFilterStatus(filt_state);