ekf: move "reset" part of the init function into a separate function

add getter for the vehicle at rest flag

EKF/ekf.cpp

@@ -47,6 +47,13 @@
 bool Ekf::init(uint64_t timestamp)
 {
 	bool ret = initialise_interface(timestamp);
+	reset(timestamp);
+
+	return ret;
+}
+
+void Ekf::reset(uint64_t timestamp)
+{
 	_state.vel.setZero();
 	_state.pos.setZero();
 	_state.gyro_bias.setZero();
@@ -94,8 +101,6 @@ bool Ekf::init(uint64_t timestamp)
 	_accel_mag_filt = 0.0f;
 	_ang_rate_mag_filt = 0.0f;
 	_prev_dvel_bias_var.zero();
-
-	return ret;
 }
 
 bool Ekf::update()

EKF/ekf.h

@@ -54,6 +54,9 @@ public:
 	// initialise variables to sane values (also interface class)
 	bool init(uint64_t timestamp) override;
 
+	// set the internal states and status to their default value
+	void reset(uint64_t timestamp) override;
+
 	// should be called every time new data is pushed into the filter
 	bool update() override;
 

EKF/estimator_interface.h

@@ -59,6 +59,7 @@ public:
 	virtual ~EstimatorInterface() = default;
 
 	virtual bool init(uint64_t timestamp) = 0;
+	virtual void reset(uint64_t timestamp) = 0;
 	virtual bool update() = 0;
 
 	// gets the innovations of velocity and position measurements
@@ -357,6 +358,8 @@ public:
 		*val = _fault_status.value;
 	}
 
+	bool isVehicleAtRest() const { return _vehicle_at_rest; }
+
 	// get GPS check status
 	virtual void get_gps_check_status(uint16_t *val) = 0;