Merge pull request #362 from PX4/pr-ekfAuxVelFuse

EKF: Add additional velocity interface to use landing beacon data

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)) {
@@ -1351,7 +1358,22 @@ void Ekf::controlVelPosFusion()
 	// 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;
 
 	}
 }
+
+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_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
+		_fuse_hor_vel_aux = true;
+		_aux_vel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
+		_aux_vel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
+		_velObsVarNE = _auxvel_sample_delayed.velVarNE;
+		_hvelInnovGate = _params.auxvel_gate;
+		fuseVelPosHeight();
+	}
+}

EKF/ekf.h

@@ -60,6 +60,9 @@ public:
 	// 0-2 vel, 3-5 pos
 	void get_vel_pos_innov(float vel_pos_innov[6]);
 
+	// gets the innovations for of the NE auxiliary velocity measurement
+	void get_aux_vel_innov(float aux_vel_innov[2]);
+
 	// gets the innovations of the earth magnetic field measurements
 	void get_mag_innov(float mag_innov[3]);
 
@@ -260,10 +263,14 @@ private:
 	bool _fuse_pos{false};		///< true when gps position data should be fused
 	bool _fuse_hor_vel{false};	///< true when gps horizontal velocity measurement should be fused
 	bool _fuse_vert_vel{false};	///< true when gps vertical velocity measurement should be fused
+	bool _fuse_hor_vel_aux{false};	///< true when auxiliary horizontal velocity measurement should be fused
 
 	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)
@@ -316,8 +323,9 @@ private:
 
 	float P[_k_num_states][_k_num_states] {};	///< state covariance matrix
 
-	float _vel_pos_innov[6] {};	///< NED velocity and position innovations: 0-2 vel (m/sec),  3-5 pos (m**2)
+	float _vel_pos_innov[6] {};	///< NED velocity and position innovations: 0-2 vel (m/sec),  3-5 pos (m)
 	float _vel_pos_innov_var[6] {};	///< NED velocity and position innovation variances: 0-2 vel ((m/sec)**2), 3-5 pos (m**2)
+	float _aux_vel_innov[2] {};	///< NE auxiliary velocity innovations: (m/sec)
 
 	float _mag_innov[3] {};		///< earth magnetic field innovations (Gauss)
 	float _mag_innov_var[3] {};	///< earth magnetic field innovation variance (Gauss**2)
@@ -561,6 +569,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/ekf_helper.cpp

@@ -756,6 +756,12 @@ void Ekf::get_vel_pos_innov(float vel_pos_innov[6])
 	memcpy(vel_pos_innov, _vel_pos_innov, sizeof(float) * 6);
 }
 
