ekf: Extract resetState from main reset and reinitialize

the covariances in the init function

EKF/ekf.cpp

@@ -54,20 +54,7 @@ bool Ekf::init(uint64_t timestamp)
 
 void Ekf::reset(uint64_t timestamp)
 {
-	_state.vel.setZero();
+	resetStatesAndCovariances();
-	_state.pos.setZero();
-	_state.gyro_bias.setZero();
-	_state.accel_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;
 
 	_delta_angle_corr.setZero();
 	_imu_down_sampled.delta_ang.setZero();
@@ -103,6 +90,30 @@ 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.gyro_bias.setZero();
+	_state.accel_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;

EKF/ekf.h

@@ -57,6 +57,9 @@ 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;
 

EKF/estimator_interface.h

@@ -60,6 +60,8 @@ 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;
 
 	// gets the innovations of velocity and position measurements