WIP ekf2 init

src/modules/ekf2/EKF/ekf.cpp

@@ -133,18 +133,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 +153,43 @@ 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;
+
+		} 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) {
+			if (initialiseTilt(_accel_lpf.getState(), _gyro_lpf.getState())) {
+
+				// initialise the state covariance matrix now we have starting values for all the states
+				initialiseCovariance();
+
+				_state.vel.zero();
+				_state.pos.zero();
+
+				// reset the output predictor state history to match the EKF initial values
+				_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
+
+				_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, _state.pos,
 						      _state.gyro_bias, _state.accel_bias);
@@ -177,54 +200,22 @@ bool Ekf::update()
 	return false;
 }
 
-bool Ekf::initialiseFilter()
+bool Ekf::initialiseTilt(const Vector3f &accel, const Vector3f &gyro)
 {
-	// Filter accel for tilt initialization
-	const imuSample &imu_init = _imu_buffer.get_newest();
+	const float accel_norm = accel.norm();
+	const float gyro_norm = gyro.norm();
 
-	// 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, _state.pos);
-
-	return true;
-}
-
-bool Ekf::initialiseTilt()
-{
-	const float accel_norm = _accel_lpf.getState().norm();
-	const float gyro_norm = _gyro_lpf.getState().norm();
+	// 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));
+	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	if (accel_norm < 0.8f * CONSTANTS_ONE_G ||
 	    accel_norm > 1.2f * CONSTANTS_ONE_G ||
-	    gyro_norm > math::radians(15.0f)) {
+	    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));
-	_R_to_earth = Dcmf(_state.quat_nominal);
-
 	return true;
 }
 

src/modules/ekf2/EKF/ekf.h

@@ -433,7 +433,7 @@ class Ekf final : public EstimatorInterface
 	// set the internal states and status to their default value
 	void reset();
 
-	bool initialiseTilt();
+	bool initialiseTilt(const Vector3f &accel, const Vector3f &gyro);
 
 	// check if the EKF is dead reckoning horizontal velocity using inertial data only
 	void updateDeadReckoningStatus();
@@ -583,11 +583,6 @@ class Ekf final : public EstimatorInterface
 	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 +621,6 @@ class Ekf final : public EstimatorInterface
 	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.h

@@ -344,6 +344,11 @@ class EstimatorInterface
 	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)

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