EKF: Improve heading reset
EKF: Improve heading reset

Improves reset of quaternion covariance matrix after a heading reset by preserving variance in roll and pitch and resetting yaw variance to the measurement variance.

PX4/ecl,PX4/ecl,PX4/ecl,PX4/ecl,PX4/ecl