In this section I am going to create a demo on using Kalman filter for processing sensor data. This demo does NOT use ROS2 (I will add ROS2 in one of the next sections).
Few important notes.
Though it is possible to use different sensors together in this code, it is not
"sensor fusion ready": I will address sensor fusion in later sections.
This code uses some kind of a "simulation", so it can be tested, but this simulation
is very inaccurate. Particularly, it does not provide drift for such sensor as gyroscope.
That is one of the reasons I do not talk about sensor fusion here - it would be too easy and
will not work in real world. I am going to use Gazebo simulator for a more accurate simulation
in a later section, plus, most likely, I will modify this code (again, later) to handle
real world behavior.
So please consider this section as a tutorial and a foundatiion for future code base. All explanations are provided as comments in the code below.