Find ways to make the sensor fusion more efficient

[thaytan]

The UKF fusion filter works fairly well, but is costly on CPU. It has to be compiled with optimisations to run fast enough, and even then chews a good chunk of a CPU core running @ 1000 Hz. A big part of this is the delay slot setup to compensate for video capture and processing latency, which expands the UKF state matrix and UKF cost grows at O(n^3) unfortunately.

Some other avenues to explore are:

• Don't do full Kalman state updates at full IMU rate, but instead integrate the IMU readiings most of the time and only do Kalman measurements / corrections at a lower rate.
• Try an implementation of the Square Root UKF to reduce the update cost
• Look at implementations like ILLIXR have for SLAM using an EKF-based MSCKF formulation