Function ekf_slam 

pub fn ekf_slam(
    state: &mut EKFSLAMState,
    u: &Vector2<f64>,
    observations: &[(f64, f64)],
)

Full EKF SLAM step (prediction + update) with unknown data association