EKF: Correct for sensor noise and baro offset during alignment

EKF/ekf.cpp

@@ -196,73 +196,89 @@ bool Ekf::update()
 
 bool Ekf::initialiseFilter(void)
 {
-	_state.ang_error.setZero();
-	_state.vel.setZero();
-	_state.pos.setZero();
-	_state.gyro_bias.setZero();
-	_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
-	_state.accel_z_bias = 0.0f;
-	_state.mag_I.setZero();
-	_state.mag_B.setZero();
-	_state.wind_vel.setZero();
+	// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter
 
-	// get initial roll and pitch estimate from accel vector, assuming vehicle is static
-	Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt;
+	// Sum the IMU delta angle measurements
+	_delVel_sum += _imu_down_sampled.delta_vel;
 
-	float pitch = 0.0f;
-	float roll = 0.0f;
-
-	if (accel_init.norm() > 0.001f) {
-		accel_init.normalize();
-
-		pitch = asinf(accel_init(0));
-		roll = -asinf(accel_init(1) / cosf(pitch));
-	}
-
-	matrix::Euler<float> euler_init(roll, pitch, 0.0f);
-
-	// Get the latest magnetic field measurement.
-	// If we don't have a measurement then we cannot initialise the filter
+	// Sum the magnetometer measurements
 	magSample mag_init = _mag_buffer.get_newest();
 
-	if (mag_init.time_us == 0) {
-		return false;
+	if (mag_init.time_us != 0) {
+		_mag_counter ++;
+		_mag_sum += mag_init.mag;
 	}
 
-	// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
-	// TODO use declination if available
-	matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
-	Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init.mag;
-	float declination = 0.0f;
-	euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
-
-	// calculate initial quaternion states
-	_state.quat_nominal = Quaternion(euler_init);
-	_output_new.quat_nominal = _state.quat_nominal;
-
-	// TODO replace this with a conditional test based on fitered angle error states.
-	_control_status.flags.angle_align = true;
-
-	// calculate initial earth magnetic field states
-	matrix::Dcm<float> R_to_earth(euler_init);
-	_state.mag_I = R_to_earth * mag_init.mag;
-
-	resetVelocity();
-	resetPosition();
-
+	// Sum the barometer measurements
 	// initialize vertical position with newest baro measurement
 	baroSample baro_init = _baro_buffer.get_newest();
 
-	if (baro_init.time_us == 0) {
-		return false;
+	if (baro_init.time_us != 0) {
+		_baro_counter ++;
+		_baro_sum += baro_init.hgt;
 	}
 
-	_state.pos(2) = -baro_init.hgt;
-	_output_new.pos(2) = -baro_init.hgt;
+	// check to see if we have enough measruements and return false if not
+	if (_baro_counter < 10 || _mag_counter < 10) {
+		return false;
 
-	initialiseCovariance();
+	} else {
+		// Zero all of the states
+		_state.ang_error.setZero();
+		_state.vel.setZero();
+		_state.pos.setZero();
+		_state.gyro_bias.setZero();
+		_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
+		_state.accel_z_bias = 0.0f;
+		_state.mag_I.setZero();
+		_state.mag_B.setZero();
+		_state.wind_vel.setZero();
 
-	return true;
+		// 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 = -asinf(_delVel_sum(1) / cosf(pitch));
+
+		} else {
+			return false;
+		}
+
+		// calculate the averaged magnetometer reading
+		Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter)));
+
+		// rotate magnetic field into earth frame assuming zero yaw and estimate yaw angle assuming zero declination
+		// TODO use declination if available
+		matrix::Euler<float> euler_init(roll, pitch, 0.0f);
+		matrix::Dcm<float> R_to_earth_zeroyaw(euler_init);
+		Vector3f mag_ef_zeroyaw = R_to_earth_zeroyaw * mag_init;
+		float declination = 0.0f;
+		euler_init(2) = declination - atan2f(mag_ef_zeroyaw(1), mag_ef_zeroyaw(0));
+
+		// calculate initial quaternion states
+		_state.quat_nominal = Quaternion(euler_init);
+		_output_new.quat_nominal = _state.quat_nominal;
+
+		// TODO replace this with a conditional test based on fitered angle error states.
+		_control_status.flags.angle_align = true;
+
+		// calculate initial earth magnetic field states
+		matrix::Dcm<float> R_to_earth(euler_init);
+		_state.mag_I = R_to_earth * mag_init;
+
+		// calculate the averaged barometer reading
+		_baro_at_alignment = _baro_sum / (float)_baro_counter;
+
+		resetVelocity();
+		resetPosition();
+
+		initialiseCovariance();
+
+		return true;
+	}
 }
 
 void Ekf::predictState()

EKF/ekf.h

@@ -145,6 +145,13 @@ private:
 	uint64_t _last_gps_origin_time_us = 0;              // time the origin was last set (uSec)
 	float _gps_alt_ref = 0.0f;                          // WGS-84 height (m)
 
+	// Variables used to initialise the filter states
+	uint8_t _baro_counter = 0;      // number of baro samples averaged
+	float _baro_sum = 0.0f;         // summed baro measurement
+	uint8_t _mag_counter = 0;       // number of magnetometer samples averaged
+	Vector3f _mag_sum = {};         // summed magnetometer measurement
+	Vector3f _delVel_sum = {};      // summed delta velocity
+	float _baro_at_alignment;       // baro offset relative to alignment position
 
 	gps_check_fail_status_u _gps_check_fail_status;
 

EKF/ekf_helper.cpp

@@ -78,7 +78,7 @@ void Ekf::resetPosition()
 	}
 
 	baroSample baro_newest = _baro_buffer.get_newest();
-	_state.pos(2) = -baro_newest.hgt;
+	_state.pos(2) = _baro_at_alignment - baro_newest.hgt;
 }
 
 #if defined(__PX4_POSIX) && !defined(__PX4_QURT)

EKF/gps_checks.cpp

@@ -63,7 +63,8 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 			double lat = gps->lat / 1.0e7;
 			double lon = gps->lon / 1.0e7;
 			map_projection_init(&_pos_ref, lat, lon);
-			_gps_alt_ref = gps->alt / 1e3f;
+			// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
+			_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
 			_NED_origin_initialised = true;
 			_last_gps_origin_time_us = _time_last_imu;
 		}

EKF/vel_pos_fusion.cpp

@@ -100,7 +100,7 @@ void Ekf::fuseVelPosHeight()
 	if (_fuse_height) {
 		fuse_map[5] = true;
         // vertical position innovation - baro measurement has opposite sign to earth z axis
-        _vel_pos_innov[5] = _state.pos(2) - (-_baro_sample_delayed.hgt);
+        _vel_pos_innov[5] = _state.pos(2) - (_baro_at_alignment -_baro_sample_delayed.hgt);
         // observation variance - user parameter defined
         R[5] = fmaxf(_params.baro_noise, 0.01f);
         R[5] = R[5] * R[5];