From 2e939bd5727c07a927f2d1645f3a7e50bde8e3da Mon Sep 17 00:00:00 2001 From: Ferry Schoenmakers Date: Mon, 6 May 2024 20:58:22 +0200 Subject: [PATCH] Fix correct usage of angular velocity in update_odometry() function --- steering_controllers_library/src/steering_odometry.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index aadd047f2e..6fb20478a4 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -52,7 +52,7 @@ bool SteeringOdometry::update_odometry( const double linear_velocity, const double angular, const double dt) { /// Integrate odometry: - SteeringOdometry::integrate_exact(linear_velocity * dt, angular); + SteeringOdometry::integrate_exact(linear_velocity * dt, angular * dt); /// We cannot estimate the speed with very small time intervals: if (dt < 0.0001) @@ -62,7 +62,7 @@ bool SteeringOdometry::update_odometry( /// Estimate speeds using a rolling mean to filter them out: linear_acc_.accumulate(linear_velocity); - angular_acc_.accumulate(angular / dt); + angular_acc_.accumulate(angular); linear_ = linear_acc_.getRollingMean(); angular_ = angular_acc_.getRollingMean();