EKF: Add support auxiliary velocity observation

This enables the EKF to use an additional NE velocity measurement. This can be used to improve position hold stability when landing using a beacon system for positioning by fusing the beacon velocity estimates.

EKF/common.h

@@ -156,6 +156,12 @@ struct dragSample {
 	uint64_t time_us;	///< timestamp of the measurement (uSec)
 };
 
+struct auxVelSample {
+	Vector2f velNE;		///< measured NE velocity relative to the local origin (m/sec)
+	Vector2f velVarNE;	///< estimated error variance of the NE velocity (m/sec)**2
+	uint64_t time_us;	///< timestamp of the measurement (uSec)
+};
+
 // Integer definitions for vdist_sensor_type
 #define VDIST_SENSOR_BARO  0	///< Use baro height
 #define VDIST_SENSOR_GPS   1	///< Use GPS height
@@ -210,6 +216,7 @@ struct parameters {
 	float flow_delay_ms{5.0f};		///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
 	float range_delay_ms{5.0f};		///< range finder measurement delay relative to the IMU (mSec)
 	float ev_delay_ms{100.0f};		///< off-board vision measurement delay relative to the IMU (mSec)
+	float auxvel_delay_ms{0.0f};		///< auxiliary velocity measurement delay relative to the IMU (mSec)
 
 	// input noise
 	float gyro_noise{1.5e-2f};		///< IMU angular rate noise used for covariance prediction (rad/sec)
@@ -326,6 +333,9 @@ struct parameters {
 	float vert_innov_test_lim{4.5f};	///< Number of standard deviations allowed before the combined vertical velocity and position test is declared as failed
 	int bad_acc_reset_delay_us{500000};	///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec)
 
+	// auxilliary velocity fusion
+	float auxvel_noise{0.5f};		///< minimum observation noise, uses reported noise if greater (m/s)
+	float auxvel_gate{5.0f};		///< velocity fusion innovation consistency gate size (STD)
 };
 
 struct stateSample {

EKF/control.cpp

@@ -130,9 +130,12 @@ void Ekf::controlFusionModes()
 	// in a single function using sensor data from multiple sources (GPS, baro, range finder, etc)
 	controlVelPosFusion();
 
-	// Additional data from an external vision sensor can also be fused.
+	// Additional data from an external vision pose estimator can be fused.
 	controlExternalVisionFusion();
 
+	// Additional NE velocity data from an auxiliary sensor can be fused
+	controlAuxVelFusion();
+
 	// report dead reckoning if we are no longer fusing measurements that directly constrain velocity drift
 	_is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > _params.no_aid_timeout_max)
 			&& (_time_last_imu - _time_last_delpos_fuse > _params.no_aid_timeout_max)
@@ -522,13 +525,17 @@ void Ekf::controlGpsFusion()
 			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 			float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
 			_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
+			_velObsVarNE(1) = _velObsVarNE(0) = sq(fmaxf(_params.gps_vel_noise, 0.01f));
 
 			// calculate innovations
+			_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
+			_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
 			_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
 			_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
 
 			// set innovation gate size
 			_posInnovGateNE = fmaxf(_params.posNE_innov_gate, 1.0f);
+			_hvelInnovGate = fmaxf(_params.vel_innov_gate, 1.0f);
 		}
 
 	} else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > (uint64_t)10e6)) {
@@ -1355,3 +1362,20 @@ void Ekf::controlVelPosFusion()
 
 	}
 }
+
+void Ekf::controlAuxVelFusion()
+{
+	bool data_ready = _auxvel_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed);
+	bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow;
+
+	if (data_ready && primary_aiding) {
+		_fuse_vert_vel = _fuse_pos = _fuse_height = false;
+		_fuse_hor_vel = true;
+		_vel_pos_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
+		_vel_pos_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
+		_velObsVarNE = _auxvel_sample_delayed.velVarNE;
+		_hvelInnovGate = _params.auxvel_gate;
+		fuseVelPosHeight();
+		_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
+	}
+}

EKF/ekf.h

@@ -264,6 +264,9 @@ private:
 	float _posObsNoiseNE;		///< 1-STD observtion noise used for the fusion of NE position data (m)
 	float _posInnovGateNE;		///< Number of standard deviations used for the NE position fusion innovation consistency check
 
+	Vector2f _velObsVarNE;		///< 1-STD observation noise variance used for the fusion of NE velocity data (m/sec)**2
+	float _hvelInnovGate;		///< Number of standard deviations used for the horizontal velocity fusion innovation consistency check
+
 	// variables used when position data is being fused using a relative position odometry model
 	bool _fuse_hpos_as_odom{false};		///< true when the NE position data is being fused using an odometry assumption
 	Vector3f _pos_meas_prev;		///< previous value of NED position measurement fused using odometry assumption (m)
@@ -561,6 +564,9 @@ private:
 	// control fusion of velocity and position observations
 	void controlVelPosFusion();
 
+	// control fusion of auxiliary velocity observations
+	void controlAuxVelFusion();
+
 	// control for height sensor timeouts, sensor changes and state resets
 	void controlHeightSensorTimeouts();
 

