Kalman navigation fixes

This fixes the yaw estimate and decouples it from roll and pitch.