[WIP] ekf2: EKF init immediately on first IMU sample

- tilt init use filtered accel/gyro on delayed time horizon
 - publish estimator_status on every EKF update (even if !_filter_initialised)

src/modules/ekf2/EKF/ekf.cpp

@@ -43,35 +43,13 @@
 
 #include <mathlib/mathlib.h>
 
-bool Ekf::init(uint64_t timestamp)
-{
-	if (!_initialised) {
-		_initialised = initialise_interface(timestamp);
-		reset();
-	}
-
-	return _initialised;
-}
-
 void Ekf::reset()
 {
 	ECL_INFO("reset");
 
+	_state = {};
 	_state.quat_nominal.setIdentity();
-	_state.vel.setZero();
-	_state.pos.setZero();
-	_state.gyro_bias.setZero();
-	_state.accel_bias.setZero();
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	_state.mag_I.setZero();
-	_state.mag_B.setZero();
-#endif // CONFIG_EKF2_MAGNETOMETER
-
-#if defined(CONFIG_EKF2_WIND)
-	_state.wind_vel.setZero();
-#endif // CONFIG_EKF2_WIND
-	//
 #if defined(CONFIG_EKF2_TERRAIN)
 	// assume a ground clearance
 	_state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance;
@@ -101,6 +79,13 @@ void Ekf::reset()
 
 	_output_predictor.reset();
 
+	_time_latest_us = 0;
+	_time_delayed_us = 0;
+
+	_imu_updated = false;
+
+	_filter_initialised = false;
+
 	// Ekf private fields
 	_time_last_horizontal_aiding = 0;
 	_time_last_v_pos_aiding = 0;
@@ -133,18 +118,12 @@ void Ekf::reset()
 	_zero_velocity_update.reset();
 
 	updateParameters();
+
+	initialiseCovariance();
 }
 
 bool Ekf::update()
 {
-	if (!_filter_initialised) {
-		_filter_initialised = initialiseFilter();
-
-		if (!_filter_initialised) {
-			return false;
-		}
-	}
-
 	// Only run the filter if IMU data in the buffer has been updated
 	if (_imu_updated) {
 		_imu_updated = false;
@@ -159,14 +138,52 @@ bool Ekf::update()
 		float filter_update_s = 1e-6f * _params.filter_update_interval_us;
 		_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * math::constrain(input, 0.5f * filter_update_s, 2.f * filter_update_s);
 
+		// Filter accel for tilt initialization
+		if (_is_first_imu_sample) {
+			_accel_lpf.reset(imu_sample_delayed.delta_vel / imu_sample_delayed.delta_vel_dt);
+			_gyro_lpf.reset(imu_sample_delayed.delta_ang / imu_sample_delayed.delta_ang_dt);
+			_is_first_imu_sample = false;
+
+			_state = {};
+			initialiseTilt(_accel_lpf.getState());
+			initialiseCovariance();
+
+		} else {
+			_accel_lpf.update(imu_sample_delayed.delta_vel / imu_sample_delayed.delta_vel_dt);
+			_gyro_lpf.update(imu_sample_delayed.delta_ang / imu_sample_delayed.delta_ang_dt);
+		}
+
+		if (!_filter_initialised) {
+
+			const float accel_norm = _accel_lpf.getState().norm();
+			const float gyro_norm = _gyro_lpf.getState().norm();
+
+			if ((accel_norm > 0.8f * CONSTANTS_ONE_G)
+			    && (accel_norm < 1.2f * CONSTANTS_ONE_G)
+			    && (gyro_norm < math::radians(15.f))
+			   ) {
+				// once ready reset and init tilt from filtered accel
+				_state = {};
+				initialiseTilt(_accel_lpf.getState());
+				initialiseCovariance();
+
+				// reset the output predictor state history to match the EKF initial values
+				_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos);
+
+				_filter_initialised = true;
+			}
+		}
+
 		updateIMUBiasInhibit(imu_sample_delayed);
 
 		// perform state and covariance prediction for the main filter
 		predictCovariance(imu_sample_delayed);
 		predictState(imu_sample_delayed);
 
-		// control fusion of observation data
-		controlFusionModes(imu_sample_delayed);
+		if (_filter_initialised) {
+			// control fusion of observation data
+			controlFusionModes(imu_sample_delayed);
+		}
 
 		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _gpos,
 						      _state.gyro_bias, _state.accel_bias);