EKF/estimator_interface.cpp

@@ -442,6 +442,38 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
 	}
 }
 
+void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], float (&variance)[2])
+{
+	if (!_initialised || _auxvel_buffer_fail) {
+		return;
+	}
+
+	// Allocate the required buffer size if not previously done
+	// Do not retry if allocation has failed previously
+	if (_auxvel_buffer.get_length() < _obs_buffer_length) {
+		_auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length);
+		if (_auxvel_buffer_fail) {
+			ECL_ERR("EKF aux vel buffer allocation failed");
+			return;
+		}
+	}
+
+	// limit data rate to prevent data being lost
+	if (time_usec - _time_last_auxvel > _min_obs_interval_us) {
+
+		auxVelSample auxvel_sample_new;
+		auxvel_sample_new.time_us = time_usec - _params.auxvel_delay_ms * 1000;
+
+		auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
+		_time_last_auxvel = time_usec;
+
+		auxvel_sample_new.velNE = Vector2f(data);
+		auxvel_sample_new.velVarNE = Vector2f(variance);
+
+		_auxvel_buffer.push(auxvel_sample_new);
+	}
+}
+
 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
 	// find the maximum time delay the buffers are required to handle
@@ -450,8 +482,9 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 					     math::max(_params.gps_delay_ms,
 						 math::max(_params.flow_delay_ms,
 						     math::max(_params.ev_delay_ms,
+							 math::max(_params.auxvel_delay_ms,
 							     math::max(_params.min_delay_ms,
-								math::max(_params.airspeed_delay_ms, _params.baro_delay_ms)))))));
+								 math::max(_params.airspeed_delay_ms, _params.baro_delay_ms))))))));
 
 	// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
 	_imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
@@ -511,6 +544,7 @@ void EstimatorInterface::unallocate_buffers()
 	_output_buffer.unallocate();
 	_output_vert_buffer.unallocate();
 	_drag_buffer.unallocate();
+	_auxvel_buffer.unallocate();
 
 }
 

EKF/estimator_interface.h

@@ -185,6 +185,9 @@ public:
 	// set external vision position and attitude data
 	void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata);
 
+	// set auxiliary velocity data
+	void setAuxVelData(uint64_t time_usec, float (&data)[2], float (&variance)[2]);
+
 	// return a address to the parameters struct
 	// in order to give access to the application
 	parameters *getParamHandle() {return &_params;}
@@ -402,6 +405,7 @@ protected:
 	extVisionSample _ev_sample_delayed{};
 	dragSample _drag_sample_delayed{};
 	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
+	auxVelSample _auxvel_sample_delayed{};
 
 	// Used by the multi-rotor specific drag force fusion
 	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
@@ -463,6 +467,7 @@ protected:
 	RingBuffer<outputSample> _output_buffer;
 	RingBuffer<outputVert> _output_vert_buffer;
 	RingBuffer<dragSample> _drag_buffer;
+	RingBuffer<auxVelSample> _auxvel_buffer;
 
 	// observation buffer final allocation failed
 	bool _gps_buffer_fail{false};
@@ -473,6 +478,7 @@ protected:
 	bool _flow_buffer_fail{false};
 	bool _ev_buffer_fail{false};
 	bool _drag_buffer_fail{false};
+	bool _auxvel_buffer_fail{false};
 
 	uint64_t _time_last_imu{0};	// timestamp of last imu sample in microseconds
 	uint64_t _time_last_gps{0};	// timestamp of last gps measurement in microseconds
@@ -483,6 +489,7 @@ protected:
 	uint64_t _time_last_ext_vision{0}; // timestamp of last external vision measurement in microseconds
 	uint64_t _time_last_optflow{0};
 	uint64_t _time_last_gnd_effect_on{0};	//last time the baro ground effect compensation was turned on externally (uSec)
+	uint64_t _time_last_auxvel{0};
 
 	fault_status_u _fault_status{};
 

EKF/vel_pos_fusion.cpp

@@ -54,18 +54,13 @@ void Ekf::fuseVelPosHeight()
 
 	// calculate innovations, innovations gate sizes and observation variances
 	if (_fuse_hor_vel) {
+		// enable fusion for NE velocity axes
 		fuse_map[0] = fuse_map[1] = true;
-		// horizontal velocity innovations
+
-		_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
+		// Set observation noise variance and innovation consistency check gate size for the NE position observations
-		_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
+		R[0] = _velObsVarNE(0);
-		// observation variance - use receiver reported accuracy with parameter setting the minimum value
+		R[1] = _velObsVarNE(1);
-		R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
+		gate_size[1] = gate_size[0] = _hvelInnovGate;
-		R[0] = fmaxf(R[0], _gps_sample_delayed.sacc);
-		R[0] = R[0] * R[0];
-		R[1] = R[0];
-		// innovation gate sizes
-		gate_size[0] = fmaxf(_params.vel_innov_gate, 1.0f);
-		gate_size[1] = gate_size[0];
 	}
 
 	if (_fuse_vert_vel) {