search

elvout / cs393r

CS 393R Graduate Autonomous Robots, Fall 2021 | Autobots
0 stars 0 forks source link

Convert reflex angles to negative convex angles in `ParticleFilter::Predict` #50

Closed elvout closed 2 years ago

elvout commented 2 years ago

A bug occurs in ParticleFilter::Predict when the angular displacement between two odometry readings is a reflex angle (>180º). Since our standard deviation calculations are linear functions of the angular displacement, this inflates the standard deviations.

This bug arises when running rosbag play on 2020-04-01-17-23-38.bag. At time 36.5, the particle cloud explodes. Here is some debug output:

[Predict DEBUG]
    base_disp: [-0.0230, -0.0000], norm: 0.0230
    angular_disp: 358.9632º
    stddevs: [9.406819]m, [1077.678984]º

Converting the reflex angle to its negative convex counterpart (-1.0368º) should fix this problem. We should also be normalizing angles in general so they don't exceed 2pi.