Ekf optimalization

[rafal-gorecki]

Description

EKF

After the corrections, ekf ​​should generate such data, but for completeness you can do it in a circle, because I prepared the data from rosbag.

Additionally, the bag is quite simple, so it is difficult to compare individual configurations.

Simulation

topic remap - I also corrected the discrepancy in topics between the simulation and the robot.
wrong units - There was wrong values in simulated IMU g/s^2 but should be m/s^2

[delihus]

There will be conflicts due to #236. In the #236, the IMU covariances are implemented from the documentation of the sensor and the Madgwick filter.