From 469a7d1711e5f518e6ae9303c3dbc23cbd5f2efe Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Fri, 22 Apr 2016 08:43:56 +1000
Subject: [PATCH] EKF: rework height reset

calculate and save the amount that the vertical states have changed and the time of the change
apply the change delta to the output observer states and buffer
---
 EKF/ekf_helper.cpp | 49 +++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 48 insertions(+), 1 deletion(-)

diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp
index 09377f9537..cd0d93920e 100644
--- a/EKF/ekf_helper.cpp
+++ b/EKF/ekf_helper.cpp
@@ -90,12 +90,23 @@ void Ekf::resetHeight()
 	// Get the most recent GPS data
 	gpsSample gps_newest = _gps_buffer.get_newest();
 
+	// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
+	float old_vert_pos = _state.pos(2);
+	bool vert_pos_reset = false;
+	float old_vert_vel = _state.vel(2);
+	bool vert_vel_reset = false;
+
+	// reset the vertical position
 	if (_control_status.flags.rng_hgt) {
 		rangeSample range_newest = _range_buffer.get_newest();
 
 		if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) {
-			_state.pos(2) = _hgt_sensor_offset - range_newest.rng;
+			// calculate the new vertical position using range sensor
+			float new_pos_down = _hgt_sensor_offset - range_newest.rng;
+			// update the state and assoicated variance
+			_state.pos(2) = new_pos_down;
 			P[8][8] = sq(_params.range_noise);
+			vert_pos_reset = true;
 
 		} else {
 			// TODO: reset to last known range based estimate
@@ -112,6 +123,7 @@ void Ekf::resetHeight()
 		if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
 			_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
 			P[8][8] = sq(_params.baro_noise);
+			vert_pos_reset = true;
 
 		} else {
 			// TODO: reset to last known baro based estimate
@@ -122,6 +134,7 @@ void Ekf::resetHeight()
 		if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
 			_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
 			P[8][8] = sq(gps_newest.hacc);
+			vert_pos_reset = true;
 
 		} else {
 			// TODO: reset to last known gps based estimate
@@ -140,6 +153,40 @@ void Ekf::resetHeight()
 		P[5][5] = fminf(sq(_state.vel(2)),1000.0f);
 		_state.vel(2) = 0.0f;
 	}
+	vert_vel_reset = true;
+
+	// store the reset amount and time to be published
+	if (vert_pos_reset) {
+		_vert_pos_reset_delta = _state.pos(2) - old_vert_pos;
+		_time_vert_pos_reset = _time_last_imu;
+	}
+
+	if (vert_vel_reset) {
+		_vert_vel_reset_delta = _state.vel(2) - old_vert_vel;
+		_time_vert_vel_reset = _time_last_imu;
+	}
+
+	// add the reset amount to the output observer states
+	_output_new.pos(2) += _vert_pos_reset_delta;
+	_output_new.vel(2) += _vert_vel_reset_delta;
+
+	// add the reset amount to the output observer buffered data
+	outputSample output_states;
+	unsigned output_length = _output_buffer.get_length();
+	for (unsigned i=0; i < output_length; i++) {
+		output_states = _output_buffer.get_from_index(i);
+
+		if (vert_pos_reset) {
+			output_states.pos(2) += _vert_pos_reset_delta;
+		}
+
+		if (vert_vel_reset) {
+			output_states.vel(2) += _vert_vel_reset_delta;
+		}
+
+		_output_buffer.push_to_index(i,output_states);
+
+	}
 
 }