C++ Kalman filtering (EKF, UKF)