Integration of Attitude. Data Association

Attitude of the vehicle in 3 dimensions is integrated with this
function. Data Association associates detected points with their global
map co-ordinates.