Integrated velocity motion model by fbattocchia · Pull Request #501 · Ekumen-OS/beluga

fbattocchia · 2025-09-10T18:16:32Z

Proposed changes

This branch adds integrated velocity motion model to the Beluga

Type of change

🐛 Bugfix (change which fixes an issue)
🚀 Feature (change which adds functionality)
📚 Documentation (change which fixes or extends documentation)

💥 Breaking change! Explain why a non-backwards compatible change is necessary or remove this line entirely if not applicable.

Checklist

Put an x in the boxes that apply. This is simply a reminder of what we will require before merging your code.

Lint and unit tests (if any) pass locally with my changes
I have added tests that prove my fix is effective or that my feature works
I have added necessary documentation (if appropriate)
All commits have been signed for DCO

Additional comments

Anything worth mentioning to the reviewers.

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

fbattocchia · 2025-09-10T18:40:48Z

@glpuga @hidmic I have developed an initial implementation of the Integrated Velocity Motion Model based on the "Probabilistic Robotics" textbook. Below, I outline the design decisions and seek guidance on next steps:
Current implementation features:

Noise parameters: I used 4 parameters (α1-α4) maintaining compatibility with the existing odometric model, instead of the 6 parameters (α1-α6) specified in the textbook. For the additional orientation noise calculation (γ̂), I reused parameters α3 and α4.
Extended interface: I preserved the reception of current and previous poses, adding corresponding timestamps (current time and previous time) to calculate the time interval internally.
Development status: The implementation is in its initial phase, without unit tests or AMCL integration.

Questions for review:

Noise parameters: Do you consider it appropriate to maintain 4 parameters for compatibility, or would it be preferable to implement the complete 6 parameters (α5, α6) from the theoretical model?
Model selection: What would be the recommended mechanism for selecting between the existing odometric model and the new velocity model when integrating with amcl.hpp?
Interface validation: Is the proposed extended interface with separate timestamps consistent with what is desired for the project?

I look forward to your feedback on these aspects before proceeding with testing and full system integration into AMCL.

hidmic · 2025-09-11T11:45:53Z

Do you consider it appropriate to maintain 4 parameters for compatibility, or would it be preferable to implement the complete 6 parameters (α5, α6) from the theoretical model?

I'm a bit of a broken record, but there's no fundamental reason why we should maintain some form of backwards compatibility for new features. In this particular case, if the new model has 6 noise parameters, 6 noise parameters it is.

What would be the recommended mechanism for selecting between the existing odometric model and the new velocity model when integrating with amcl.hpp?

For simplicity's sake, the same as for other models. But that doesn't match the expectations of existing models taking a pair of poses. In this specific case, you can probably get away with changing that pair of poses to a pair of timestamped poses everywhere. We are due for something like:

template <typename T, typename ClockT = std::chrono::steady_clock>
struct TimeStamped {
    T value;
    std::chrono::time_point<ClockT> timestamp;
};

in the core. FWIW tf2 does something similar (though in a less modern fashion).

Is the proposed extended interface with separate timestamps consistent with what is desired for the project?

See above.

hidmic

First pass, high level comments. Great work @fbattocchia !

hidmic · 2025-10-01T12:26:30Z

beluga/include/beluga/motion/omnidirectional_drive_model.hpp

+    const auto& [timestamped, previous_timestamped] = action;
+    const auto& pose = timestamped.value;
+    const auto& previous_pose = previous_timestamped.value;


@fbattocchia meta: I'm not super fond of requiring a type we don't need. OmnidirectionalDriveModel does not use timestamps. One thing you can do (and that should) work is simply add:

operator const T&() const { return value; }

to TimeStamped<T>. If you do that, even if control_type is std::tuple<T, T> and you pass in a std::tuple<TimeStamped<T>, TimeStamped<T>> it will work (because of the common_tuple_type_t magic). Then you don't need to change the older motion models.

It is a good solution!

hidmic · 2025-10-01T12:30:11Z

beluga/include/beluga/utility/time_stamped.hpp

