From 85e0e6e89c4f061adb14dc6a8e99f986f6ff00be Mon Sep 17 00:00:00 2001
From: kamilritz <kritz@ethz.ch>
Date: Fri, 13 Dec 2019 16:26:42 +0100
Subject: [PATCH] Remove resetStates(AndCovariances) function

---
 EKF/ekf.cpp               | 46 ++++++++++++++-------------------------
 EKF/ekf.h                 |  3 ---
 EKF/estimator_interface.h |  3 +--
 3 files changed, 17 insertions(+), 35 deletions(-)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 7f322c4ff5..1ad0ad1443 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -54,7 +54,20 @@ bool Ekf::init(uint64_t timestamp)
 
 void Ekf::reset(uint64_t timestamp)
 {
-	resetStatesAndCovariances();
+	_state.vel.setZero();
+	_state.pos.setZero();
+	_state.delta_ang_bias.setZero();
+	_state.delta_vel_bias.setZero();
+	_state.mag_I.setZero();
+	_state.mag_B.setZero();
+	_state.wind_vel.setZero();
+	_state.quat_nominal.setIdentity();
+
+	_output_new.vel.setZero();
+	_output_new.pos.setZero();
+	_output_new.quat_nominal.setIdentity();
+
+	initialiseCovariance();
 
 	_delta_angle_corr.setZero();
 	_imu_down_sampled.delta_ang.setZero();
@@ -63,10 +76,7 @@ void Ekf::reset(uint64_t timestamp)
 	_imu_down_sampled.delta_vel_dt = 0.0f;
 	_imu_down_sampled.time_us = timestamp;
 
-	_q_down_sampled(0) = 1.0f;
-	_q_down_sampled(1) = 0.0f;
-	_q_down_sampled(2) = 0.0f;
-	_q_down_sampled(3) = 0.0f;
+	_q_down_sampled.setIdentity();
 
 	_imu_updated = false;
 	_NED_origin_initialised = false;
@@ -90,30 +100,6 @@ void Ekf::reset(uint64_t timestamp)
 	_prev_dvel_bias_var.zero();
 }
 
-void Ekf::resetStatesAndCovariances()
-{
-	resetStates();
-	initialiseCovariance();
-}
-
-void Ekf::resetStates()
-{
-	_state.vel.setZero();
-	_state.pos.setZero();
-	_state.delta_ang_bias.setZero();
-	_state.delta_vel_bias.setZero();
-	_state.mag_I.setZero();
-	_state.mag_B.setZero();
-	_state.wind_vel.setZero();
-	_state.quat_nominal.setZero();
-	_state.quat_nominal(0) = 1.0f;
-
-	_output_new.vel.setZero();
-	_output_new.pos.setZero();
-	_output_new.quat_nominal.setZero();
-	_output_new.quat_nominal(0) = 1.0f;
-}
-
 bool Ekf::update()
 {
 	bool updated = false;
@@ -153,7 +139,7 @@ bool Ekf::initialiseFilter()
 {
 	// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
 
-	// Sum the IMU delta angle measurements
+	// Sum the IMU delta velocity measurements
 	const imuSample &imu_init = _imu_buffer.get_newest();
 	_delVel_sum += imu_init.delta_vel;
 
diff --git a/EKF/ekf.h b/EKF/ekf.h
index 92f17d0e40..8b1ef38918 100644
--- a/EKF/ekf.h
+++ b/EKF/ekf.h
@@ -57,9 +57,6 @@ public:
 	// set the internal states and status to their default value
 	void reset(uint64_t timestamp) override;
 
-	void resetStatesAndCovariances() override;
-	void resetStates() override;
-
 	// should be called every time new data is pushed into the filter
 	bool update() override;
 
diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h
index 2d71e36b03..f9630de8eb 100644
--- a/EKF/estimator_interface.h
+++ b/EKF/estimator_interface.h
@@ -63,8 +63,7 @@ public:
 
 	virtual bool init(uint64_t timestamp) = 0;
 	virtual void reset(uint64_t timestamp) = 0;
-	virtual void resetStates() = 0;
-	virtual void resetStatesAndCovariances() = 0;
+
 	virtual bool update() = 0;
 
 	virtual void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) = 0;