Kalman filter implementation based on libfixmatrix