[Bug/Feature]: Horizontal plane calibration

[marbosjo]

What do we want to achieve?

To calibrate potential misalignment of IMU mounting with respect to horizontal plane.

How do we plan to do it?

The compensation is done now by applying an offset to each axes, but this is wrong as first the correction for the ground plane must be applied.

In the set_horizontal service:

1. compensation matrix must be computed an filled.
2. bias can be computed after applying the previous rotation.

How can it be tested?

1. Place robot in the ground.
2. Check acceleration values, there should offset a bit from (0,0,9.8)
3. Call service for calibration
4. Check acceleration values, now they should be closer to (0,0,9.8)
5. Restart robot a check values again.

Subtasks

No response