Implement Local Position Estimator

[podhrmic]

We are trying to mimic PX4 local position estimator - a Kalman filter that uses optical flow and bunch of other sensors. Here is the full code: https://github.com/PX4/Firmware/tree/master/src/modules/local_position_estimator

It is a 10 state Kalman filter, and at this point we want to write a general implementation since we might decide to reduce the number of states in the end (and don't implement some sensors, such as GPS etc).

Structure:

• parameters are defined here: https://github.com/PX4/Firmware/blob/master/src/modules/local_position_estimator/params.c We probably want this in some config file
• header file (not really interesting): https://github.com/PX4/Firmware/blob/master/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
• actual implementation: https://github.com/PX4/Firmware/blob/master/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

Sensors

1. sonar (min, max, offset, eps, derotation)
2. lidar (min, max, offset, derotation)
3. baro (mean value, reset)
4. flow
5. accelerometers

How does it work?

1. init state matrices and other objects to their default values
2. when a callback from a sensor happens (new data available) set a sensor flag to true and copy the new measurement (this can be converted to just periodic read from sensors since everything is at the same frequency of 200Hz)
3. in the main loop:
   • perform sanitty checks (std, errors, reset)
   • see which sensors are available
   • check for NaN in matrices (are they numerically stable?)
   • prediction step:
     1. get accel input
     2. correction logic (calculates bias)
     3. propagate
   • measurement update for each sensor
   • final checks & publish results

[podhrmic]

This would be great but maybe somebody else will pick it up from here.