@@ -177,52 +194,10 @@ bool Ekf::update()
 	return false;
 }
 
-bool Ekf::initialiseFilter()
+bool Ekf::initialiseTilt(const Vector3f &accel)
 {
-	// Filter accel for tilt initialization
-	const imuSample &imu_init = _imu_buffer.get_newest();
-
-	// protect against zero data
-	if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
-		return false;
-	}
-
-	if (_is_first_imu_sample) {
-		_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
-		_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
-		_is_first_imu_sample = false;
-
-	} else {
-		_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
-		_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
-	}
-
-	if (!initialiseTilt()) {
-		return false;
-	}
-
-	// initialise the state covariance matrix now we have starting values for all the states
-	initialiseCovariance();
-
-	// reset the output predictor state history to match the EKF initial values
-	_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos);
-
-	return true;
-}
-
-bool Ekf::initialiseTilt()
-{
-	const float accel_norm = _accel_lpf.getState().norm();
-	const float gyro_norm = _gyro_lpf.getState().norm();
-
-	if (accel_norm < 0.8f * CONSTANTS_ONE_G ||
-	    accel_norm > 1.2f * CONSTANTS_ONE_G ||
-	    gyro_norm > math::radians(15.0f)) {
-		return false;
-	}
-
 	// get initial tilt estimate from delta velocity vector, assuming vehicle is static
-	_state.quat_nominal = Quatf(_accel_lpf.getState(), Vector3f(0.f, 0.f, -1.f));
+	_state.quat_nominal = Quatf(accel, Vector3f(0.f, 0.f, -1.f));
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	return true;

src/modules/ekf2/EKF/ekf.h

@@ -80,15 +80,15 @@ public:
 	Ekf()
 	{
 		reset();
+		initialise_interface();
 	};
 
-	virtual ~Ekf() = default;
-
-	// initialise variables to sane values (also interface class)
-	bool init(uint64_t timestamp) override;
+	~Ekf() = default;
 
 	void print_status();
 
+	void reset();
+
 	// should be called every time new data is pushed into the filter
 	bool update();
 
@@ -427,10 +427,7 @@ private:
 	friend class EvVelLocalFrameNed;
 	friend class EvVelLocalFrameFrd;
 
-	// set the internal states and status to their default value
-	void reset();
-
-	bool initialiseTilt();
+	bool initialiseTilt(const Vector3f &accel);
 
 	// check if the EKF is dead reckoning horizontal velocity using inertial data only
 	void updateDeadReckoningStatus();
@@ -583,11 +580,6 @@ private:
 	estimator_aid_source2d_s _aid_src_aux_vel {};
 #endif // CONFIG_EKF2_AUXVEL
 
-	// Variables used by the initial filter alignment
-	bool _is_first_imu_sample{true};
-	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
-	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
-
 #if defined(CONFIG_EKF2_BAROMETER)
 	estimator_aid_source1d_s _aid_src_baro_hgt {};
 
@@ -626,9 +618,6 @@ private:
 	uint64_t _time_good_vert_accel{0};	///< last time a good vertical accel was detected (uSec)
 	uint16_t _clip_counter[3];		///< counter per axis that increments when clipping ad decrements when not
 
-	// initialise filter states of both the delayed ekf and the real time complementary filter
-	bool initialiseFilter(void);
-
 	// initialise ekf covariance matrix
 	void initialiseCovariance();
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -78,11 +78,6 @@ EstimatorInterface::~EstimatorInterface()
 // Accumulate imu data and store to buffer at desired rate
 void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 {
-	// TODO: resolve misplaced responsibility
-	if (!_initialised) {
-		_initialised = init(imu_sample.time_us);
-	}
-
 	_time_latest_us = imu_sample.time_us;
 
 	// the output observer always runs
@@ -122,18 +117,19 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 void EstimatorInterface::setMagData(const magSample &mag_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_mag_buffer == nullptr) {
-		_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_mag_buffer = new RingBuffer<magSample>(_obs_buffer_length);
 
-		if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
-			delete _mag_buffer;
-			_mag_buffer = nullptr;
-			printBufferAllocationFailed("mag");
+			if (_mag_buffer == nullptr || !_mag_buffer->valid()) {
+				delete _mag_buffer;
+				_mag_buffer = nullptr;
+				printBufferAllocationFailed("mag");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -161,18 +157,19 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 #if defined(CONFIG_EKF2_GNSS)
 void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_gps_buffer == nullptr) {
-		_gps_buffer = new RingBuffer<gnssSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_gps_buffer = new RingBuffer<gnssSample>(_obs_buffer_length);
 
-		if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
-			delete _gps_buffer;
-			_gps_buffer = nullptr;
-			printBufferAllocationFailed("GPS");
+			if (_gps_buffer == nullptr || !_gps_buffer->valid()) {
+				delete _gps_buffer;
+				_gps_buffer = nullptr;
+				printBufferAllocationFailed("GPS");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -208,18 +205,19 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 #if defined(CONFIG_EKF2_BAROMETER)
 void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_baro_buffer == nullptr) {
-		_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_baro_buffer = new RingBuffer<baroSample>(_obs_buffer_length);
 
-		if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
-			delete _baro_buffer;
-			_baro_buffer = nullptr;
-			printBufferAllocationFailed("baro");
+			if (_baro_buffer == nullptr || !_baro_buffer->valid()) {
+				delete _baro_buffer;
+				_baro_buffer = nullptr;
+				printBufferAllocationFailed("baro");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -247,18 +245,19 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 #if defined(CONFIG_EKF2_AIRSPEED)
 void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_airspeed_buffer == nullptr) {
-		_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_airspeed_buffer = new RingBuffer<airspeedSample>(_obs_buffer_length);
 
-		if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
-			delete _airspeed_buffer;
-			_airspeed_buffer = nullptr;
-			printBufferAllocationFailed("airspeed");
+			if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) {
+				delete _airspeed_buffer;
+				_airspeed_buffer = nullptr;
+				printBufferAllocationFailed("airspeed");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -285,18 +284,19 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_range_buffer == nullptr) {
-		_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);
 
-		if (_range_buffer == nullptr || !_range_buffer->valid()) {
-			delete _range_buffer;
-			_range_buffer = nullptr;
-			printBufferAllocationFailed("range");
+			if (_range_buffer == nullptr || !_range_buffer->valid()) {
+				delete _range_buffer;
+				_range_buffer = nullptr;
+				printBufferAllocationFailed("range");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -324,18 +324,19 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_flow_buffer == nullptr) {
-		_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);
+		if (_imu_buffer_length > 0) {
+			_flow_buffer = new RingBuffer<flowSample>(_imu_buffer_length);
 
-		if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
-			delete _flow_buffer;
-			_flow_buffer = nullptr;
-			printBufferAllocationFailed("flow");
+			if (_flow_buffer == nullptr || !_flow_buffer->valid()) {
+				delete _flow_buffer;
+				_flow_buffer = nullptr;
+				printBufferAllocationFailed("flow");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -362,18 +363,19 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_ext_vision_buffer == nullptr) {
-		_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_ext_vision_buffer = new RingBuffer<extVisionSample>(_obs_buffer_length);
 
-		if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
-			delete _ext_vision_buffer;
-			_ext_vision_buffer = nullptr;
-			printBufferAllocationFailed("vision");
+			if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) {
+				delete _ext_vision_buffer;
+				_ext_vision_buffer = nullptr;
+				printBufferAllocationFailed("vision");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -402,18 +404,19 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 #if defined(CONFIG_EKF2_AUXVEL)
 void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_auxvel_buffer == nullptr) {
-		_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_auxvel_buffer = new RingBuffer<auxVelSample>(_obs_buffer_length);
 
-		if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
-			delete _auxvel_buffer;
-			_auxvel_buffer = nullptr;
-			printBufferAllocationFailed("aux vel");
+			if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) {
+				delete _auxvel_buffer;
+				_auxvel_buffer = nullptr;
+				printBufferAllocationFailed("aux vel");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -439,18 +442,19 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 
 void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 {
-	if (!_initialised) {
-		return;
-	}
-
 	// Allocate the required buffer size if not previously done
 	if (_system_flag_buffer == nullptr) {
-		_system_flag_buffer = new RingBuffer<systemFlagUpdate>(_obs_buffer_length);
+		if (_obs_buffer_length > 0) {
+			_system_flag_buffer = new RingBuffer<systemFlagUpdate>(_obs_buffer_length);
 
-		if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) {
-			delete _system_flag_buffer;
-			_system_flag_buffer = nullptr;
-			printBufferAllocationFailed("system flag");
+			if (_system_flag_buffer == nullptr || !_system_flag_buffer->valid()) {
+				delete _system_flag_buffer;
+				_system_flag_buffer = nullptr;
+				printBufferAllocationFailed("system flag");
+				return;
+			}
+
+		} else {
 			return;
 		}
 	}
@@ -477,16 +481,21 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
 	// sufficient samples have been collected
-	if (_params.drag_ctrl > 0) {
+	if (_params.drag_ctrl) {
 
 		// Allocate the required buffer size if not previously done
 		if (_drag_buffer == nullptr) {
-			_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);
+			if (_obs_buffer_length > 0) {
+				_drag_buffer = new RingBuffer<dragSample>(_obs_buffer_length);
 
-			if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
-				delete _drag_buffer;
-				_drag_buffer = nullptr;
-				printBufferAllocationFailed("drag");
+				if (_drag_buffer == nullptr || !_drag_buffer->valid()) {
+					delete _drag_buffer;
+					_drag_buffer = nullptr;
+					printBufferAllocationFailed("drag");
+					return;
+				}
+
+			} else {
 				return;
 			}
 		}
@@ -536,7 +545,7 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 }
 #endif // CONFIG_EKF2_DRAG_FUSION
 
-bool EstimatorInterface::initialise_interface(uint64_t timestamp)
+bool EstimatorInterface::initialise_interface()
 {
 	const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
 
@@ -560,11 +569,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		return false;
 	}
 
-	_time_delayed_us = timestamp;
-	_time_latest_us = timestamp;
-
-	_fault_status.value = 0;
-
 	return true;
 }
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -318,9 +318,7 @@ public:
 protected:
 
 	EstimatorInterface() = default;
-	virtual ~EstimatorInterface();
-
-	virtual bool init(uint64_t timestamp) = 0;
+	~EstimatorInterface();
 
 	parameters _params{};		// filter parameters
 
@@ -346,6 +344,11 @@ protected:
 	uint64_t _time_delayed_us{0}; // captures the imu sample on the delayed time horizon
 	uint64_t _time_latest_us{0}; // imu sample capturing the newest imu data
 
+	// Variables used by the initial filter alignment
+	bool _is_first_imu_sample{true};
+	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
+	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
+
 	OutputPredictor _output_predictor{};
 
 #if defined(CONFIG_EKF2_AIRSPEED)
@@ -378,7 +381,6 @@ protected:
 	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};		// air density (kg/m**3)
 
 	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
-	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
 
 	// Variables used to publish the WGS-84 location of the EKF local NED origin
 	MapProjection _local_origin_lat_lon{};
@@ -449,7 +451,7 @@ protected:
 	fault_status_u _fault_status{};
 
 	// allocate data buffers and initialize interface variables
-	bool initialise_interface(uint64_t timestamp);
+	bool initialise_interface();
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	uint64_t _wmm_mag_time_last_checked {0}; // time WMM update last checked by mag control

src/modules/ekf2/EKF2.cpp

@@ -833,6 +833,14 @@ void EKF2::Run()
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 			UpdateMagCalibration(now);
 #endif // CONFIG_EKF2_MAGNETOMETER
+
+		} else {
+			// if filter not initialized
+
+
+
+
+
 		}
 
 		// publish ekf2_timestamps

src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv

@@ -1,6 +1,6 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,0,0,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
-90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,0,0,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
+10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,0,0,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
+90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,0,0,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
 190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,0,0,-1.2e+02,-3e-11,-2.6e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,-1.2e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082
 290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,0,0,-1.2e+02,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11
 390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0,0,-1.2e+02,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,-1.2e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13

src/modules/ekf2/test/change_indication/iris_gps.csv

@@ -1,6 +1,6 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,0,0,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
-90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,0,0,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
+10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,0,0,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032
+90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,0,0,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058
 190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.035,0,0,-4.9e+02,1.6e-09,-1.4e-09,-6.4e-11,0,0,-3.2e-06,0,0,0,0,0,0,0,0,-4.9e+02,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082
 290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.038,0,0,-4.9e+02,7.3e-09,-1.1e-08,-3.9e-10,0,0,-2e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11
 390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.05,0,0,-4.9e+02,-2.1e-10,-1.4e-08,-3.2e-10,0,0,-1.4e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13

src/modules/ekf2/test/test_EKF_accelerometer.cpp

@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

src/modules/ekf2/test/test_EKF_airspeed.cpp

@@ -62,7 +62,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

src/modules/ekf2/test/test_EKF_basics.cpp

@@ -54,7 +54,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

src/modules/ekf2/test/test_EKF_drag_fusion.cpp

@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_is_fixed_wing(false);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

src/modules/ekf2/test/test_EKF_externalVision.cpp

@@ -62,7 +62,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}

src/modules/ekf2/test/test_EKF_fake_pos.cpp

@@ -56,7 +56,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

src/modules/ekf2/test/test_EKF_flow.cpp

@@ -65,7 +65,7 @@ public:
 		_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);
 
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

src/modules/ekf2/test/test_EKF_fusionLogic.cpp

@@ -60,7 +60,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

src/modules/ekf2/test/test_EKF_gnss_yaw.cpp

@@ -62,7 +62,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

src/modules/ekf2/test/test_EKF_gyroscope.cpp

@@ -59,7 +59,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}

src/modules/ekf2/test/test_EKF_height_fusion.cpp

@@ -58,7 +58,7 @@ public:
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf_wrapper.disableBaroHeightFusion();
 		_ekf_wrapper.disableRangeHeightFusion();
 		_sensor_simulator.runSeconds(0.1);

src/modules/ekf2/test/test_EKF_imuSampling.cpp

@@ -47,8 +47,7 @@ public:
 	// Setup the Ekf with synthetic measurements
 	void SetUp() override
 	{
-		_ekf.init(0);
-
+		_ekf.reset();
 	}
 
 	void TearDown() override

src/modules/ekf2/test/test_EKF_initialization.cpp

@@ -56,7 +56,7 @@ public:
 	void SetUp() override
 	{
 		// first init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}

src/modules/ekf2/test/test_EKF_mag.cpp

@@ -58,7 +58,7 @@ public:
 	void SetUp() override
 	{
 		// Init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);
 	}

src/modules/ekf2/test/test_EKF_measurementSampling.cpp

@@ -45,7 +45,7 @@ public:
 
 	void SetUp() override
 	{
-		_ekf->init(0);
+		_ekf->reset();
 	}
 };
 

src/modules/ekf2/test/test_EKF_terrain.cpp

@@ -58,7 +58,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);

src/modules/ekf2/test/test_EKF_yaw_estimator.cpp

@@ -58,7 +58,7 @@ public:
 	void SetUp() override
 	{
 		// run briefly to init, then manually set in air and at rest (default for a real vehicle)
-		_ekf->init(0);
+		_ekf->reset();
 		_sensor_simulator.runSeconds(0.1);
 		_ekf->set_in_air_status(false);
 		_ekf->set_vehicle_at_rest(true);