+/// Helper function to create a control action tuple with TimeStamped values
+template <typename T>
+auto make_control_action(const T& current, const T& previous) {
+  return std::make_tuple(TimeStamped{current}, TimeStamped{previous});
+}
+
+/// Helper function to create a control action tuple with TimeStamped values and explicit timestamps
+template <typename T, typename ClockT = std::chrono::system_clock>
+auto make_control_action(
+    const T& current,
+    const T& previous,
+    std::chrono::time_point<ClockT> current_timestamp,
+    std::chrono::time_point<ClockT> previous_timestamp) {
+  return std::make_tuple(
+      TimeStamped<T, ClockT>{current, current_timestamp}, TimeStamped<T, ClockT>{previous, previous_timestamp});
+}


@fbattocchia hmm, this is adding semantics to a type that has none. Consider moving the helper to tests where we need them. Users can call std::make_tuple which is a lot more general already.

hidmic · 2025-10-01T12:32:00Z

beluga/include/beluga/algorithm/amcl_core.hpp

   *         or std::nullopt if no update was performed.
   */
-  auto update(state_type control_action, measurement_type measurement) -> std::optional<estimation_type> {
+  auto update(TimeStamped<state_type> control_action, measurement_type measurement) -> std::optional<estimation_type> {


@fbattocchia rather than forcing this here, take the control action type from the motion model like we do for the measurement type.

This still applies. Just pull control_type from MotionModel. If they don't define it (they should 🤔), define it.

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

hidmic

Second pass. Solid work 💪

hidmic · 2025-10-03T19:17:39Z

beluga/include/beluga/motion.hpp

 * \file
 * \brief Includes all Beluga motion models.
+ *
+ * Motion models in Beluga can be classified into two categories:


@fbattocchia nit: consider rephrasing this to:

Suggested change

* Motion models in Beluga can be classified into two categories:

* Motion models in Beluga include:

We may add more models in the future that don't fit these categories.

hidmic · 2025-10-03T19:18:00Z

beluga/include/beluga/motion/ackermann_drive_model.hpp

+
+/// Parameters to construct a VelocityDriveModel instance.
+/**
+ * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 5.3, particularly table 5.3.


@fbattocchia 💯

hidmic · 2025-10-03T19:22:32Z

beluga/include/beluga/motion/ackerman_drive_model.hpp

+    // Linear velocity calculation
+    double linear_velocity = 0.0;
+
+    if (std::abs(angle_change) > 1e-6) {


@fbattocchia nit: consider using an proper epsilon for this ie. std::numeric_limits<double>::epsilon(). Same elsewhere.

hidmic · 2025-10-03T19:25:43Z

beluga/include/beluga/motion/ackermann_drive_model.hpp

@@ -0,0 +1,269 @@
+// Copyright 2022-2023 Ekumen, Inc.


@fbattocchia this file needs a new name I think.

I was in doubt about the name, so that it is the same as the class it could be called velocity_drive_model.hpp what do you think?

Hmm, maybe differential velocity drive model is the least surprising name for this. It's not entirely accurate but it's not opaque either.

hidmic · 2025-10-03T19:30:59Z

beluga/include/beluga/motion/ackerman_drive_model.hpp

+  }
+
+  /// Calculate linear and angular velocities from two poses and delta time
+  std::pair<double, double>


@fbattocchia nit: consider using a custom struct for this. v and w read better than first and second.

hidmic · 2025-10-03T19:34:05Z

beluga/include/beluga/motion/ackerman_drive_model.hpp

+  double orientation_noise_from_rotation;
+};
+
+/// Sampled velocity model for a differential drive.


@fbattocchia documentation needs an update I think

I Changed odometry to velocity and I updated the reference to the book

hidmic · 2025-10-03T19:35:15Z

beluga/include/beluga/motion/differential_drive_model.hpp

+
  /// Current and previous odometry estimates as motion model control action.
-  using control_type = std::tuple<state_type, state_type>;
+  using control_type = std::tuple<timestamped_state_type, timestamped_state_type>;


@fbattocchia earlier comment still applies. This model doesn't need timestamped state.

hidmic · 2025-10-03T19:36:58Z

beluga_amcl/src/amcl_nodelet.cpp

  const auto update_start_time = std::chrono::high_resolution_clock::now();
  const auto new_estimate = particle_filter_->update(
-      base_pose_in_odom,  //
+      {base_pose_in_odom, laser_scan_stamp},  //


@fbattocchia fyi: you can also retrieve the stamp from the looked up transform message above. Same value, code may read better.

Actually, explicitly constructing the Timestamped<Sophus::SE2d> above may be better than implicitly constructing it here. Same elsewhere.

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

fbattocchia · 2025-10-06T17:32:04Z

@hidmic Thanks for the suggestions, I'm attentive to any changes that may be needed.

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

hidmic

Last pass. Amazing work. Barring a few more comments, this LGTM.

hidmic · 2025-10-08T17:35:52Z

beluga/include/beluga/motion/omnidirectional_drive_model.hpp

    const auto& [pose, previous_pose] = action;

-    const auto translation = pose.translation() - previous_pose.translation();
+    const auto translation = pose->translation() - previous_pose->translation();


@fbattocchia if we make this change, it won't work when pose is actually of state_type.

You can work around this with:

Suggested change

const auto translation = pose->translation() - previous_pose->translation();

const state_type& pose = std::get<0>(action);

const state_type& previous_pose = std::get<1>(action);

const auto translation = pose.translation() - previous_pose.translation();

We have to sacrifice structured bindings but it'll work in all cases.

It's a good idea. I had considered this solution too, but I wasn't sure if the change would be okay.

hidmic · 2025-10-08T17:37:43Z

beluga/test/beluga/motion/test_differential_drive_model.cpp

 TEST_F(DifferentialDriveModelTest, OneUpdate) {
  constexpr double kTolerance = 0.001;
-  const auto control_action = std::make_tuple(
+  const auto control_action = beluga::testing::make_control_action(


@fbattocchia for tests that apply to models that don't need timestamped control actions, consider keeping them as they were, without timestamps, and adding a few more tests that use timestamps. That way we ensure it works both ways.

hidmic · 2025-10-08T17:38:13Z

beluga_amcl/docs/ros2-reference.md


 `robot_model_type` _(`string`)_
-: Which odometry motion model to use. Supported models are `differential_drive` {cite}`thrun2005probabilistic`, `omnidirectional_drive` and `stationary`.
+: Which odometry motion model to use. Supported models are `differential_drive` {cite}`thrun2005probabilistic`, `omnidirectional_drive`, `ackerman_drive` and `stationary`.


@fbattocchia meta: is it really ackerman_drive? Same elsewhere.

Notice that Ackermann has double N. We need to fix in filenames an class names.

hidmic · 2025-10-08T17:39:52Z

beluga_amcl/src/amcl_nodelet.cpp

-        base_pose_in_odom);
+    odom_to_base_transform =
+        tf_buffer_->lookupTransform(config_.odom_frame_id, config_.base_frame_id, laser_scan->header.stamp);
+    tf2::convert(odom_to_base_transform.transform, base_pose_in_odom);


@fbattocchia nit^N!, feel free to ignore: we could have a tf2::convert(const geometry_msgs::TransformStamped&, Timestamped<Sophus::SE2d>&) overload too.

Implemented! I liked the suggestion.

glpuga

Looking good! Left a few comments. Take a look at the comment regarding the difference between modelling a diff drive velocity model and the single track velocity model.

glpuga · 2025-10-08T20:08:52Z

beluga/include/beluga/motion/differential_velocity_drive_model.hpp

+    auto time = timestamped.timestamp;
+    auto previous_time = previous_timestamped.timestamp;


nit: both can be const

glpuga · 2025-10-08T20:10:14Z

beluga/include/beluga/motion/differential_velocity_drive_model.hpp

+    const auto delta_time = std::chrono::duration<double>(time - previous_time).count();
+    if constexpr (std::is_same_v<state_type, Sophus::SE2d>) {
+      return sampling_fn_2d(pose, previous_pose, delta_time);
+    } else {
+      return sampling_fn_3d(pose, previous_pose, delta_time);
+    }


is it possible to keep the delta_t in a std::chrono::duration, so that it retains units until we actually need to do some calculation?

glpuga · 2025-10-08T20:14:01Z

beluga/include/beluga/motion/differential_velocity_drive_model.hpp

+  [[nodiscard]] auto sampling_fn_3d(const Sophus::SE3d& pose, const Sophus::SE3d& previous_pose, double delta_time)
+      const {
+    const auto current_pose_2d = To2d(pose);
+    const auto previous_pose_pose_2d = To2d(previous_pose);
+    const auto two_d_sampling_fn = sampling_fn_2d(current_pose_2d, previous_pose_pose_2d, delta_time);
+    return [=](const state_type& state, auto& gen) { return To3d(two_d_sampling_fn(To2d(state), gen)); };
+  }


What's the use of providing support for 3D state (and flattening it) if this model is 2D only? Do we need this to support some other model? they are all 2D AFAIK.

glpuga · 2025-10-08T20:35:38Z

beluga/include/beluga/motion/differential_velocity_drive_model.hpp

+    // Calculate velocities from poses
+    const auto velocity = calculate_velocities(pose, previous_pose, delta_time);
+
+    using DistributionParam = typename std::normal_distribution<double>::param_type;


I don't think we can do this. I don't think we can do assumptions regarding the (undefined) normal_distribution::param_type , in particular of what arguments it takes for construction.

The normal_distribution(const param_type& parm) is meant to be used with the param() function member of a preexisting instance, not from an instance of param_type created by us. In particular, I found no definitions of what the constructor of such a type must look like, which makes me think it's implementation dependent.

A normal_distribution object is defined by two parameters: its mean (μ) and its standard deviation (stddev, &sigma). An object of type param_type carries this information, but it is meant to be used only to construct or specify the parameters for a normal_distribution object, not to inspect the individual parameters.

The risk is this not being portable. If we are lucky, this might be a build error, but if we are not it might be an invisible error if instead of storing the mean and the std, an implementation decides to store the mean and the variance instead.

I looked into this because I would have expected the second constructor argument to be the variance, not the standard deviation.

To solve this, I can avoid building param_type directly. Instead, I can create instances of std::normal_distribution with the correct parameters, but the other models are using this same implementation. If necessary, I can change it in the other models as well after this pr.

glpuga · 2025-10-08T20:40:11Z

beluga/include/beluga/motion/differential_velocity_drive_model.hpp

+    const auto linear_velocity_params = DistributionParam{
+        velocity.v, std::sqrt(
+                        params_.translation_noise_from_translation * std::abs(velocity.v) +
+                        params_.translation_noise_from_rotation * std::abs(velocity.w))};
+
+    const auto angular_velocity_params = DistributionParam{
+        velocity.w, std::sqrt(
+                        params_.rotation_noise_from_translation * std::abs(velocity.v) +
+                        params_.rotation_noise_from_rotation * std::abs(velocity.w))};
+
+    // Additional orientation noise (gamma_hat) using rotation parameters
+    const auto gamma_params = DistributionParam{
+        0.0,  // zero mean
+        std::sqrt(
+            params_.orientation_noise_from_translation * std::abs(velocity.v) +
+            params_.orientation_noise_from_rotation * std::abs(velocity.w))};


If I'm reading tables 5.1 and 5.3 correctly, everything in std::abs must instead be squared before multiplication. std::abs would be correct only if they were outside of the square root.

glpuga · 2025-10-08T20:43:10Z

beluga/include/beluga/motion/differential_velocity_drive_model.hpp

+    // Angular velocity from orientation change
+    const auto angular_change = pose.so2() * previous_pose.so2().inverse();
+    const double angle_change = angular_change.log();
+    const double angular_velocity = angle_change / delta_time;


Do we check if delta_t == 0 somewhere?

if Δt is zero, the resulting velocity is also zero, so the particle remains exactly in the same initial state.

This makes the filter too rigid, as there is no dispersion in the particle cloud.

In this case, is it advisable to maintain a small amount of noise to allow for some dispersion even when Δt = 0?
In other words, is it advisable to inject noise even if there is no deterministic motion?

Is there a standard way or practice for handling this case in the probabilistic motion model?

glpuga · 2025-10-08T20:50:27Z

beluga/include/beluga/motion/differential_velocity_drive_model.hpp

+    const auto forward_direction =
+        Eigen::Vector2d{std::cos(previous_pose.so2().log()), std::sin(previous_pose.so2().log())};
+    const double dot_product = translation.dot(forward_direction);


I think you can use the unit_complex() method of the pose instead to get the complex equivalente with x() and y() members being the projections youre calculating trigonometrically.

If fact, log() is doing the exact opposite calculation, so this will be expensive if benchmarked: https://github.com/strasdat/Sophus/blob/d0b7315a0d90fc6143defa54596a3a95d9fa10ec/sophus/so2.hpp#L165

In any case, an alternative calculation is to calculate the current-pose-in-the-previous-pose-frame (which you're already partly doing in line 201 for the orientation), and check if the A-to-B transform that results has positive X coordinate.

glpuga · 2025-10-08T21:16:09Z

beluga/include/beluga/motion/differential_velocity_drive_model.hpp

+    if (std::abs(angle_change) > std::numeric_limits<double>::epsilon()) {
+      // 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));
+
+      // Arc length: s = r · θ
+      const double arc_distance = radius * std::abs(angle_change);
+
+      // Linear velocity with direction sign
+      linear_velocity = sign * arc_distance / delta_time;


This calculation is probably not numerically very stable for straight motion. In regular straight motion, the angle change will never be exactly zero (or below epsilon), it will just be a very small number. For the purposes of this code, the sin of a very small number is is the same number (in the limit), so follow me here:

we willl calculate radius, a huge number, because we divide a regular number by something that is basically half of angle-change, a very small number.

that exploding radius is then multiplied by that small number again to get the arc_distance, which gets something in human scale again, but only if resolution were infinite. You're using doubles (accurate to 15 digits) , so you have a lot of leeway here, but very small angles might give totally unreasonable numbers if the radius cannot be represented accurately.

I think we should take a shortcut here if std::abs(angle_change) < 0.01, and in that case:

arc_distance = chord_distance

the error in this cheap approximation is the error in the 1 / (2 * sin(0.01 / 2)) == 1 / 0.01 for small x, which a quick calculation tells me is about 10e-5 and otherwise problem-free, so totally worth it.

glpuga · 2025-10-08T21:34:48Z

beluga/include/beluga/motion/differential_velocity_drive_model.hpp

+using VelocityDriveModel2d = DifferentialVelocityDriveModel<Sophus::SE2d>;
+
+/// Alias for a 3D velocity drive model, for convenience.
+using VelocityDriveModel3d = DifferentialVelocityDriveModel<Sophus::SE3d>;


I don't think DifferentialVelocityDriveModel is a good name, and it might be that this is not the best model.
Remember that we aimed at creating a motion model for Ackermann/Bicycle (more generally SingleTrack ). Check the first reference in https://github.com/ekumenlabs/monaco_f1tenth/issues/24 .

A differential can have 0 translation and yet non-zero angular velocity; a Single Track cannot. This extra freedom probably accounts for the extra noise sources.

The models are quite similar, but I think the Single Track is the one we are aiming for.

@glpuga I will schedule a meeting to discuss this part.

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

glpuga · 2025-10-18T11:49:33Z

Not to lose track of our goals, this is the ticket for this task: https://github.com/ekumenlabs/monaco_f1tenth/issues/58

glpuga · 2025-10-18T12:01:04Z

Also, reminder that we aim of the task is to create a prototype to measure and decide if we continue this, not a polished version. We don't want to spend time polishing code we may decide to throw away after measuring.

…ckerman Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

fbattocchia · 2025-10-26T15:53:30Z

beluga/include/beluga/motion/ackermann_drive_model.hpp

+   * Also known as `alpha7`.
+   */
+  double orientation_noise_from_rotation;
+


@glpuga The alpha6 and alpha7 parameters are from probabilistic robotics to calculate additional angular noise in the final orientation. In this hybrid model for the Ackerman, I could eliminate it and not use this additional noise. would it be correct?

As we discussed in the meeting yesterday, let's keep them. From the arguments in Probabilistic Roboticis, they have the same reason to exist in our hybrid model as they do in the diff model.

fbattocchia · 2025-10-26T15:56:27Z

beluga/include/beluga/motion/ackerman_drive_model.hpp

+    if (std::abs(linear_velocity) > params_.small_angle_threshold) {
+      const double ratio = params_.wheelbase * angular_velocity / linear_velocity;
+      // TODO: use Taylor series for small angles to avoid atan?
+      steering_angle = std::atan(ratio);


@glpuga std::atan and std::tan are computationally expensive. Would it be better to implement some other calculation? Like the Taylor series?

I think we are fine, I don't think it's worth optimizing here.

glpuga

Left a few more comments.

glpuga · 2025-10-29T15:09:20Z

beluga/include/beluga/motion/ackerman_drive_model.hpp

+struct Velocity {
+  double v;    ///< Linear velocity (m/s)
+  double w;    ///< Angular velocity (rad/s)
+  double phi;  ///< Steering angle (rad)
+};
+


Feels like we don't need both w and phi, since they are related to eachother by the wheelbase. Also, the struct probably should be renamed as "AckermannControls" (if w is removed and v and phi are left).

glpuga · 2025-10-29T15:10:44Z

beluga/include/beluga/motion/ackerman_drive_model.hpp

+   * How much rotational noise is generated by the rotational velocity.
+   * Also known as `alpha1 in the differential drive model param`.
+   */
+  double rotation_noise_from_rotation;


I think we need to update the names of the variables, since they are now not rotation and translation, but velocity and steering.

glpuga · 2025-10-29T15:11:33Z

beluga/include/beluga/motion/ackerman_drive_model.hpp

+   * 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;


This can be a constant in AckermannDriveModel itself, I don't think this needs to be a parameter.

glpuga · 2025-10-29T15:15:04Z

beluga/include/beluga/motion/ackerman_drive_model.hpp

+  /// Length of the robot (meters).
+  /**
+   * See \cite Localization and Mapping in Local Occupancy Grid Maps: Simulation
+   * in Ackerman model mobile robot by Ronald A. Cardenas , Jasper W. Huanay
+   * and Ivan Calle
+   */


This is not really the lenght of the robot, but the distance between the rear and front wheel axles. See https://en.wikipedia.org/wiki/Wheelbase .

In the SingleTrack (or Bicycle) model, which is a simplification of the ackermann structure, it's the distance between the rear and front wheel.

glpuga · 2025-10-29T15:18:43Z

beluga/include/beluga/motion/ackermann_drive_model.hpp

+ * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 5.3.
+ *


Explain here that the model is and adaptation using the single track kinematic model and the noise models of Probabilistic Robotics.

Notice that this model serves for any drive that can be simplified to a Single Track vehicle: ackermann, bicycle, tri-cycle, etc.

glpuga · 2025-10-29T15:27:05Z

beluga/include/beluga/motion/ackerman_drive_model.hpp

+        0.0,  // zero mean
+        std::sqrt(
+            params_.orientation_noise_from_translation * velocity.v * velocity.v +
+            params_.orientation_noise_from_rotation * velocity.w * velocity.w)};


Same, it should be the steering angle.

glpuga · 2025-10-29T15:31:55Z

beluga/include/beluga/motion/ackerman_drive_model.hpp

+    const double angular_velocity = angle_change / delta_t_sec;
+
+    // Determine direction sign (forward/backward motion)
+    const auto relative_transform = previous_pose.inverse() * pose;


Notice you're doing the relative rotation calculation twice in 219 and 214. You could calculate line 219 first, then line 214 is simply the rotation part of the relative transform.

beluga/include/beluga/motion/ackerman_drive_model.hpp

beluga/include/beluga/motion/ackermann_drive_model.hpp

glpuga · 2025-10-29T15:40:30Z

beluga_amcl/docs/ros2-reference.md


 `robot_model_type` _(`string`)_
-: Which odometry motion model to use. Supported models are `differential_drive` {cite}`thrun2005probabilistic`, `omnidirectional_drive` and `stationary`.
+: Which odometry motion model to use. Supported models are `differential_drive` {cite}`thrun2005probabilistic`, `omnidirectional_drive`, `ackerman_drive` and `stationary`.


Notice that Ackermann has double N. We need to fix in filenames an class names.

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

glpuga

I need to think about this. Our decomposition of the motion is using separate sources of truth for linear and angular velocity, and that does not seem fully right to me, and causes is to double check if a bunch of mutually dependent stuff is zero multiple times.

glpuga · 2025-11-18T20:32:31Z

beluga/include/beluga/motion/ackermann_drive_model.hpp

 * Supports 2D and (flattened) 3D state types.
 * This class satisfies \ref MotionModelPage.
 *
+ * The model is and adaptation using the single track kinematic model


Suggested change

* The model is and adaptation using the single track kinematic model

* The model is an adaptation using the single track kinematic model

glpuga · 2025-11-18T20:33:28Z

beluga/include/beluga/motion/ackermann_drive_model.hpp

+    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();


glpuga · 2025-11-18T21:05:47Z

beluga_amcl/src/amcl_node.cpp

+    params.orientation_noise_from_velocity = get_parameter("alpha6").as_double();
+    params.orientation_noise_from_steering = get_parameter("alpha7").as_double();


Why do we skip 5?

glpuga · 2025-11-18T21:31:21Z

beluga/include/beluga/motion/ackermann_drive_model.hpp

+      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);
+      }


