From dce249232189ddf5e645f4fa4d8b09571cb0cf8d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 16 Apr 2024 08:31:22 +1000 Subject: [PATCH] AP_NavEKF3: use filtered gyro in INS position correction this reduces the impact of IMU noise on the output velocity from the EKF --- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index d689ab8591..af10084e6c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -863,7 +863,7 @@ void NavEKF3_core::calcOutputStates() if (!accelPosOffset.is_zero()) { // calculate the average angular rate across the last IMU update // note delAngDT is prevented from being zero in readIMUData() - Vector3F angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT); + Vector3F angRate = dal.ins().get_gyro(gyro_index_active).toftype(); // Calculate the velocity of the body frame origin relative to the IMU in body frame // and rotate into earth frame. Note % operator has been overloaded to perform a cross product