Testing Kalman filter

Let's start with uncommenting GPS sensor in dictSensors dictionary:

The error is (as expected, because we set it so) around 10 m.

Let's turn off GPS and use IMU accelerometer. Note that IMU is an internal sensor, so its error should accumulate.

Gyroscope:

Magnetometer:

Let's use landmarks alone. Note that, compared to "the book", I have specified much larger sensor errors, plus in code, approximately half of landmarks are marker as hidden. Nevertheless, result is quite good. Also note as when the robot moves away from the cluster of landmarks, the uncertainty increases, and as it moves back, it decreases again:

Now let's use GPS and Landmarks. Notice that filter performs better than with either of these sensors alone:

The ig chunk of possible improvements is related to landmarks, but not in a traditional sense. Imagine that our robot runs on the road. We know it is on the road, right? And in the same time, the "uncertainty spot" Kalman returns covers both part of the road and some land outside. But we KNOW we are on the road, so we can make our localization way more accurate.

You can see that trick used in GPS navigators: the car icon stays on the road, even though the GPS "spot" is much wider. But using this approach with Kalman filter is very difficult, and hone.

What exactly is an intersection of Kalman's Gaussian with the segment of a road? For filter to continue to work, it should be another Gaussian... which it is not. What if road bends? Even "less Gaussian"! What if there are two close roads, and "GPS spot" overlaps both of them? What if robot (or car) drives under the bridge, and there is a second road under it? Which one to choose?

In this case we need to keep robot's history: if we were driving on road A, we should not assume that we suddenly jumped to road B.

Actually, at some points this approach can give us even more accuracy: we can assume that slowing down is related to crossing intersections, and when we turn, we can suggest both x and y coordinates of a robot... But it requires a really complex code.

I am not going to handle "road matching", at least not in the nearest future. Let me just mention that it can be done by running a separate chunk of code for matching, and using it to adjust robot's coordinates. So it is like an extra sensor, but if you want to use it with Kalman filter, you should always keep in mind its non-Kalman properties.