Improve odometry of overdetermined measurements

steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp

@@ -72,7 +72,7 @@ class SteeringOdometry
    * \brief Updates the odometry class with latest wheels position
    * \param right_traction_wheel_pos  Right traction wheel velocity [rad]
    * \param left_traction_wheel_pos  Left traction wheel velocity [rad]
-   * \param front_steer_pos Steer wheel position [rad]
+   * \param steer_pos Steer wheel position [rad]
    * \param dt      time difference to last call
    * \return true if the odometry is actually updated
    */
@@ -231,6 +231,16 @@ class SteeringOdometry
    */
   double convert_twist_to_steering_angle(const double v_bx, const double omega_bz);
 
+  /**
+   * \brief Calculates linear velocity of a robot with double traction axle
+   * \param right_traction_wheel_vel  Right traction wheel velocity [rad/s]
+   * \param left_traction_wheel_vel  Left traction wheel velocity [rad/s]
+   * \param steer_pos Steer wheel position [rad]
+   */
+  double get_lin_velocity_double_traction_axle(
+    const double right_traction_wheel_vel, const double left_traction_wheel_vel,
+    const double steer_pos);
+
   /**
    *  \brief Reset linear and angular accumulators
    */

steering_controllers_library/src/steering_odometry.cpp

@@ -129,13 +129,26 @@ bool SteeringOdometry::update_from_velocity(
   return update_odometry(linear_velocity, angular_velocity, dt);
 }
 
+double SteeringOdometry::get_lin_velocity_double_traction_axle(
+  const double right_traction_wheel_vel, const double left_traction_wheel_vel,
+  const double steer_pos)
+{
+  double turning_radius = wheelbase_ / std::tan(steer_pos);
+  // overdetermined, we take the average
+  double vel_r = right_traction_wheel_vel * wheel_radius_ * turning_radius /
+                 (turning_radius + wheel_track_ * 0.5);
+  double vel_l = left_traction_wheel_vel * wheel_radius_ * turning_radius /
+                 (turning_radius - wheel_track_ * 0.5);
+  return (vel_r + vel_l) * 0.5;
+}
+
 bool SteeringOdometry::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double steer_pos, const double dt)
 {
-  double linear_velocity =
-    (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5;
   steer_pos_ = steer_pos;
+  double linear_velocity = get_lin_velocity_double_traction_axle(
+    right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_);
 
   const double angular_velocity = tan(steer_pos_) * linear_velocity / wheelbase_;
 
@@ -146,9 +159,17 @@ bool SteeringOdometry::update_from_velocity(
   const double right_traction_wheel_vel, const double left_traction_wheel_vel,
   const double right_steer_pos, const double left_steer_pos, const double dt)
 {
-  steer_pos_ = (right_steer_pos + left_steer_pos) * 0.5;
-  double linear_velocity =
-    (right_traction_wheel_vel + left_traction_wheel_vel) * wheel_radius_ * 0.5;
+  // overdetermined, we take the average
+  const double right_steer_pos_est = std::atan(
+    wheelbase_ * std::tan(right_steer_pos) /
+    (wheelbase_ - wheel_track_ / 2 * std::tan(right_steer_pos)));
+  const double left_steer_pos_est = std::atan(
+    wheelbase_ * std::tan(left_steer_pos) /
+    (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos)));
+  steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5;
+
+  double linear_velocity = get_lin_velocity_double_traction_axle(
+    right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_);
   const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_;
 
   return update_odometry(linear_velocity, angular_velocity, dt);

steering_controllers_library/test/test_steering_odometry.cpp

@@ -36,7 +36,7 @@ TEST(TestSteeringOdometry, ackermann_odometry)
   odom.set_wheel_params(1., 1., 1.);
   odom.set_odometry_type(steering_odometry::ACKERMANN_CONFIG);
   ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1));
-  EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3);
+  EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3);
   EXPECT_NEAR(odom.get_angular(), .1, 1e-3);
   EXPECT_NEAR(odom.get_x(), .1, 1e-3);
   EXPECT_NEAR(odom.get_heading(), .01, 1e-3);
@@ -226,7 +226,7 @@ TEST(TestSteeringOdometry, tricycle_odometry)
   odom.set_wheel_params(1., 1., 1.);
   odom.set_odometry_type(steering_odometry::TRICYCLE_CONFIG);
   ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1));
-  EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3);
+  EXPECT_NEAR(odom.get_linear(), 1.002, 1e-3);
   EXPECT_NEAR(odom.get_angular(), .1, 1e-3);
   EXPECT_NEAR(odom.get_x(), .1, 1e-3);
   EXPECT_NEAR(odom.get_heading(), .01, 1e-3);