EKF: Add required declarations for optical flow

EKF/common.h

@@ -64,6 +64,13 @@ typedef matrix::Vector<float, 3> Vector3f;
 typedef matrix::Quaternion<float> Quaternion;
 typedef matrix::Matrix<float, 3, 3> Matrix3f;
 
+struct flow_message {
+	uint8_t quality;			// Quality of Flow data
+	Vector2f flowdata;			// Flow data received
+	Vector2f gyrodata;			// Gyro data from flow sensor
+	uint32_t dt;				// integration time of flow samples
+};
+
 struct outputSample {
 	Quaternion  quat_nominal;	// nominal quaternion describing vehicle attitude
 	Vector3f    vel;	// NED velocity estimate in earth frame in m/s
@@ -107,44 +114,91 @@ struct airspeedSample {
 };
 
 struct flowSample {
-	Vector2f    flowRadXY;
-	Vector2f    flowRadXYcomp;
-	uint64_t    time_us;
+	uint8_t  quality; // quality indicator between 0 and 255
+	Vector2f flowRadXY; // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
+	Vector2f flowRadXYcomp;	// measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
+	Vector2f gyroXY; // measured delta angle of the inertial frame about the X and Y body axes obtained from rate gyro measurements (rad), RH rotation is positive
+	float    dt; // amount of integration time (sec)
+	uint64_t time_us; // timestamp in microseconds of the integration period mid-point
 };
 
+enum vdist_sensor_type_t {
+	VDIST_SENSOR_RANGE,
+	VDIST_SENSOR_BARO,
+	VDIST_SENSOR_GPS,
+	VDIST_SENSOR_NONE
+};
+
+// Bit locations for mag_declination_source
+#define MASK_USE_GEO_DECL   (1<<0)  // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
+#define MASK_SAVE_GEO_DECL  (1<<1)  // set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
+#define MASK_FUSE_DECL      (1<<2)  // set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
+
+// Bit locations for fusion_mode
+#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
+
+// Integer definitions for mag_fusion_type
+#define MAG_FUSE_TYPE_AUTO      0   // The selection of either heading or 3D magnetometer fusion will be automatic
+#define MAG_FUSE_TYPE_HEADING   1   // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
+#define MAG_FUSE_TYPE_3D        2   // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
+#define MAG_FUSE_TYPE_2D        3   // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
+
 struct parameters {
-	float mag_delay_ms;         // magnetometer measurement delay relative to the IMU
-	float baro_delay_ms;        // barometer height measurement delay relative to the IMU
-	float gps_delay_ms;         // GPS measurement delay relative to the IMU
-	float airspeed_delay_ms;    // airspeed measurement delay relative to the IMU
+	// measurement source control
+	int fusion_mode;
+	int vdist_sensor_type;
+
+	// measurement time delays
+	float mag_delay_ms;		// magnetometer measurement delay relative to the IMU (msec)
+	float baro_delay_ms;		// barometer height measurement delay relative to the IMU (msec)
+	float gps_delay_ms;		// GPS measurement delay relative to the IMU (msec)
+	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)
 
 	// input noise
-	float gyro_noise;           // IMU angular rate noise used for covariance prediction
-	float accel_noise;          // IMU acceleration noise use for covariance prediction
+	float gyro_noise;		// IMU angular rate noise used for covariance prediction (rad/sec)
+	float accel_noise;		// IMU acceleration noise use for covariance prediction (m/sec/sec)
 
 	// process noise
-	float gyro_bias_p_noise;    // process noise for IMU delta angle bias prediction
-	float accel_bias_p_noise;   // process noise for IMU delta velocity bias prediction
-	float gyro_scale_p_noise;   // process noise for gyro scale factor prediction
-	float mag_p_noise;          // process noise for magnetic field prediction
-	float wind_vel_p_noise;     // process noise for wind velocity prediction
-
-	float gps_vel_noise;        // observation noise for gps velocity fusion
-	float gps_pos_noise;        // observation noise for gps position fusion
-	float pos_noaid_noise;      // observation noise for non-aiding position fusion
-	float baro_noise;           // observation noise for barometric height fusion
-	float baro_innov_gate;      // barometric height innovation consistency gate size in standard deviations
-	float posNE_innov_gate;     // GPS horizontal position innovation consistency gate size in standard deviations
-	float vel_innov_gate;       // GPS velocity innovation consistency gate size in standard deviations
-
-	float mag_heading_noise;    // measurement noise used for simple heading fusion
-	float mag_noise;            // measurement noise used for 3-axis magnetoemeter fusion
-	float mag_declination_deg;  // magnetic declination in degrees
-	float heading_innov_gate;   // heading fusion innovation consistency gate size in standard deviations
-	float mag_innov_gate;       // magnetometer fusion innovation consistency gate size in standard deviations
+	float gyro_bias_p_noise;	// process noise for IMU delta angle bias prediction (rad/sec)
+	float accel_bias_p_noise;	// process noise for IMU delta velocity bias prediction (m/sec/sec)
+	float gyro_scale_p_noise;	// process noise for gyro scale factor prediction (N/A)
+	float mag_p_noise;		// process noise for magnetic field prediction (Guass/sec)
+	float wind_vel_p_noise;		// process noise for wind velocity prediction (m/sec/sec)
+	float terrain_p_noise;		// process noise for terrain offset (m/sec)
+
+	// position and velocity fusion
+	float gps_vel_noise;		// observation noise for gps velocity fusion (m/sec)
+	float gps_pos_noise;		// observation noise for gps position fusion (m)
+	float pos_noaid_noise;		// observation noise for non-aiding position fusion (m)
+	float baro_noise;		// observation noise for barometric height fusion (m)
+	float baro_innov_gate;		// barometric height innovation consistency gate size (STD)
+	float posNE_innov_gate;		// GPS horizontal position innovation consistency gate size (STD)
+	float vel_innov_gate;		// GPS velocity innovation consistency gate size (STD)
+
+	// magnetometer fusion
+	float mag_heading_noise;	// measurement noise used for simple heading fusion (rad)
+	float mag_noise;		// measurement noise used for 3-axis magnetoemeter fusion (Gauss)
+	float mag_declination_deg;	// magnetic declination (degrees)
+	float heading_innov_gate;	// heading fusion innovation consistency gate size (STD)
+	float mag_innov_gate;		// magnetometer fusion innovation consistency gate size (STD)
 	int mag_declination_source;	// bitmask used to control the handling of declination data
 	int mag_fusion_type;		// integer used to specify the type of magnetometer fusion used
 
+	// range finder fusion
+	float range_noise;		// observation noise for range finder measurements (m)
+	float range_innov_gate;		// range finder fusion innovation consistency gate size (STD)
+	float rng_gnd_clearance;	// minimum valid value for range when on ground (m)
+
+	// optical flow fusion
+	float flow_noise;		// observation noise for optical flow LOS rate measurements (rad/sec)
+	float flow_noise_qual_min;	// observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
+	int flow_qual_min;		// minimum acceptable quality integer from  the flow sensor
+	float flow_innov_gate;		// optical flow fusion innovation consistency gate size (STD)
+	float flow_rate_max;		// maximum valid optical flow rate (rad/sec)
+
 	// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
 	// good enough to set a local origin and commence aiding
 	int gps_check_mask;     // bitmask used to control which GPS quality checks are used
@@ -159,10 +213,17 @@ struct parameters {
 	// Initialize parameter values.  Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
 	parameters()
 	{
+		// measurement source control
+		fusion_mode = MASK_USE_OF;
+		vdist_sensor_type = VDIST_SENSOR_BARO;
+
+		// measurement time delays
 		mag_delay_ms = 0.0f;
 		baro_delay_ms = 0.0f;
 		gps_delay_ms = 200.0f;
 		airspeed_delay_ms = 200.0f;
+		flow_delay_ms = 60.0f;
+		range_delay_ms = 200.0f;
 
 		// input noise
 		gyro_noise = 1.0e-3f;
@@ -174,7 +235,9 @@ struct parameters {
 		gyro_scale_p_noise = 3.0e-3f;
 		mag_p_noise = 2.5e-2f;
 		wind_vel_p_noise = 1.0e-1f;
+		terrain_p_noise = 0.5f;
 
+		// position and velocity fusion
 		gps_vel_noise = 5.0e-1f;
 		gps_pos_noise = 1.0f;
 		pos_noaid_noise = 10.0f;
@@ -183,15 +246,28 @@ struct parameters {
 		posNE_innov_gate = 3.0f;
 		vel_innov_gate = 3.0f;
 
-		mag_heading_noise = 5.0e-1f;
+		// magnetometer fusion
+		mag_heading_noise = 1.7e-1f;
 		mag_noise = 5.0e-2f;
 		mag_declination_deg = 0.0f;
 		heading_innov_gate = 3.0f;
 		mag_innov_gate = 3.0f;
-
-		mag_declination_source = 7;
+		mag_declination_source = 3;
 		mag_fusion_type = 0;
 
+		// range finder fusion
+		range_noise = 0.1f;
+		range_innov_gate = 5.0f;
+		rng_gnd_clearance = 0.1f;
+
+		// optical flow fusion
+		flow_noise = 0.15f;
+		flow_noise_qual_min = 0.5f;
+		flow_qual_min = 1;
+		flow_innov_gate = 3.0f;
+		flow_rate_max = 2.5f;
+
+		// GPS quality checks
 		gps_check_mask = 21;
 		req_hacc = 5.0f;
 		req_vacc = 8.0f;
@@ -203,17 +279,6 @@ struct parameters {
 	}
 };
 
-// Bit locations for mag_declination_source
-#define MASK_USE_GEO_DECL   (1<<0)  // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
-#define MASK_SAVE_GEO_DECL  (1<<1)  // set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
-#define MASK_FUSE_DECL      (1<<2)  // set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
-
-// Integer definitions for mag_fusion_type
-#define MAG_FUSE_TYPE_AUTO      0   // The selection of either heading or 3D magnetometer fusion will be automatic
-#define MAG_FUSE_TYPE_HEADING   1   // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
-#define MAG_FUSE_TYPE_3D        2   // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
-#define MAG_FUSE_TYPE_2D        3   // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
-
 struct stateSample {
 	Vector3f    ang_error;	// attitude axis angle error (error state formulation)
 	Vector3f    vel;	// NED velocity in earth frame in m/s
@@ -259,17 +324,20 @@ union gps_check_fail_status_u {
 // bitmask containing filter control status
 union filter_control_status_u {
 	struct {
-		uint8_t tilt_align  : 1; // 0 - true if the filter tilt alignment is complete
-		uint8_t yaw_align   : 1; // 1 - true if the filter yaw alignment is complete
-		uint8_t gps         : 1; // 2 - true if GPS measurements are being fused
-		uint8_t opt_flow    : 1; // 3 - true if optical flow measurements are being fused
-		uint8_t mag_hdg     : 1; // 4 - true if a simple magnetic yaw heading is being fused
-		uint8_t mag_2D      : 1; // 5 - true if the horizontal projection of magnetometer data is being fused
-		uint8_t mag_3D      : 1; // 6 - true if 3-axis magnetometer measurement are being fused
-		uint8_t mag_dec     : 1; // 7 - true if synthetic magnetic declination measurements are being fused
-		uint8_t in_air      : 1; // 8 - true when the vehicle is airborne
-		uint8_t armed       : 1; // 9 - true when the vehicle motors are armed
-		uint8_t wind        : 1; // 10 - true when wind velocity is being estimated
+		uint16_t tilt_align  : 1; // 0 - true if the filter tilt alignment is complete
+		uint16_t yaw_align   : 1; // 1 - true if the filter yaw alignment is complete
+		uint16_t gps         : 1; // 2 - true if GPS measurements are being fused
+		uint16_t opt_flow    : 1; // 3 - true if optical flow measurements are being fused
+		uint16_t mag_hdg     : 1; // 4 - true if a simple magnetic yaw heading is being fused
+		uint16_t mag_2D      : 1; // 5 - true if the horizontal projection of magnetometer data is being fused
+		uint16_t mag_3D      : 1; // 6 - true if 3-axis magnetometer measurement are being fused
+		uint16_t mag_dec     : 1; // 7 - true if synthetic magnetic declination measurements are being fused
+		uint16_t in_air      : 1; // 8 - true when the vehicle is airborne
+		uint16_t armed       : 1; // 9 - true when the vehicle motors are armed
+		uint16_t wind        : 1; // 10 - true when wind velocity is being estimated
+		uint16_t baro_hgt    : 1; // 11 - true when baro height is being fused as a primary height reference
+		uint16_t rng_hgt     : 1; // 12 - true when range finder height is being fused as a primary height reference
+		uint16_t gps_hgt     : 1; // 15 - true when range finder height is being fused as a primary height reference
 	} flags;
 	uint16_t value;
 };

EKF/ekf.h

@@ -76,6 +76,18 @@ public:
 	// gets the innovation variance of the heading measurement
 	void get_heading_innov_var(float *heading_innov_var);
 
+	// gets the innovation variance of the flow measurement
+	void get_flow_innov_var(float flow_innov_var[2]);
+
+	// gets the innovation of the flow measurement
+	void get_flow_innov(float flow_innov[2]);
+
+	// gets the innovation variance of the HAGL measurement
+	void get_hagl_innov_var(float *hagl_innov_var);
+
+	// gets the innovation of the HAGL measurement
+	void get_hagl_innov(float *hagl_innov);
+
 	// get the state vector at the delayed time horizon
 	void get_state_delayed(float *state);
 
@@ -98,6 +110,17 @@ public:
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning);
 
+	void get_vel_var(Vector3f &vel_var);
+
+	void get_pos_var(Vector3f &pos_var);
+
+	// return true if the global position estimate is valid
+	bool global_position_is_valid();
+
+	// return true if the etimate is valid
+	// return the estimated terrain vertical position relative to the NED origin
+	bool get_terrain_vert_pos(float *ret);
+
 private:
 
 	static const uint8_t _k_num_states = 24;
@@ -105,13 +128,15 @@ private:
 
 	stateSample _state;		// state struct of the ekf running at the delayed time horizon
 
-	bool _filter_initialised;
-	bool _earth_rate_initialised;
+	bool _filter_initialised;	// true when the EKF sttes and covariances been initialised
+	bool _earth_rate_initialised;	// true when we know the earth rotatin rate (requires GPS)
 
 	bool _fuse_height;		// baro height data should be fused
 	bool _fuse_pos;			// gps position data should be fused
 	bool _fuse_hor_vel;		// gps horizontal velocity measurement should be fused
 	bool _fuse_vert_vel;		// gps vertical velocity measurement should be fused
+	bool _fuse_flow;		// flow measurement should be fused
+	bool _fuse_hagl_data;		// if true then range data will be fused to estimate terrain height
 
 	uint64_t _time_last_fake_gps;	// last time in us at which we have faked gps measurement for static mode
 
@@ -131,13 +156,24 @@ private:
 	float KHP[_k_num_states][_k_num_states]; // intermediate variable for the covariance update
 
 	float _vel_pos_innov[6];	// innovations: 0-2 vel,  3-5 pos
-	float _mag_innov[3];		// earth magnetic field innovations
-	float _heading_innov;		// heading measurement innovation
-
 	float _vel_pos_innov_var[6];	// innovation variances: 0-2 vel, 3-5 pos
+
+	float _mag_innov[3];		// earth magnetic field innovations
 	float _mag_innov_var[3];	// earth magnetic field innovation variance
+
+	float _heading_innov;		// heading measurement innovation
 	float _heading_innov_var;	// heading measurement innovation variance
 
+	Vector3f _tilt_err_vec;         // Vector of the most recent attitude error correction from velocity and position fusion
+	float _tilt_err_length_filt;    // filtered length of _tilt_err_vec
+
+	// optical flow processing
+	float _flow_innov[2];		// flow measurement innovation
+	float _flow_innov_var[2];	// flow innovation variance
+	Vector2f _flow_gyro_bias;	// bias errors in optical flow sensor rate gyro outputs
+	Vector2f _imu_del_ang_of;	// bias corrected XY delta angle measurements accumulated across the same time frame as the optical flow rates
+	float _delta_time_of;		// time in sec that _imu_del_ang_of was accumulated over
+
 	float _mag_declination;		// magnetic declination used by reset and fusion functions (rad)
 
 	// complementary filter states
@@ -161,15 +197,23 @@ private:
 	float _gps_alt_ref;		// WGS-84 height (m)
 
 	// Variables used to initialise the filter states
-	uint8_t _baro_counter;         // number of baro samples averaged
-	float _baro_sum;               // summed baro measurement
+	uint8_t _hgt_counter;		// number of baro samples averaged
+	float _hgt_sum;			// summed baro measurement
 	uint8_t _mag_counter;		// 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
+	float _hgt_at_alignment;	// baro offset relative to alignment position
 
 	gps_check_fail_status_u _gps_check_fail_status;
 
+	// Terrain height state estimation
+	float _terrain_vpos;		// estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
+	float _terrain_var;		// variance of terrain position estimate (m^2)
+	float _hagl_innov;		// innovation of the last height above terrain measurement (m)
+	float _hagl_innov_var;		// innovation variance for the last height above terrain measurement (m^2)
+	uint64_t _time_last_hagl_fuse;	// last system time in usec that the hagl measurement failed it's checks
+	bool _terrain_initialised;	// true when the terrain estimator has been intialised
+
 	// update the real time complementary filter states. This includes the prediction
 	// and the correction step
 	void calculateOutputStates();
@@ -201,15 +245,28 @@ private:
 	// fuse airspeed measurement
 	void fuseAirspeed();
 
-	// fuse range measurements
-	void fuseRange();
-
 	// fuse velocity and position measurements (also barometer height)
 	void fuseVelPosHeight();
 
 	// reset velocity states of the ekf
 	void resetVelocity();
 
+	// fuse optical flow line of sight rate measurements
+	void fuseOptFlow();
+
+	// calculate optical flow bias errors
+	void calcOptFlowBias();
+
+	// initialise the terrain vertical position estimator
+	// return true if the initialisation is successful
+	bool initHagl();
+
+	// predict the terrain vertical position state and variance
+	void predictHagl();
+
+	// update the terrain vertical position estimate using a height above ground measurement from the range finder
+	void fuseHagl();
+
 	// reset the heading and magnetic field states using the declination and magnetometer measurements
 	// return true if successful
 	bool resetMagHeading(Vector3f &mag_init);

EKF/estimator_interface.h

@@ -81,6 +81,22 @@ public:
 	virtual void get_covariances(float *covariances) = 0;
 
 	// get the ekf WGS-84 origin position and height and the system time it was last set
+	virtual void get_vel_var(Vector3f &vel_var) = 0;
+	virtual void get_pos_var(Vector3f &pos_var) = 0;
+
+	// gets the innovation variance of the flow measurement
+	virtual void get_flow_innov_var(float flow_innov_var[2]) = 0;
+
+	// gets the innovation of the flow measurement
+	virtual void get_flow_innov(float flow_innov[2]) = 0;
+
+	// gets the innovation variance of the HAGL measurement
+	virtual void get_hagl_innov_var(float *flow_innov_var) = 0;
+
+	// gets the innovation of the HAGL measurement
+	virtual void get_hagl_innov(float *flow_innov_var) = 0;
+
+	// get the ekf WGS-84 origin positoin and height and the system time it was last set
 	virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
@@ -99,7 +115,7 @@ public:
 
 	virtual bool collect_range(uint64_t time_usec, float *data) { return true; }
 
-	virtual bool collect_opticalflow(uint64_t time_usec, float *data) { return true; }
+	virtual bool collect_opticalflow(uint64_t time_usec, flow_message *flow) { return true; }
 
 	// set delta angle imu data
 	void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel);
@@ -121,7 +137,7 @@ public:
 	void setRangeData(uint64_t time_usec, float *data);
 
 	// set optical flow data
-	void setOpticalFlowData(uint64_t time_usec, float *data);
+	void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
 
 	// return a address to the parameters struct
 	// in order to give access to the application
@@ -133,7 +149,15 @@ public:
 	// set vehicle landed status data
 	void set_in_air_status(bool in_air) {_in_air = in_air;}
 
-	bool position_is_valid();
+	// return true if the global position estimate is valid
+	virtual bool global_position_is_valid() = 0;
+
+	// return true if the estimate is valid
+	// return the estimated terrain vertical position relative to the NED origin
+	virtual bool get_terrain_vert_pos(float *ret) = 0;
+
+	// return true if the local position estimate is valid
+	bool local_position_is_valid();
 
 
 	void copy_quaternion(float *quat)
@@ -196,13 +220,13 @@ protected:
 	bool _vehicle_armed;    // vehicle arm status used to turn off functionality used on the ground
 	bool _in_air;           // we assume vehicle is in the air, set by the given landing detector
 
-	bool _NED_origin_initialised;
-	bool _gps_speed_valid;
-	float _gps_speed_accuracy;                  // GPS receiver reported speed accuracy (m/s)
-	struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians)
-	float _gps_hpos_accuracy; // GPS receiver reported 1-sigma horizontal accuracy (m)
-	float _gps_origin_eph; // horizontal position uncertainty of the GPS origin
-	float _gps_origin_epv; // vertical position uncertainty of the GPS origin
+	bool _NED_origin_initialised = false;
+	bool _gps_speed_valid = false;
+	float _gps_speed_accuracy = 0.0f; // GPS receiver reported 1-sigma speed accuracy (m/s)
+	float _gps_hpos_accuracy = 0.0f; // GPS receiver reported 1-sigma horizontal accuracy (m)
+	float _gps_origin_eph = 0.0f; // horizontal position uncertainty of the GPS origin
+	float _gps_origin_epv = 0.0f; // vertical position uncertainty of the GPS origin
+	struct map_projection_reference_s _pos_ref = {};    // Contains WGS-84 position latitude and longitude (radians)
 
 	bool _mag_healthy;              // computed by mag innovation test
 	float _yaw_test_ratio;          // yaw innovation consistency check ratio
@@ -226,6 +250,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_optflow;
 
 	fault_status_t _fault_status;