EKF: Add control of optical flow and range finder fusion

EKF/control.cpp

@@ -49,12 +49,71 @@ void Ekf::controlFusionModes()
 	// Get the magnetic declination
 	calcMagDeclination();
 
+	// Check for tilt convergence during initial alignment
+	// filter the tilt error vector using a 1 sec time constant LPF
+	float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt;
+	_tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt;
+
+	// Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states
+	if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) {
+		_control_status.flags.tilt_align = true;
+		_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+	}
+
 	// optical flow fusion mode selection logic
-	_control_status.flags.opt_flow = false;
+	// to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data
+	if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align
+	    && (_time_last_imu - _time_last_optflow) < 5e5 && (_time_last_imu - _time_last_hagl_fuse) < 5e5) {
+		// If the heading is not aligned, reset the yaw and magnetic field states
+		if (!_control_status.flags.yaw_align) {
+			_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
+		}
+
+		// If the heading is valid, start using optical flow aiding
+		if (_control_status.flags.yaw_align) {
+			// set the flag and reset the fusion timeout
+			_control_status.flags.opt_flow = true;
+			_time_last_of_fuse = _time_last_imu;
+
+			// if we are not using GPS and are in air, then we need to reset the velocity to be consistent with the optical flow reading
+			if (!_control_status.flags.gps) {
+				// calculate the rotation matrix from body to earth frame
+				matrix::Dcm<float> body_to_earth(_state.quat_nominal);
+
+				// constrain height above ground to be above minimum possible
+				float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
+
+				// calculate absolute distance from focal point to centre of frame assuming a flat earth
+				float range = heightAboveGndEst / body_to_earth(2, 2);
+
+				if (_in_air && (range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
+					// calculate X and Y body relative velocities from OF measurements
+					Vector3f vel_optflow_body;
+					vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
+					vel_optflow_body(1) =   range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
+					vel_optflow_body(2) = 0.0f;
+
+					// rotate from body to earth frame
+					Vector3f vel_optflow_earth;
+					vel_optflow_earth = body_to_earth * vel_optflow_body;
+
+					// take x and Y components
+					_state.vel(0) = vel_optflow_earth(0);
+					_state.vel(1) = vel_optflow_earth(1);
+
+				} else {
+					_state.vel.setZero();
+				}
+			}
+		}
+
+	} else if (!(_params.fusion_mode & MASK_USE_OF)) {
+		_control_status.flags.opt_flow = false;
+	}
 
 	// GPS fusion mode selection logic
-	// To start using GPS we need tilt and yaw alignment completed, the local NED origin set and fresh GPS data
-	if (!_control_status.flags.gps) {
+	// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
+	if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
 		if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised
 		    && (_time_last_imu - _last_gps_fail_us > 5e6)) {
 			// If the heading is not aligned, reset the yaw and magnetic field states
@@ -67,13 +126,12 @@ void Ekf::controlFusionModes()
 				resetPosition();
 				resetVelocity();
 				_control_status.flags.gps = true;
+				_time_last_gps = _time_last_imu;
 			}
 		}
-	}
 
-	// decide when to start using optical flow data
-	if (!_control_status.flags.opt_flow) {
-		// TODO optical flow start logic
+	}  else if (!(_params.fusion_mode & MASK_USE_GPS)) {
+		_control_status.flags.gps = false;
 	}
 
 	// handle the case when we are relying on GPS fusion and lose it
@@ -106,7 +164,15 @@ void Ekf::controlFusionModes()
 
 	// handle the case when we are relying on optical flow fusion and lose it
 	if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
-		// TODO
+		// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
+		if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
+			// Switch to the non-aiding mode, zero the veloity states
+			// and set the synthetic position to the current estimate
+			_control_status.flags.opt_flow = false;
+			_last_known_posNE(0) = _state.pos(0);
+			_last_known_posNE(1) = _state.pos(1);
+			_state.vel.setZero();
+		}
 	}
 
 	// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
@@ -177,6 +243,12 @@ void Ekf::controlFusionModes()
 		_control_status.flags.mag_dec = false;
 	}
 