+// gets the innovations of the earth magnetic field measurements
+void Ekf::get_aux_vel_innov(float aux_vel_innov[2])
+{
+	memcpy(aux_vel_innov, _aux_vel_innov, sizeof(float) * 2);
+}
+
 // writes the innovations of the earth magnetic field measurements
 void Ekf::get_mag_innov(float mag_innov[3])
 {

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.min_delay_ms,
-								math::max(_params.airspeed_delay_ms, _params.baro_delay_ms)))))));
+							 math::max(_params.auxvel_delay_ms,
+							     math::max(_params.min_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

@@ -61,6 +61,9 @@ public:
 	// 0-2 vel, 3-5 pos
 	virtual void get_vel_pos_innov(float vel_pos_innov[6]) = 0;
 
+	// gets the innovations for of the NE auxiliary velocity measurement
+	virtual void get_aux_vel_innov(float aux_vel_innov[2]) = 0;
+
 	// gets the innovations of the earth magnetic field measurements
 	virtual void get_mag_innov(float mag_innov[3]) = 0;
 
@@ -185,6 +188,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 +408,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 +470,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 +481,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 +492,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

@@ -51,27 +51,31 @@ void Ekf::fuseVelPosHeight()
 	float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD]
 	float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations
 	float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used
+	float innovation[6]; // local copy of innovations for  [VN,VE,VD,PN,PE,PD]
+	memcpy(innovation, _vel_pos_innov, sizeof(_vel_pos_innov));
 
 	// calculate innovations, innovations gate sizes and observation variances
-	if (_fuse_hor_vel) {
+	if (_fuse_hor_vel || _fuse_hor_vel_aux) {
+		// 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);
-		_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
-		// observation variance - use receiver reported accuracy with parameter setting the minimum value
-		R[0] = fmaxf(_params.gps_vel_noise, 0.01f);
-		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];
+
+		// handle special case where we are getting velocity observations from an auxiliary source
+		if (!_fuse_hor_vel) {
+			innovation[0] = _aux_vel_innov[0];
+			innovation[1] = _aux_vel_innov[1];
+		}
+
+		// Set observation noise variance and innovation consistency check gate size for the NE position observations
+		R[0] = _velObsVarNE(0);
+		R[1] = _velObsVarNE(1);
+		gate_size[1] = gate_size[0] = _hvelInnovGate;
+
 	}
 
 	if (_fuse_vert_vel) {
 		fuse_map[2] = true;
 		// vertical velocity innovation
-		_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
+		innovation[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
 		// observation variance - use receiver reported accuracy with parameter setting the minimum value
 		R[2] = fmaxf(_params.gps_vel_noise, 0.01f);
 		// use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP
@@ -95,7 +99,7 @@ void Ekf::fuseVelPosHeight()
 		if (_control_status.flags.baro_hgt) {
 			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 - _baro_hgt_offset - _hgt_sensor_offset;
+			innovation[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
 			// observation variance - user parameter defined
 			R[5] = fmaxf(_params.baro_noise, 0.01f);
 			R[5] = R[5] * R[5];
@@ -119,7 +123,7 @@ void Ekf::fuseVelPosHeight()
 		} else if (_control_status.flags.gps_hgt) {
 			fuse_map[5] = true;
 			// vertical position innovation - gps measurement has opposite sign to earth z axis
-			_vel_pos_innov[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
+			innovation[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
 			// observation variance - receiver defined and parameter limited
 			// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
 			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
@@ -132,7 +136,7 @@ void Ekf::fuseVelPosHeight()
 		} else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) {
 			fuse_map[5] = true;
 			// use range finder with tilt correction
-			_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
+			innovation[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
 							     _params.rng_gnd_clearance)) - _hgt_sensor_offset;
 			// observation variance - user parameter defined
 			R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
@@ -141,7 +145,7 @@ void Ekf::fuseVelPosHeight()
 		} else if (_control_status.flags.ev_hgt) {
 			fuse_map[5] = true;
 			// calculate the innovation assuming the external vision observaton is in local NED frame
-			_vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
+			innovation[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
 			// observation variance - defined externally
 			R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
 			R[5] = R[5] * R[5];
@@ -158,7 +162,7 @@ void Ekf::fuseVelPosHeight()
 			unsigned state_index = obs_index + 4;	// we start with vx and this is the 4. state
 			_vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index];
 			// Compute the ratio of innovation to gate size
-			_vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) *
+			_vel_pos_test_ratio[obs_index] = sq(innovation[obs_index]) / (sq(gate_size[obs_index]) *
 							 _vel_pos_innov_var[obs_index]);
 		}
 	}
@@ -175,12 +179,13 @@ void Ekf::fuseVelPosHeight()
 	innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f) || !_control_status.flags.tilt_align;
 
 	// record the successful velocity fusion event
-	if (vel_check_pass && _fuse_hor_vel) {
+	if ((_fuse_hor_vel || _fuse_hor_vel_aux) && vel_check_pass) {
 		_time_last_vel_fuse = _time_last_imu;
 		_innov_check_fail_status.flags.reject_vel_NED = false;
 	} else if (!vel_check_pass) {
 		_innov_check_fail_status.flags.reject_vel_NED = true;
 	}
+	_fuse_hor_vel = _fuse_hor_vel_aux = false;
 
 	// record the successful position fusion event
 	if (pos_check_pass && _fuse_pos) {
@@ -193,6 +198,7 @@ void Ekf::fuseVelPosHeight()
 	} else if (!pos_check_pass) {
 		_innov_check_fail_status.flags.reject_pos_NE = true;
 	}
+	_fuse_pos = false;
 
 	// record the successful height fusion event
 	if (innov_check_pass_map[5] && _fuse_height) {
@@ -201,6 +207,7 @@ void Ekf::fuseVelPosHeight()
 	} else if (!innov_check_pass_map[5]) {
 		_innov_check_fail_status.flags.reject_pos_D = true;
 	}
+	_fuse_height = false;
 
 	for (unsigned obs_index = 0; obs_index < 6; obs_index++) {
 		// skip fusion if not requested or checks have failed
@@ -280,7 +287,7 @@ void Ekf::fuseVelPosHeight()
 			fixCovarianceErrors();
 
 			// apply the state corrections
-			fuse(Kfusion, _vel_pos_innov[obs_index]);
+			fuse(Kfusion, innovation[obs_index]);
 		}
 	}
 }