From 5fec0df70d7671fedcb98e7da2b717267183cb28 Mon Sep 17 00:00:00 2001
From: mcsauder <mcsauder@greypointcorp.com>
Date: Thu, 25 Feb 2016 22:51:14 -0700
Subject: [PATCH] Additional initializations required to reset complimentary
 filter values if the state estimate ever diverges and requires
 re-initiailization.

---
 EKF/ekf.cpp | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index fe7e617990..e8eb696a0e 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -115,7 +115,11 @@ bool Ekf::init(uint64_t timestamp)
 	_output_new.vel.setZero();
 	_output_new.pos.setZero();
 	_output_new.quat_nominal = matrix::Quaternion<float>();
-
+    
+	_delta_angle_corr.setZero();
+	_delta_vel_corr.setZero();
+	_vel_corr.setZero();
+    
 	_imu_down_sampled.delta_ang.setZero();
 	_imu_down_sampled.delta_vel.setZero();
 	_imu_down_sampled.delta_ang_dt = 0.0f;
@@ -131,6 +135,9 @@ bool Ekf::init(uint64_t timestamp)
 	_NED_origin_initialised = false;
 	_gps_speed_valid = false;
 	_mag_healthy = false;
+
+	_filter_initialised = false;
+
 	return ret;
 }