I need to think this, the fact that we need to guard this is flagging me that there's an problem with how we are calculating motion.

add a draft of the ackerman drive model

4f5d8d4

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

fbattocchia marked this pull request as ready for review September 30, 2025 15:43

fbattocchia requested review from glpuga and hidmic September 30, 2025 15:43

hidmic reviewed Oct 1, 2025

View reviewed changes

fbattocchia force-pushed the fbattocchia/integrated_velocity_motion_mode branch from 72550b1 to 3d98bfe Compare October 2, 2025 21:02

fbattocchia requested a review from hidmic October 2, 2025 21:02

fbattocchia added 2 commits October 2, 2025 18:02

add implisit conversion of timestamp to value T

3d98bfe

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

fixed pr

1cee41d

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

hidmic reviewed Oct 3, 2025

View reviewed changes

fixed pr

554ea7b

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

fbattocchia requested a review from hidmic October 6, 2025 17:30

fixed pr

49424a0

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

hidmic approved these changes Oct 8, 2025

View reviewed changes

glpuga reviewed Oct 8, 2025

View reviewed changes

fixed pr

4284785

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

fbattocchia added 2 commits October 26, 2025 12:42

implemented an hibrid model between probabilistic robotic and paper a…

5c61f56

…ckerman Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

implemented an hibrid model between probabilistic robotic and paper a…

