EKF: Add interfaces and variables to use ext vision data

EKF/common.h

@@ -71,6 +71,13 @@ struct flow_message {
 	uint32_t dt;				// integration time of flow samples
 };
 
+struct ext_vision_message {
+	Vector3f posNED;  // measured NED position relative to the local origin (m)
+	Quaternion quat;  // measured quaternion orientation defining rotation from NED to body frame
+	float posErr;     // 1-Sigma spherical position accuracy (m)
+	float angErr;     // 1-Sigma angular error (rad)
+};
+
 struct outputSample {
 	Quaternion  quat_nominal;	// nominal quaternion describing vehicle attitude
 	Vector3f    vel;	// NED velocity estimate in earth frame in m/s
@@ -126,6 +133,14 @@ struct flowSample {
 	uint64_t time_us; // timestamp in microseconds of the integration period mid-point
 };
 
+struct extVisionSample {
+	Vector3f posNED;  // measured NED position relative to the local origin (m)
+	Quaternion quat;  // measured quaternion orientation defining rotation from NED to body frame
+	float posErr;     // 1-Sigma spherical position accuracy (m)
+	float angErr;     // 1-Sigma angular error (rad)
+	uint64_t time_us; // timestamp of the measurement in microseconds
+};
+
 // Integer definitions for vdist_sensor_type
 #define VDIST_SENSOR_BARO  0	// Use baro height
 #define VDIST_SENSOR_GPS   1	// Use GPS height
@@ -140,6 +155,8 @@ struct flowSample {
 #define MASK_USE_GPS    (1<<0)  // set to true to use GPS data
 #define MASK_USE_OF     (1<<1)  // set to true to use optical flow data
 #define MASK_INHIBIT_ACC_BIAS (1<<2)  // set to true to inhibit estimation of accelerometer delta velocity bias
+#define MASK_USE_EVPOS	(1<<3)  // set to true to use external vision NED position data
+#define MASK_USE_EVYAW  (1<<4)	// set to true to use exernal vision quaternion data for yaw
 
 // Integer definitions for mag_fusion_type
 #define MAG_FUSE_TYPE_AUTO      0   // The selection of either heading or 3D magnetometer fusion will be automatic
@@ -163,6 +180,7 @@ struct parameters {
 	float airspeed_delay_ms;	// airspeed measurement delay relative to the IMU (msec)
 	float flow_delay_ms;		// optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval
 	float range_delay_ms;		// range finder measurement delay relative to the IMU (msec)
+	float ev_delay_ms;		// off-board vision measurement delay relative to the IMU (msec)
 
 	// input noise
 	float gyro_noise;		// IMU angular rate noise used for covariance prediction (rad/sec)
@@ -247,6 +265,7 @@ struct parameters {
 		airspeed_delay_ms = 200.0f;
 		flow_delay_ms = 5.0f;
 		range_delay_ms = 5.0f;
+		ev_delay_ms = 100.0f;
 
 		// input noise
 		gyro_noise = 1.5e-2f;
@@ -383,6 +402,8 @@ union filter_control_status_u {
 		uint16_t baro_hgt    : 1; // 9 - true when baro height is being fused as a primary height reference
 		uint16_t rng_hgt     : 1; // 10 - true when range finder height is being fused as a primary height reference
 		uint16_t gps_hgt     : 1; // 11 - true when range finder height is being fused as a primary height reference
+		uint16_t ev_pos      : 1; // 12 - true when local position data from external vision is being fused
+		uint16_t ev_yaw      : 1; // 13 - true when yaw data from external vision measurements is being fused
 	} flags;
 	uint16_t value;
 };

EKF/estimator_interface.cpp

@@ -65,6 +65,7 @@ EstimatorInterface::EstimatorInterface():
 	_time_last_baro(0),
 	_time_last_range(0),
 	_time_last_airspeed(0),
+	_time_last_ext_vision(0),
 	_mag_declination_gps(0.0f),
 	_mag_declination_to_save_deg(0.0f)
 {
@@ -284,6 +285,30 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 	}
 }
 
+// set attitude and position data derived from an external vision system
+void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message *evdata)
+{
+	if (!_initialised) {
+		return;
+	}
+
+	// if data passes checks, push to buffer
+	if (time_usec > _time_last_ext_vision) {
+		extVisionSample ev_sample_new;
+		// calculate the system time-stamp for the mid point of the integration period
+		ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000;
+		// copy required data
+		ev_sample_new.angErr = evdata->angErr;
+		ev_sample_new.posErr = evdata->posErr;
+		ev_sample_new.quat = evdata->quat;
+		ev_sample_new.posNED = evdata->posNED;
+		// record time for comparison next measurement
+		_time_last_ext_vision = time_usec;
+		// push to buffer
+		_ext_vision_buffer.push(ev_sample_new);
+	}
+}
+
 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
 
@@ -294,6 +319,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 	      _range_buffer.allocate(OBS_BUFFER_LENGTH) &&
 	      _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) &&
 	      _flow_buffer.allocate(OBS_BUFFER_LENGTH) &&
+	      _ext_vision_buffer.allocate(OBS_BUFFER_LENGTH) &&
 	      _output_buffer.allocate(IMU_BUFFER_LENGTH))) {
 		printf("EKF buffer allocation failed!");
 		unallocate_buffers();
@@ -314,6 +340,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 		_airspeed_buffer.push(airspeed_sample_init);
 		flowSample flow_sample_init = {};
 		_flow_buffer.push(flow_sample_init);
+		extVisionSample ext_vision_sample_init = {};
+		_ext_vision_buffer.push(ext_vision_sample_init);
 	}
 
 	// zero the data in the imu data and output observer state buffers
@@ -357,6 +385,7 @@ void EstimatorInterface::unallocate_buffers()
 	_range_buffer.unallocate();
 	_airspeed_buffer.unallocate();
 	_flow_buffer.unallocate();
+	_ext_vision_buffer.unallocate();
 	_output_buffer.unallocate();
 
 }

EKF/estimator_interface.h

@@ -145,6 +145,9 @@ public:
 	// set optical flow data
 	void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
 
+	// set external vision position and attitude data
+	void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata);
+
 	// return a address to the parameters struct
 	// in order to give access to the application
 	parameters *getParamHandle() {return &_params;}
@@ -277,6 +280,7 @@ protected:
 	RingBuffer<rangeSample> _range_buffer;
 	RingBuffer<airspeedSample> _airspeed_buffer;
 	RingBuffer<flowSample> 	_flow_buffer;
+	RingBuffer<extVisionSample> _ext_vision_buffer;
 	RingBuffer<outputSample> _output_buffer;
 
 	uint64_t _time_last_imu;	// timestamp of last imu sample in microseconds
@@ -285,6 +289,7 @@ protected:
 	uint64_t _time_last_baro;	// timestamp of last barometer measurement in microseconds
 	uint64_t _time_last_range;	// timestamp of last range measurement in microseconds
 	uint64_t _time_last_airspeed;	// timestamp of last airspeed measurement in microseconds
+	uint64_t _time_last_ext_vision; // timestamp of last external vision measurement in microseconds
 	uint64_t _time_last_optflow;
 
 	fault_status_u _fault_status;