GPS, IMU and Magnetometer with UKF in ROS2

The following code works by replacing run_localization() function above with a timer function. 10 (or 100) times per second we supply sensor readings to the Kalman Filter, and draw both "ground truth" and Kalman predicted trajectories.

Let's take a look at the results. Accelerometer was presented earlier, but here it is again:

GPS

As GPS doesn't have drift, we have more or less constant error. Modern GPS receivers can be accurate within centimeters, but for an inexpensive commercial one 10m is probably a good starting point. Plus GPS doesn't work well indoors and in other 3d environments, like city canyons etc.

Gyroscope:

Gyroscope drifts:

As you can see, gyroscope based navigation misses: at straight part of a path it miscalculates speed and while turning, miscalculates angles.

Magnetometer:

Magnetometer results look strange: way too accurate. What is the reason? The magnetometer has no slippage in terms of orientation, so it works great when our robot turns. While when going straight on a flat surface, the robot has no or almost no slippage. So yes, we can perform "ideal" navigation with just compass, if we have an ideal odometry (in our case, if we have no slippage while going straight).

Let's fix this glitch. As same applies to all sensors except for GPS and Landmarks, I am going to use a different world: one where part of the path is across a very slippery object. So, let's take a look at the same sensors, when slipping is present. For that we are going to use "slippery" world and a bit longer path:

Note that - due to Gazebo problems - I exercise extra caution when moving around: no switching gears at speed, slow down to stop then start the next maneuvre. No idea why, but if I do not do it, accelerometer goes crazy.

slip.sfd

GPS with slippage

No difference, really, as GPS doesn't rely on dead reckonning.

Gyroscope with slippage

As you can see, slippage is present.

Accelerometer with slippage

Now, let's switch to 10 Hz and run accelerometer without slippage, and then with.

Here we expect no slippage, but we got it! The reason is probably in the fact that the world is not 3d: when we go up and down on slippery road block, we affect horizontal acceleration by tilting the robot. Also the reason can be in the fact that the simulation went wrong, after all, we introduces vibration that Gazebo doesn't like.

Conclusion: we can not rely on "dead reckon" sensors on the long run. We need to do sensor fusion, in order to add data from external sensors, like GPS, landmarks, LIDAR or something else like this.

Note that landmarks are still not supported as our robot doesn't have any means of detecting them. This is to be done.

Once again, just to make it absolutely clear: I am building this code gradually, so that whatever changes I make, they are not lost in the overcomplicated universal solution. Therefore, this is not a final version, and I am going to modify this code in the next few sections before it is production ready. Currently, it is a tutorial, not a final code.