ekf from robot_localization for odometry sensor fusion

[SimoneMic]

Try to test robot_localization for fusing the waist_imu information on yaw with the direct kinematics of the walking-controller

The first test with "vanilla" parameters of the covariance matrix gives worst result than the standard localization with normal odom and AMCL (note: AMCL is still used with the fused odom, also with the same parameters).

Example 1:

2023-09-11.13-59-42.mp4

In this case there is a visualization oscillation in Rviz2. This is due to the transform odom->root_link which is being fused between the imu and the infos from the internal kinematic. This causes oscillations in roll and pitch, but shouldn't influence the rest of the navigation since it's in 2D.
In this case the planner stopped since it has seen obstacles over the goal (the depth of camera detected the floor as an obstacle).

We could start fuse not only the yaw info from the IMU to try to improve the overall behavior, also the ekf must be tuned.

[SimoneMic]

As a benchmark, I leave a normal localization and navigation using our custom theta-star planner as a reference

2023-09-11.14-06-20.mp4

In this case, noise from the pointcloud was seen as an obstacle and thus the planner decided to take the longer path. This is due to the fact that the whole costmap is treated as lethal area (so also a point could block the corridor)