Function observation 

pub fn observation(
    x_true: &Vector3<f64>,
    xd: &Vector3<f64>,
    u: &[f64; 2],
    rfid: &[(f64, f64)],
) -> (Vector3<f64>, Option<Vec<Observation>>, Vector3<f64>, [f64; 2])

Simulate observation and motion