From 90e1bd3e36f2b2dcbeffad62d95dca440969f615 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 19 Feb 2016 16:53:55 +1100
Subject: [PATCH] EKF: wrap compass yaw estimate

---
 EKF/mag_fusion.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp
index 629f1791ac..af252ad21f 100644
--- a/EKF/mag_fusion.cpp
+++ b/EKF/mag_fusion.cpp
@@ -613,6 +613,9 @@ void Ekf::fuseHeading()
 	matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
 	float measured_yaw = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination;
 
+	// wrap the yaw to the interval between +-pi
+	measured_yaw = matrix::wrap_pi(measured_yaw);
+
 	// calculate the innovation
 	matrix::Euler<float> euler(_state.quat_nominal);
 	float innovation = euler(2) - measured_yaw;