EKF code working, need to test nbv