+	// Control the soure of height measurements for the main filter
+	_control_status.flags.baro_hgt = true;
+	_control_status.flags.rng_hgt = false;
+	_control_status.flags.gps_hgt = false;
+
+
 	// Placeholder for control of wind velocity states estimation
 	// TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement
 	if (false) {
@@ -199,6 +271,7 @@ void Ekf::calculateVehicleStatus()
 
 	// Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming
 	bool in_air = _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f;
+
 	if (!_control_status.flags.in_air && in_air) {
 		_control_status.flags.in_air = true;
 	}

EKF/ekf.cpp

@@ -36,7 +36,7 @@
  * Core functions for ekf attitude and position estimator.
  *
  * @author Roman Bast <bapstroman@gmail.com>
- *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
  */
 
 #include "ekf.h"
@@ -48,6 +48,8 @@ Ekf::Ekf():
 	_fuse_pos(false),
 	_fuse_hor_vel(false),
 	_fuse_vert_vel(false),
+	_fuse_flow(false),
+	_fuse_hagl_data(false),
 	_time_last_fake_gps(0),
 	_time_last_pos_fuse(0),
 	_time_last_vel_fuse(0),
@@ -56,6 +58,7 @@ Ekf::Ekf():
 	_last_disarmed_posD(0.0f),
 	_heading_innov(0.0f),
 	_heading_innov_var(0.0f),
+	_delta_time_of(0.0f),
 	_mag_declination(0.0f),
 	_gpsDriftVelN(0.0f),
 	_gpsDriftVelE(0.0f),
@@ -66,10 +69,14 @@ Ekf::Ekf():
 	_last_gps_fail_us(0),
 	_last_gps_origin_time_us(0),
 	_gps_alt_ref(0.0f),
-	_baro_counter(0),
-	_baro_sum(0.0f),
+	_hgt_counter(0),
+	_hgt_sum(0.0f),
 	_mag_counter(0),
-	_baro_at_alignment(0.0f)
+	_hgt_at_alignment(0.0f),
+	_terrain_vpos(0.0f),
+	_hagl_innov(0.0f),
+	_hagl_innov_var(0.0f),
+	_time_last_hagl_fuse(0)
 {
 	_control_status = {};
 	_control_status_prev = {};
@@ -89,6 +96,8 @@ Ekf::Ekf():
 	_q_down_sampled.setZero();
 	_mag_sum = {};
 	_delVel_sum = {};
+	_flow_gyro_bias = {};
+	_imu_del_ang_of = {};
 }
 
 Ekf::~Ekf()
@@ -137,93 +146,137 @@ bool Ekf::init(uint64_t timestamp)
 	_mag_healthy = false;
 
 	_filter_initialised = false;
+	_terrain_initialised = false;
 
 	return ret;
 }
 
 bool Ekf::update()
 {
-	bool ret = false;	// indicates if there has been an update
-
-	if (!_filter_initialised) {
-		_filter_initialised = initialiseFilter();
 
+	// Only run the filter if IMU data in the buffer has been updated
+	if (_imu_updated) {
 		if (!_filter_initialised) {
-			return false;
+			_filter_initialised = initialiseFilter();
+
+			if (!_filter_initialised) {
+				return false;
+			}
 		}
-	}
 
-	//printStates();
-	//printStatesFast();
-	// prediction
-	if (_imu_updated) {
-		ret = true;
+		// perform state and covariance prediction for the main filter
 		predictState();
 		predictCovariance();
-	}
 
-	// control logic
-	controlFusionModes();
+		// perform state and variance prediction for the terrain estimator
+		if (!_terrain_initialised) {
+			_terrain_initialised = initHagl();
+
+		} else {
+			predictHagl();
+		}
 
-	// measurement updates
+		// control logic
+		controlFusionModes();
 
-	// Fuse magnetometer data using the selected fusion method and only if angular alignment is complete
-	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
-		if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
-			fuseMag();
+		// measurement updates
 
-			if (_control_status.flags.mag_dec) {
-				fuseDeclination();
+		// Fuse magnetometer data using the selected fuson method and only if angular alignment is complete
+		if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
+			if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
+				fuseMag();
+
+				if (_control_status.flags.mag_dec) {
+					fuseDeclination();
+				}
+
+			} else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) {
+				fuseMag2D();
+
+			} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
+				// fusion of a Euler yaw angle from either a 321 or 312 rotation sequence
+				fuseHeading();
+
+			} else {
+				// do no fusion at all
 			}
+		}
 
-		} else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) {
-			fuseMag2D();
+		// determine if we have height data that can be fused
+		if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
+		    && (_R_prev(2, 2) > 0.7071f)) {
+			// if we have range data we always try to estimate terrain nheight
+			_fuse_hagl_data = true;
 
-		} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
-			// fusion of a Euler yaw angle from either a 321 or 312 rotation sequence
-			fuseHeading();
+			// only use if for height in the main filter if specifically enabled
+			if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
+				_fuse_height = true;
+			}
 
-		} else {
-			// do no fusion at all
+		} else if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)
+			   && _params.vdist_sensor_type == VDIST_SENSOR_BARO) {
+			_fuse_height = true;
 		}
-	}
 
-	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
-		_fuse_height = true;
-	}
+		// If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it
+		// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
+		// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
+		if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) {
+			_fuse_pos = true;
+			_fuse_vert_vel = true;
+			_fuse_hor_vel = true;
+			_fuse_flow = false;
+
+		} else if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
+			   && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
+			   && (_R_prev(2, 2) > 0.7071f)) {
+			_fuse_pos = false;
+			_fuse_vert_vel = false;
+			_fuse_hor_vel = false;
+			_fuse_flow = true;
+
+		} else if (!_control_status.flags.gps && !_control_status.flags.opt_flow
+			   && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
+			_fuse_pos = true;
+			_gps_sample_delayed.pos(0) = _last_known_posNE(0);
+			_gps_sample_delayed.pos(1) = _last_known_posNE(1);
+			_time_last_fake_gps = _time_last_imu;
+		}
 
