Fix differential drive motion model

[hidmic]

Proposed changes

Going over the math a few times, it doesn't seem to me that the rotation variance computation distributes over rotation composition. Not with those nonlinear max, min, abs operations in between.

Type of change

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

Checklist

• 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

Not sure how to write a test for this. Would a formal proof (that rotation_variance(a) * rotation_variance(b) != rotation_variance(a * b)) do?

[glpuga]

Going over the math a few times, it doesn't seem to me that the rotation variance computation distributes over rotation composition. Not with those nonlinear max, min, abs operations in between.

I totally agree that that operation most definitely is not distributive.

However, I have broader concerns regarding the rotation_variance() function itself. From what I gather it's meant to bring the 2nd/3rd quadrant rotation values to the 1st/4th quadrant when moving backwards, needed because $\Delta x$ is negative (plus also squaring the result angle).

But that seems to be done at the end of all the math. By then, we already calculated the second rotation using the pre-adjustment first rotation, and it's not obvious to me that that can be fixed by moving second rotation back into 1st/4th quadrant too afterwards.

Not having gone through t all the math in Prob. Robotics, the definitions of $\delta_{rot1}$ and $\delta_{rot2}$ seem very straightforward (the aperture angles between the end/star pose and the direction between the end/start pose, range $-\pi/2$ to $\pi/2$). Maybe we should just do the quadrant adjustment once when calculating first $\delta_{rot1}$ the first time; that should take care of $\delta_{rot2}$ right away.

Additional comments

Not sure how to write a test for this. Would a formal proof (that rotation_variance(a) * rotation_variance(b) != rotation_variance(a * b)) do?

From this code we don't really have good observability to the results of this function, which would be the best way to test this. Given that the parameters are private you can only test by generating samples of the distribution and comparing it, which is not ideal.

I think it's worth either

1. providing observability to the distribution parameter values through getters for testing's sake (not a fan),
2. better yet, architecting this differently so that the actual code that does this critical calculation is in an external function/class that can be tested independently, feeding in pose and last_pose, and getting the distribution parameters at the other end, like we do with calculate_covariance() in the estimation mixin.

In this last scenario update_motion() would just call this function to calculate the distribution parameters, and manage the mixin last_pose_ state.

[hidmic]

From what I gather it's meant to bring the 2nd/3rd quadrant rotation values to the 1st/4th quadrant when moving backwards, needed because $\Delta x$ is negative (plus also squaring the result angle).

IIUC it's computing the minimum rotation angle magnitude. Because the model assumes forward motion, moving backwards means larger rotations than necessary (e.g. $(0, -1, 0)$ is expressed as $(\pi, 1, -\pi)$ ).

Maybe we should just do the quadrant adjustment once when calculating first $\delta_{rot1}$ the first time; that should take care of $\delta_{rot2}$ right away.

I guess we could. The mean of the translation distribution would have to be negative.

better yet, architecting this differently so that the actual code that does this critical calculation is in an external function/class that can be tested independently

Sounds reasonable.

[nahueespinosa]

Given that the parameters are private you can only test by generating samples of the distribution and comparing it, which is not ideal.

FWIW, we have tests that generate samples and assert distribution parameter values, no need to provide observability to the internal state.

beluga/beluga/test/beluga/motion/test_differential_drive_model.cpp

Line 151 in 9de0417

ASSERT_NEAR(stddev, std::sqrt(alpha * flipped_angle * flipped_angle), 0.01);

Factoring out the critical calculation is also very reasonable, but the other option is not that bad IMO.

BTW, thanks for digging in, this is great!

[glpuga]

FWIW, we have tests that generate samples and assert distribution parameter values, no need to provide observability to the internal state.

beluga/beluga/test/beluga/motion/test_differential_drive_model.cpp

Line 151 in 9de0417

ASSERT_NEAR(stddev, std::sqrt(alpha * flipped_angle * flipped_angle), 0.01);

Good point. Bears asking why that test in insensitive to this fix then.

In that test, the translation distance is zero, so first_rotation would be zero and the result both after and before the change would be the same, right?.

[nahueespinosa]

In that test, the translation distance is zero, so first_rotation would be zero and the result both after and before the change would be the same, right?.

Right, I would imagine a new test where all parameters but translation_noise_from_rotation are zero, and we set a move that requires two rotations. I volunteer to give it a shot tonight if it helps.