Function fastslam_update 

pub fn fastslam_update(
    particles: &mut Vec<Particle>,
    u: Vector2<f64>,
    z: &[(f64, f64, usize)],
)

FastSLAM update step