Filter validation

[juangallostra]

Feature / Bug description

The main goal is to implement a successful Altitude Hold flight mode that can be used to fly the Mosquito.

To do so, we should validate that all the states required for flying in Altitude Hold mode are properly estimated. This means:

1. Height (1 state)
2. Vertical velocity (1 state)
3. Orientation (4 states)
4. Gyrometer bias (3 states)
5. Accelerometer bias (3 states)

Although this are the required states and special care should be put on them, the full filter (16 true and nominal states and 15 error states) should be implemented for the next release.

Expected behavior

Required states to fly in Altitude Hold mode are estimated correctly.

[juangallostra]

Things to check/implement:

• Altitude estimation
• Optical flow outlier detection & removal
• Tune covariance(s) and parameters