From 400a6e12ba56237c760c3baa033f35082838e8d6 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sun, 20 Mar 2016 15:13:27 +1100
Subject: [PATCH] EKF: Ensure all data in buffers is initialised

This is a defensive change to prevent introduction of NaN's into the filter if data is read from the incorrect place in the buffer.
---
 EKF/estimator_interface.cpp | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp
index 7be7c9ba58..373a703f80 100644
--- a/EKF/estimator_interface.cpp
+++ b/EKF/estimator_interface.cpp
@@ -299,6 +299,29 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		return false;
 	}
 
+	// zero the data in the observation buffers
+	for (int index=0; index < OBS_BUFFER_LENGTH; index++) {
+		gpsSample gps_sample_init = {};
+		_gps_buffer.push(gps_sample_init);
+		magSample mag_sample_init = {};
+		_mag_buffer.push(mag_sample_init);
+		baroSample baro_sample_init = {};
+		_baro_buffer.push(baro_sample_init);
+		rangeSample range_sample_init = {};
+		_range_buffer.push(range_sample_init);
+		airspeedSample airspeed_sample_init = {};
+		_airspeed_buffer.push(airspeed_sample_init);
+		flowSample flow_sample_init = {};
+		_flow_buffer.push(flow_sample_init);
+	}
+
+	// zero the data in the imu data and output observer state buffers
+	for (int index=0; index < IMU_BUFFER_LENGTH; index++) {
+		imuSample imu_sample_init = {};
+		_imu_buffer.push(imu_sample_init);
+		outputSample output_sample_init = {};
+		_output_buffer.push(output_sample_init);
+	}
 
 	_dt_imu_avg = 0.0f;