Tilt Initialisation: Average Filter -> LowPass Filter (#686)

* Tilt Initialisation: Average Filter -> LowPass Filter

* Add _is_first_imu_sample variable

* Remove not needed comments

EKF/control.cpp

@@ -61,18 +61,18 @@ void Ekf::controlFusionModes()
 
 			// send alignment status message to the console
 			if (_control_status.flags.baro_hgt) {
-				ECL_INFO("EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
+				ECL_INFO("%llu: EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length);
 
 			} else if (_control_status.flags.ev_hgt) {
-				ECL_INFO("EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
+				ECL_INFO("%llu: EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length);
 
 			} else if (_control_status.flags.gps_hgt) {
-				ECL_INFO("EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
+				ECL_INFO("%llu: EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length);
 
 			} else if (_control_status.flags.rng_hgt) {
-				ECL_INFO("EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
+				ECL_INFO("%llu: EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length);
 			} else {
-				ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length);
+				ECL_ERR("%llu: EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length);
 			}
 
 		}

EKF/ekf.cpp

@@ -114,6 +114,8 @@ bool Ekf::update()
 
 	// Only run the filter if IMU data in the buffer has been updated
 	if (_imu_updated) {
+		const imuSample &imu_init = _imu_buffer.get_newest();
+		_accel_lpf.update(imu_init.delta_vel);
 
 		// perform state and covariance prediction for the main filter
 		predictState();
@@ -139,9 +141,17 @@ bool Ekf::initialiseFilter()
 {
 	// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
 
-	// Sum the IMU delta velocity measurements
+	// Filter accel for tilt initialization
 	const imuSample &imu_init = _imu_buffer.get_newest();
-	_delVel_sum += imu_init.delta_vel;
+
+	if(_is_first_imu_sample){
+		_accel_lpf.reset(imu_init.delta_vel);
+		_is_first_imu_sample = false;
+	}
+	else
+	{
+		_accel_lpf.update(imu_init.delta_vel);
+	}
 
 	// Sum the magnetometer measurements
 	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
@@ -199,35 +209,10 @@ bool Ekf::initialiseFilter()
 		_gps_drift_velD = 0.0f;
 		_gps_alt_ref = 0.0f;
 
-		// Zero all of the states
-		_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();
-
-		// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
-		float pitch = 0.0f;
-		float roll = 0.0f;
-
-		if (_delVel_sum.norm() > 0.001f) {
-			_delVel_sum.normalize();
-			pitch = asinf(_delVel_sum(0));
-			roll = atan2f(-_delVel_sum(1), -_delVel_sum(2));
-
-		} else {
+		if(!initialiseTilt()){
 			return false;
 		}
 
-		// calculate initial tilt alignment
-		Eulerf euler_init(roll, pitch, 0.0f);
-		_state.quat_nominal = Quatf(euler_init);
-
-		// update transformation matrix from body to world frame
-		_R_to_earth = Dcmf(_state.quat_nominal);
-
 		// calculate the initial magnetic field and yaw alignment
 		_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false);
 
@@ -258,6 +243,25 @@ bool Ekf::initialiseFilter()
 	}
 }
 
+bool Ekf::initialiseTilt()
+{
+	if (_accel_lpf.getState().norm() < 0.001f) {
+		return false;
+	}
+
+	// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
+	Vector3f gravity_in_body = _accel_lpf.getState();
+	gravity_in_body.normalize();
+	const float pitch = asinf(gravity_in_body(0));
+	const float roll = atan2f(-gravity_in_body(1), -gravity_in_body(2));
+
+	const Eulerf euler_init(roll, pitch, 0.0f);
+	_state.quat_nominal = Quatf(euler_init);
+	_R_to_earth = Dcmf(_state.quat_nominal);
+
+	return true;
+}
+
 void Ekf::predictState()
 {
 	if (!_earth_rate_initialised) {

EKF/ekf.h

@@ -57,6 +57,8 @@ class Ekf : public EstimatorInterface
 	// set the internal states and status to their default value
 	void reset(uint64_t timestamp) override;
 
+	bool initialiseTilt();
+
 	// should be called every time new data is pushed into the filter
 	bool update() override;
 
@@ -462,10 +464,11 @@ class Ekf : public EstimatorInterface
 	// Variables used to initialise the filter states
 	uint32_t _hgt_counter{0};		///< number of height samples read during initialisation
 	float _rng_filt_state{0.0f};		///< filtered height measurement (m)
+	bool _is_first_imu_sample{true};
 	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 	uint64_t _time_last_mag{0};		///< measurement time of last magnetomter sample (uSec)
-	AlphaFilterVector3f _mag_lpf;		///< filtered magnetometer measurement (Gauss)
-	Vector3f _delVel_sum;			///< summed delta velocity (m/sec)
+	AlphaFilterVector3f _mag_lpf;		///< filtered magnetometer measurement for instant reset(Gauss)
+	AlphaFilterVector3f _accel_lpf;		///< filtered accelerometer measurement for instant reset(Gauss)
 	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
 	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)
 

EKF/estimator_interface.cpp

@@ -48,6 +48,7 @@
 // Accumulate imu data and store to buffer at desired rate
 void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 {
+	// TODO: resolve misplaced responsibility
 	if (!_initialised) {
 		init(imu_sample.time_us);
 		_initialised = true;