Correct ordered output of measurements in pose array implemented.
Added state estimator to loop (and launch file)