Code : optimised processing for 2-state EKF