1305df6

…ckerman Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

fbattocchia commented Oct 26, 2025

View reviewed changes

fbattocchia requested a review from glpuga October 26, 2025 15:59

glpuga reviewed Oct 29, 2025

View reviewed changes

fixed pr comment

ae399ae

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

fbattocchia requested a review from glpuga November 11, 2025 16:36

fixed pr comment

0ea2fe3

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

glpuga reviewed Nov 19, 2025

View reviewed changes

	* Motion models in Beluga can be classified into two categories:
	* Motion models in Beluga include:

		auto time = timestamped.timestamp;
		auto previous_time = previous_timestamped.timestamp;

		* See Probabilistic Robotics \cite thrun2005probabilistic Chapter 5.3.
		*

	* The model is and adaptation using the single track kinematic model
	* The model is an adaptation using the single track kinematic model

		params.orientation_noise_from_velocity = get_parameter("alpha6").as_double();
		params.orientation_noise_from_steering = get_parameter("alpha7").as_double();

Conversation

fbattocchia commented Sep 10, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Proposed changes

Type of change

Checklist

Additional comments

Uh oh!

fbattocchia commented Sep 10, 2025

Uh oh!

hidmic commented Sep 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

hidmic left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

hidmic left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

hidmic Oct 3, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

fbattocchia commented Oct 6, 2025

Uh oh!

hidmic left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

glpuga left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

fbattocchia commented Sep 10, 2025 •

edited

Loading

hidmic commented Sep 11, 2025 •

edited

Loading

hidmic Oct 3, 2025 •

edited

Loading