From 98c8cbb27fca8a2e3876e1eee7defd68df136721 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Wed, 30 Sep 2020 15:05:14 +0200 Subject: [PATCH] ekf2: fix odom body rate sign bug --- src/modules/ekf2/EKF2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index e723273be9..53e689e6c4 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1251,7 +1251,7 @@ void EKF2::publish_odometry(const hrt_abstime ×tamp, const imuSample &imu, // Vehicle odometry angular rates const Vector3f gyro_bias = _ekf.getGyroBias(); - const Vector3f rates(imu.delta_ang * imu.delta_ang_dt); + const Vector3f rates(imu.delta_ang / imu.delta_ang_dt); odom.rollspeed = rates(0) - gyro_bias(0); odom.pitchspeed = rates(1) - gyro_bias(1); odom.yawspeed = rates(2) - gyro_bias(2);