Extended kalman filter example code