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
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: