Added function to approximate 5dof meas as a 6dof one
Merge branch 'master', remote-tracking branch 'origin'