-	// If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it
-	// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
-	// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
-	if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) {
-		_fuse_pos = true;
-		_fuse_vert_vel = true;
-		_fuse_hor_vel = true;
-
-	} else if (!_control_status.flags.gps && !_control_status.flags.opt_flow
-		   && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
-		_fuse_pos = true;
-		_gps_sample_delayed.pos(0) = _last_known_posNE(0);
-		_gps_sample_delayed.pos(1) = _last_known_posNE(1);
-		_time_last_fake_gps = _time_last_imu;
-	}
+		// fuse available range finder data into a terrain height estimator if it has been initialised
+		if (_fuse_hagl_data && _terrain_initialised) {
+			fuseHagl();
+			_fuse_hagl_data = false;
+		}
 
-	if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
-		fuseVelPosHeight();
-		_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
-	}
+		// Fuse available NED velocity and position data into the main filter
+		if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
+			fuseVelPosHeight();
+			_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
+		}
 
-	if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
-		fuseRange();
-	}
+		// Update optical flow bias estimates
+		calcOptFlowBias();
 
-	if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
-		fuseAirspeed();
+		// Fuse optical flow LOS rate observations into the main filter
+		if (_fuse_flow) {
+			fuseOptFlow();
+			_last_known_posNE(0) = _state.pos(0);
+			_last_known_posNE(1) = _state.pos(1);
+			_fuse_flow = false;
+		}
 	}
 
+	// the output observer always runs
 	calculateOutputStates();
 
-	return ret;
+	// We don't have valid data to output until tilt and yaw alignment is complete
+	if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
+		return true;
+
+	} else {
+		return false;
+	}
 }
 
 bool Ekf::initialiseFilter(void)
@@ -231,7 +284,8 @@ bool Ekf::initialiseFilter(void)
 	// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter
 
 	// Sum the IMU delta angle measurements
-	_delVel_sum += _imu_down_sampled.delta_vel;
+	imuSample imu_init = _imu_buffer.get_newest();
+	_delVel_sum += imu_init.delta_vel;
 
 	// Sum the magnetometer measurements
 	magSample mag_init = _mag_buffer.get_newest();
@@ -241,17 +295,26 @@ bool Ekf::initialiseFilter(void)
 		_mag_sum += mag_init.mag;
 	}
 
-	// Sum the barometer measurements
-	// initialize vertical position with newest baro measurement
-	baroSample baro_init = _baro_buffer.get_newest();
+	if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
+		rangeSample range_init = _range_buffer.get_newest();
 
-	if (baro_init.time_us != 0) {
-		_baro_counter ++;
-		_baro_sum += baro_init.hgt;
+		if (range_init.time_us != 0) {
+			_hgt_counter ++;
+			_hgt_sum += range_init.rng;
+		}
+
+	} else {
+		// initialize vertical position with newest baro measurement
+		baroSample baro_init = _baro_buffer.get_newest();
+
+		if (baro_init.time_us != 0) {
+			_hgt_counter ++;
+			_hgt_sum += baro_init.hgt;
+		}
 	}
 
-	// check to see if we have enough measurements and return false if not
-	if (_baro_counter < 10 || _mag_counter < 10) {
+	// check to see if we have enough measruements and return false if not
+	if (_hgt_counter < 10 || _mag_counter < 10) {
 		return false;
 
 	} else {
@@ -283,16 +346,18 @@ bool Ekf::initialiseFilter(void)
 		matrix::Euler<float> euler_init(roll, pitch, 0.0f);
 		_state.quat_nominal = Quaternion(euler_init);
 		_output_new.quat_nominal = _state.quat_nominal;
-		_control_status.flags.tilt_align = true;
+
+		// initialise the filtered alignment error estimate to a larger starting value
+		_tilt_err_length_filt = 1.0f;
 
 		// calculate the averaged magnetometer reading
 		Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter)));
 
 		// calculate the initial magnetic field and yaw alignment
-		_control_status.flags.yaw_align = resetMagHeading(mag_init);
+		resetMagHeading(mag_init);
 
 		// calculate the averaged barometer reading
-		_baro_at_alignment = _baro_sum / (float)_baro_counter;
+		_hgt_at_alignment = _hgt_sum / (float)_hgt_counter;
 
 		// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)
 		resetVelocity();
@@ -300,6 +365,9 @@ bool Ekf::initialiseFilter(void)
 		// initialise the state covariance matrix
 		initialiseCovariance();
 
+		// initialise the terrain estimator
+		initHagl();
+
 		return true;
 	}
 }
@@ -354,7 +422,6 @@ bool Ekf::collect_imu(imuSample &imu)
 	_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt;
 	_imu_down_sampled.delta_vel_dt += imu.delta_vel_dt;
 
-
 	Quaternion delta_q;
 	delta_q.rotate(imu.delta_ang);
 	_q_down_sampled =  _q_down_sampled * delta_q;
@@ -453,8 +520,3 @@ void Ekf::fuseAirspeed()
 {
 
 }
-
-void Ekf::fuseRange()
-{
-
-}