Add covariances, fix incorrect yaw & pitch angular direction

Works in ROS nav stack with robot_pose_ekf