Integrated velocity motion model

beluga/include/beluga/motion.hpp

@@ -15,7 +15,7 @@
 #ifndef BELUGA_MOTION_HPP
 #define BELUGA_MOTION_HPP
 
-#include <beluga/motion/ackerman_drive_model.hpp>
+#include <beluga/motion/ackermann_drive_model.hpp>
 #include <beluga/motion/differential_drive_model.hpp>
 #include <beluga/motion/omnidirectional_drive_model.hpp>
 #include <beluga/motion/stationary_model.hpp>

beluga/include/beluga/motion/ackerman_drive_model.hpp → beluga/include/beluga/motion/ackermann_drive_model.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BELUGA_MOTION_ACKERMAN_DRIVE_MODEL_HPP
-#define BELUGA_MOTION_ACKERMAN_DRIVE_MODEL_HPP
+#ifndef BELUGA_MOTION_ACKERMANN_DRIVE_MODEL_HPP
+#define BELUGA_MOTION_ACKERMANN_DRIVE_MODEL_HPP
 
 #include <chrono>
 #include <random>
@@ -36,9 +36,8 @@
 namespace beluga {
 
 /// Velocity components for differential drive motion model.
-struct Velocity {
+struct AckermannControls {
   double v;    ///< Linear velocity (m/s)
-  double w;    ///< Angular velocity (rad/s)
   double phi;  ///< Steering angle (rad)
 };
 
@@ -47,64 +46,61 @@ struct Velocity {
  * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 5.3, particularly table 5.3.
  */
 struct AckermannDriveModelParam {
-  /// Rotational noise from rotational velocity
+  /// Steering noise from steering angle
   /**
-   * How much rotational noise is generated by the rotational velocity.
-   * Also known as `alpha1 in the differential drive model param`.
+   * How much steering noise is generated by the steering angle.
+   * Also known as `alpha1 in the Ackermann drive model param`.
    */
-  double rotation_noise_from_rotation;
-  /// Rotational noise from translation velocity
+  double steering_noise_from_steering;
+  /// Steering noise from linear velocity
   /**
-   * How much rotational noise is generated by the linear velocity.
-   * Also known as `alpha2 in the differential drive model param`.
+   * How much steering noise is generated by the linear velocity.
+   * Also known as `alpha2 in the Ackermann drive model param`.
    */
-  double rotation_noise_from_translation;
-  /// Translational noise from translation velocity
+  double steering_noise_from_velocity;
+  /// Velocity noise from linear velocity
   /**
-   * How much translational noise is generated by the linear velocity.
-   * Also known as `alpha3 in the differential drive model param`.
+   * How much velocity noise is generated by the linear velocity.
+   * Also known as `alpha3 in the Ackermann drive model param`.
    */
-  double translation_noise_from_translation;
-  /// Translational noise from rotational velocity
+  double velocity_noise_from_velocity;
+  /// Velocity noise from steering angle
   /**
-   * How much translational noise is generated by the rotational velocity.
-   * Also known as `alpha4 in the differential drive model param`.
+   * How much velocity noise is generated by the steering angle.
+   * Also known as `alpha4 in the Ackermann drive model param`.
    */
-  double translation_noise_from_rotation;
-  /// Additional orientation noise from translational velocity
+  double velocity_noise_from_steering;
+  /// Additional orientation noise from linear velocity
   /**
    * How much extra orientation noise is generated by the linear velocity.
    * Also known as `alpha6`.
    */
-  double orientation_noise_from_translation;
-  /// Additional orientation noise from rotational velocity
+  double orientation_noise_from_velocity;
+  /// Additional orientation noise from steering angle
   /**
-   * How much extra orientation noise is generated by the rotational velocity.
+   * How much extra orientation noise is generated by the steering angle.
    * Also known as `alpha7`.
    */
-  double orientation_noise_from_rotation;
+  double orientation_noise_from_steering;
 
-  /// Threshold for distinguishing between straight-line and circular motion.
-  /**
-   * Below this threshold (~0.57 degrees), motion is treated as straight-line to avoid
-   * numerical instabilities in radius calculations for nearly-zero angular velocities.
-   */
-  static constexpr double small_angle_threshold = 0.01;
-
-  /// Length of the robot (meters).
+  /// Distance between the rear and front wheel axles (meters).
   /**
    * See \cite Localization and Mapping in Local Occupancy Grid Maps: Simulation
-   * in Ackerman model mobile robot by Ronald A. Cardenas , Jasper W. Huanay
+   * in Ackermann model mobile robot by Ronald A. Cardenas , Jasper W. Huanay
    * and Ivan Calle
    */
   double wheelbase;
 };
 
-/// Sampled velocity model for a differential drive.
+/// Velocity model for a Ackermann drive.
 /**
  * Supports 2D and (flattened) 3D state types.
  * This class satisfies \ref MotionModelPage.
  *
+ * The model is and adaptation using the single track kinematic model
+ * and the noise models of Probabilistic Robotics.
+ * The model serves for any drive that can be simplified to a Single Track vehicle:
+ * ackermann, bicycle, tri-cycle, etc.
  * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 5.3.
  *
  * \tparam StateType Type for particle's state. Either Sophus::SE2d or Sophus::SE3d.
@@ -161,28 +157,28 @@ class AckermannDriveModel {
       const Sophus::SE2d& previous_pose,
       std::chrono::duration<double> delta_time) const {
     // Calculate velocities from poses
-    const auto velocity = calculate_velocities(pose, previous_pose, delta_time);
+    const auto controls = calculate_velocities(pose, previous_pose, delta_time);
 
     // Velocity noise parameters (following velocity motion model from Probabilistic Robotics)
     // Use temporary distributions to safely extract param_type objects
     const auto linear_velocity_distribution = std::normal_distribution<double>{
-        velocity.v, std::sqrt(
-                        params_.translation_noise_from_translation * velocity.v * velocity.v +
-                        params_.translation_noise_from_rotation * velocity.w * velocity.w)};
+        controls.v, std::sqrt(
+                        params_.velocity_noise_from_velocity * controls.v * controls.v +
+                        params_.velocity_noise_from_steering * controls.phi * controls.phi)};
     const auto linear_velocity_params = linear_velocity_distribution.param();
 
     const auto steering_angle_distribution = std::normal_distribution<double>{
-        velocity.phi, std::sqrt(
-                          params_.rotation_noise_from_translation * velocity.v * velocity.v +
-                          params_.rotation_noise_from_rotation * velocity.phi * velocity.phi)};
+        controls.phi, std::sqrt(
+                          params_.steering_noise_from_velocity * controls.v * controls.v +
+                          params_.steering_noise_from_steering * controls.phi * controls.phi)};
     const auto steering_angle_params = steering_angle_distribution.param();
 
     // Additional orientation noise (gamma_hat) using rotation parameters
     const auto gamma_distribution = std::normal_distribution<double>{
         0.0,  // zero mean
         std::sqrt(
-            params_.orientation_noise_from_translation * velocity.v * velocity.v +
-            params_.orientation_noise_from_rotation * velocity.w * velocity.w)};
+            params_.orientation_noise_from_velocity * controls.v * controls.v +
+            params_.orientation_noise_from_steering * controls.phi * controls.phi)};
     const auto gamma_params = gamma_distribution.param();
 
     return [=](const auto& state, auto& gen) {
@@ -200,7 +196,7 @@ class AckermannDriveModel {
   }
 
   /// Calculate linear and angular velocities from two poses and delta time
-  Velocity calculate_velocities(
+  AckermannControls calculate_velocities(
       const Sophus::SE2d& pose,
       const Sophus::SE2d& previous_pose,
       std::chrono::duration<double> delta_time) const {
@@ -210,20 +206,20 @@ class AckermannDriveModel {
     const auto translation = pose.translation() - previous_pose.translation();
     const double chord_distance = translation.norm();
 
+    const auto relative_transform = previous_pose.inverse() * pose;
     // Angular velocity from orientation change
-    const auto angular_change = pose.so2() * previous_pose.so2().inverse();
+    const auto angular_change = relative_transform.so2();
     const double angle_change = angular_change.log();
     const double angular_velocity = angle_change / delta_t_sec;
 
     // Determine direction sign (forward/backward motion)
-    const auto relative_transform = previous_pose.inverse() * pose;
     const double dx = relative_transform.translation().x();
     const double sign = (dx >= 0.0) ? 1.0 : -1.0;
 
     // Linear velocity calculation
     double linear_velocity = 0.0;
-
-    if (std::abs(angle_change) > params_.small_angle_threshold) {
+    double steering_angle = 0.0;
+    if (std::abs(angle_change) > small_angle_threshold) {
       // Circular motion: calculate radius from chord and angle
       // For an arc: chord = 2r·sin(θ/2), therefore r = chord / (2·sin(θ/2))
       const double radius = chord_distance / (2.0 * std::sin(std::abs(angle_change) / 2.0));
@@ -233,20 +229,15 @@ class AckermannDriveModel {
 
       // Linear velocity with direction sign
       linear_velocity = sign * arc_distance / delta_t_sec;
-
+      if (std::abs(linear_velocity) > small_angle_threshold) {
+        const double ratio = params_.wheelbase * angular_velocity / linear_velocity;
+        steering_angle = std::atan(ratio);
+      }
     } else {
       // Straight line motion: v = distance / time
       linear_velocity = sign * chord_distance / delta_t_sec;
     }
-
-    double steering_angle = 0.0;
-
-    if (std::abs(linear_velocity) > params_.small_angle_threshold) {
-      const double ratio = params_.wheelbase * angular_velocity / linear_velocity;
-      steering_angle = std::atan(ratio);
-    }
-
-    return Velocity{linear_velocity, angular_velocity, steering_angle};
+    return AckermannControls{linear_velocity, steering_angle};
   }
   /// Apply velocity motion model to get new pose
   Sophus::SE2d apply_velocity_motion(
@@ -261,7 +252,7 @@ class AckermannDriveModel {
 
     Sophus::SE2d new_pose;
 
-    if (std::abs(omega_hat) < params_.small_angle_threshold) {
+    if (std::abs(omega_hat) < small_angle_threshold) {
       // Nearly straight line motion
       const auto translation =
           Eigen::Vector2d{v_hat * delta_t_sec * std::cos(current_theta), v_hat * delta_t_sec * std::sin(current_theta)};
@@ -277,14 +268,20 @@ class AckermannDriveModel {
       const auto new_theta = current_theta + omega_hat * delta_t_sec + gamma_hat * delta_t_sec;
       new_pose = Sophus::SE2d{Sophus::SO2d{new_theta}, state.translation() + translation};
     }
-
     return new_pose;
   }
 
   param_type params_;
+
+  /// Threshold for distinguishing between straight-line and circular motion.
+  /**
+   * Below this threshold (~0.57 degrees), motion is treated as straight-line to avoid
+   * numerical instabilities in radius calculations for nearly-zero angular velocities.
+   */
+  static constexpr double small_angle_threshold = 0.01;
 };
 
-/// Alias for a 2D Ackerman drive model, for convenience.
+/// Alias for a 2D Ackermann drive model, for convenience.
 using AckermannDriveModel2d = AckermannDriveModel<Sophus::SE2d>;
 }  // namespace beluga
 

beluga/test/beluga/CMakeLists.txt

@@ -33,7 +33,7 @@ add_executable(
   algorithm/test_unscented_transform.cpp
   containers/test_circular_array.cpp
   containers/test_tuple_vector.cpp
-  motion/test_ackerman_drive_model.cpp
+  motion/test_ackermann_drive_model.cpp
   motion/test_differential_drive_model.cpp
   motion/test_omnidirectional_drive_model.cpp
   policies/test_every_n.cpp

beluga/test/beluga/motion/test_ackerman_drive_model.cpp → beluga/test/beluga/motion/test_ackermann_drive_model.cpp

@@ -33,7 +33,7 @@
 #include <range/v3/view/take_exactly.hpp>
 
 #include "beluga/3d_embedding.hpp"
-#include "beluga/motion/ackerman_drive_model.hpp"
+#include "beluga/motion/ackermann_drive_model.hpp"
 #include "beluga/test/motion_utils.hpp"
 #include "beluga/testing/sophus_matchers.hpp"
 
@@ -48,13 +48,13 @@ using beluga::testing::SE2Near;
 
 using UUT = beluga::AckermannDriveModel2d;
 
-class AckermanDriveModelTest : public ::testing::Test {
+class AckermannDriveModelTest : public ::testing::Test {
  protected:
   const UUT motion_model_{beluga::AckermannDriveModelParam{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5}};
   std::mt19937 generator_{std::random_device()()};
 };
 
-TEST_F(AckermanDriveModelTest, OneUpdate) {
+TEST_F(AckermannDriveModelTest, OneUpdate) {
   constexpr double kTolerance = 0.001;
   const auto base_pose_in_odom = SE2d{SO2d{Constants::pi()}, Vector2d{1.0, -2.0}};
   const auto previous_pose_in_odom = SE2d{SO2d{Constants::pi()}, Vector2d{1.0, -2.0}};
@@ -67,7 +67,7 @@ TEST_F(AckermanDriveModelTest, OneUpdate) {
   ASSERT_THAT(state_sampling_function(pose, generator_), SE2Near(pose, kTolerance));
 }
 
-TEST_F(AckermanDriveModelTest, Translate) {
+TEST_F(AckermannDriveModelTest, Translate) {
   constexpr double kTolerance = 0.001;
   const auto base_pose_in_odom = SE2d{SO2d{0.0}, Vector2d{1.0, 0.0}};
   const auto previous_pose_in_odom = SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}};
@@ -83,7 +83,7 @@ TEST_F(AckermanDriveModelTest, Translate) {
   ASSERT_THAT(result2, SE2Near(SO2d{0.0}, Vector2d{1.0, 3.0}, kTolerance));
 }
 
-TEST_F(AckermanDriveModelTest, ArcOfCircumference) {
+TEST_F(AckermannDriveModelTest, ArcOfCircumference) {
   constexpr double kTolerance = 0.001;
   const auto base_pose_in_odom = SE2d{SO2d{Constants::pi() / 2}, Vector2d{0.0, 1.0}};
   const auto previous_pose_in_odom = SE2d{SO2d{0.0}, Vector2d{0.0, 0.0}};
@@ -114,7 +114,7 @@ auto get_statistics(Range&& range) {
   return std::pair{mean, stddev};
 }
 
-TEST(AckermanDriveModelSamples, Translate) {
+TEST(AckermannDriveModelSamples, Translate) {
   const double tolerance = 0.015;
   const double alpha = 0.2;
   const double origin = 5.0;
@@ -140,7 +140,7 @@ TEST(AckermanDriveModelSamples, Translate) {
   ASSERT_NEAR(stddev, std::sqrt(alpha * expected_velocity * expected_velocity) * delta_time, tolerance);
 }
 
-TEST(AckermanDriveModelSamples, ArcOfCircumference) {
+TEST(AckermannDriveModelSamples, ArcOfCircumference) {
   const double tolerance = 0.1;
   const double alpha = 0.2;
   const auto motion_model = UUT{beluga::AckermannDriveModelParam{0.0, 0.0, 0.0, alpha, 0.0, 0.0, 0.5}};
@@ -176,8 +176,8 @@ TEST(AckermanDriveModelSamples, ArcOfCircumference) {
   ASSERT_NEAR(orientation_mean, Constants::pi() / 2, tolerance);
 
   // Noise propagation: φ̂ = φ + N(0, α₄ω²) through ω̂ = v̂ * tan(φ̂) / L affects final orientation
-  // The steering angle model produces this empirically observed noise level
-  const double expected_orientation_stddev = 0.35;
+  // The Ackermann steering angle model produces this empirically observed noise level
+  const double expected_orientation_stddev = 0.006;
   ASSERT_NEAR(orientation_stddev, expected_orientation_stddev, tolerance);
 }
 }  // namespace

beluga_amcl/docs/ros2-reference.md

@@ -121,39 +121,39 @@ Also available as a standalone `amcl_node` executable.
 ##### Motion Model Parameters
 
 `robot_model_type` _(`string`)_
-: Which odometry motion model to use. Supported models are `differential_drive` {cite}`thrun2005probabilistic`, `omnidirectional_drive`, `ackerman_drive` and `stationary`.
+: Which odometry motion model to use. Supported models are `differential_drive` {cite}`thrun2005probabilistic`, `omnidirectional_drive`, `ackermann_drive` and `stationary`.
 : Defaults to `differential_drive`.
 
 `alpha1` _(`float`)_
-: Expected process noise in odometry’s rotation estimate from rotation for the `differential_drive`, `ackerman_drive` and `omnidirectional_drive` models. Must be nonnegative.
+: Expected process noise in odometry’s rotation estimate from rotation for the `differential_drive`, `ackermann_drive` and `omnidirectional_drive` models. Must be nonnegative.
 : Defaults to `0.2`.
 
 `alpha2` _(`float`)_
-: Expected process noise in odometry’s rotation estimate from translation for the `differential_drive`, `ackerman_drive` and `omnidirectional_drive` models. Must be nonnegative.
+: Expected process noise in odometry’s rotation estimate from translation for the `differential_drive`, `ackermann_drive` and `omnidirectional_drive` models. Must be nonnegative.
 : Defaults to `0.2`.
 
 `alpha3` _(`float`)_
-: Expected process noise in odometry’s translation estimate from translation for the `differential_drive`, `ackerman_drive` and `omnidirectional_drive` models. Must be nonnegative.
+: Expected process noise in odometry’s translation estimate from translation for the `differential_drive`, `ackermann_drive` and `omnidirectional_drive` models. Must be nonnegative.
 : Defaults to `0.2`.
 
 `alpha4` _(`float`)_
-: Expected process noise in odometry’s translation estimate from rotation for the `differential_drive`, `ackerman_drive` and `omnidirectional_drive` models. Must be nonnegative.
+: Expected process noise in odometry’s translation estimate from rotation for the `differential_drive`, `ackermann_drive` and `omnidirectional_drive` models. Must be nonnegative.
 : Defaults to `0.2`.
 
 `alpha5` _(`float`)_
 : Expected process noise in odometry's strafe estimate from translation for the `omnidirectional_drive` model. Must be nonnegative.
 : Defaults to `0.2`.
 
 `alpha6` _(`float`)_
-: Expected process noise in odometry's orientation noise from translational velocity for the `ackerman_drive` model. Must be nonnegative.
+: Expected process noise in odometry's orientation noise from translational velocity for the `ackermann_drive` model. Must be nonnegative.
 : Defaults to `0.2`.
 
 `alpha7` _(`float`)_
-: Expected process noise in odometry's orientation noise from rotational velocity for the `ackerman_drive` model. Must be nonnegative.
+: Expected process noise in odometry's orientation noise from rotational velocity for the `ackermann_drive` model. Must be nonnegative.
 : Defaults to `0.2`.
 
 `wheelbase` _(`float`)_
-: Expected length of the robot for the `ackerman_drive` model. Must be nonnegative.
+: Expected length of the robot for the `ackermann_drive` model. Must be nonnegative.
 : Defaults to `0.5`.
 ##### Observation Model Parameters
 

beluga_amcl/include/beluga_amcl/ros2_common.hpp

@@ -48,8 +48,8 @@ constexpr std::string_view kStationaryModelName = "stationary";
 constexpr std::string_view kNav2DifferentialModelName = "nav2_amcl::DifferentialMotionModel";
 /// String identifier for a omnidirectional model name.
 constexpr std::string_view kNav2OmnidirectionalModelName = "nav2_amcl::OmniMotionModel";
-/// String identifier for a ackerman drive model.
-constexpr std::string_view kAckermanDriveModelName = "ackerman_drive";
+/// String identifier for a ackermann drive model.
+constexpr std::string_view kAckermannDriveModelName = "ackermann_drive";
 /// Supported execution policies.
 using ExecutionPolicyVariant = std::variant<std::execution::sequenced_policy, std::execution::parallel_policy>;