Pitch/Roll/Yaw corrected and latitude navigation added