Classic extended kalman finished.
Cpp kalman.