Kalman filter implementation based on libfixmatrix - View it on GitHub