Merge pull request #3748 from jgoppert/lpe-improvements

improved position based flow correction for lpe

cmake/configs/nuttx_px4fmu-v2_lpe.cmake

@@ -1,5 +1,9 @@
 include(cmake/configs/nuttx_px4fmu-v2_default.cmake)
 
+list(REMOVE_ITEM config_module_list
+	modules/ekf_att_pos_estimator
+	)
+
 list(APPEND config_module_list
 	modules/local_position_estimator
 	)

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -34,7 +34,7 @@ using namespace Eigen;
 #include <uORB/Publication.hpp>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/filtered_bottom_flow.h>
+//#include <uORB/topics/filtered_bottom_flow.h>
 #include <uORB/topics/estimator_status.h>
 
 #define CBRK_NO_VISION_KEY	328754
@@ -108,6 +108,19 @@ class BlockLocalPositionEstimator : public control::SuperBlock
 // 	mocap: px, py, pz
 //
 public:
+
+	// constants
+	enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz, X_tz, n_x};
+	enum {U_ax = 0, U_ay, U_az, n_u};
+	enum {Y_baro_z = 0, n_y_baro};
+	enum {Y_lidar_z = 0, n_y_lidar};
+	enum {Y_flow_x = 0, Y_flow_y, n_y_flow};
+	enum {Y_sonar_z = 0, n_y_sonar};
+	enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz, n_y_gps};
+	enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, n_y_vision};
+	enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z, n_y_mocap};
+	enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM, n_poll};
+
 	BlockLocalPositionEstimator();
 	void update();
 	virtual ~BlockLocalPositionEstimator();
@@ -117,27 +130,6 @@ private:
 	BlockLocalPositionEstimator(const BlockLocalPositionEstimator &);
 	BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &);
 
-	// constants
-	static const uint8_t n_x = 9;
-	static const uint8_t n_u = 3; // 3 accelerations
-	static const uint8_t n_y_flow = 2;
-	static const uint8_t n_y_sonar = 1;
-	static const uint8_t n_y_baro = 1;
-	static const uint8_t n_y_lidar = 1;
-	static const uint8_t n_y_gps = 6;
-	static const uint8_t n_y_vision = 3;
-	static const uint8_t n_y_mocap = 3;
-	enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz};
-	enum {U_ax = 0, U_ay, U_az};
-	enum {Y_baro_z = 0};
-	enum {Y_lidar_z = 0};
-	enum {Y_flow_x = 0, Y_flow_y};
-	enum {Y_sonar_z = 0};
-	enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz};
-	enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz};
-	enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z};
-	enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM};
-
 	// methods
 	// ----------------------------
 	void initP();
@@ -152,7 +144,10 @@ private:
 	void correctFlow();
 	void correctSonar();
 	void correctVision();
-	void correctmocap();
+	void correctMocap();
+
+	// sensor timeout checks
+	void checkTimeouts();
 
 	// sensor initialization
 	void updateHome();
@@ -162,12 +157,12 @@ private:
 	void initSonar();
 	void initFlow();
 	void initVision();
-	void initmocap();
+	void initMocap();
 
 	// publications
 	void publishLocalPos();
 	void publishGlobalPos();
-	void publishFilteredFlow();
+	//void publishFilteredFlow();
 	void publishEstimatorStatus();
 
 	// attributes
@@ -181,60 +176,90 @@ private:
 	uORB::Subscription<vehicle_attitude_setpoint_s> _sub_att_sp;
 	uORB::Subscription<optical_flow_s> _sub_flow;
 	uORB::Subscription<sensor_combined_s> _sub_sensor;
-	uORB::Subscription<distance_sensor_s> _sub_distance;
 	uORB::Subscription<parameter_update_s> _sub_param_update;
 	uORB::Subscription<manual_control_setpoint_s> _sub_manual;
 	uORB::Subscription<home_position_s> _sub_home;
 	uORB::Subscription<vehicle_gps_position_s> _sub_gps;
 	uORB::Subscription<vision_position_estimate_s> _sub_vision_pos;
 	uORB::Subscription<att_pos_mocap_s> _sub_mocap;
+	uORB::Subscription<distance_sensor_s> *_distance_subs[ORB_MULTI_MAX_INSTANCES];
+	uORB::Subscription<distance_sensor_s> *_sub_lidar;
+	uORB::Subscription<distance_sensor_s> *_sub_sonar;
 
 	// publications
 	uORB::Publication<vehicle_local_position_s> _pub_lpos;
 	uORB::Publication<vehicle_global_position_s> _pub_gpos;
-	uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow;
+	//uORB::Publication<filtered_bottom_flow_s> _pub_filtered_flow;
 	uORB::Publication<estimator_status_s> _pub_est_status;
 
 	// map projection
 	struct map_projection_reference_s _map_ref;
 
-	// parameters
+	// general parameters
 	BlockParamInt  _integrate;
 
-	BlockParamFloat  _flow_xy_stddev;
+	// sonar parameters
 	BlockParamFloat  _sonar_z_stddev;
+	BlockParamFloat  _sonar_z_offset;
 
+	// lidar parameters
 	BlockParamFloat  _lidar_z_stddev;
+	BlockParamFloat  _lidar_z_offset;
 
+	// accel parameters
 	BlockParamFloat  _accel_xy_stddev;
 	BlockParamFloat  _accel_z_stddev;
 
+	// baro parameters
 	BlockParamFloat  _baro_stddev;
 
+	// gps parameters
 	BlockParamFloat  _gps_xy_stddev;
 	BlockParamFloat  _gps_z_stddev;
-
 	BlockParamFloat  _gps_vxy_stddev;
 	BlockParamFloat  _gps_vz_stddev;
-
 	BlockParamFloat  _gps_eph_max;
 
+	// vision parameters
 	BlockParamFloat  _vision_xy_stddev;
 	BlockParamFloat  _vision_z_stddev;
 	BlockParamInt    _no_vision;
-	BlockParamFloat  _beta_max;
 
+	// mocap parameters
 	BlockParamFloat  _mocap_p_stddev;
 
+	// flow parameters
+	BlockParamFloat  _flow_z_offset;
+	BlockParamFloat  _flow_xy_stddev;
+	//BlockParamFloat  _flow_board_x_offs;
+	//BlockParamFloat  _flow_board_y_offs;
+	BlockParamInt    _flow_min_q;
+
 	// process noise
 	BlockParamFloat  _pn_p_noise_power;
 	BlockParamFloat  _pn_v_noise_power;
 	BlockParamFloat  _pn_b_noise_power;
+	BlockParamFloat  _pn_t_noise_power;
+
+	// flow gyro filter
+	BlockHighPass _flow_gyro_x_high_pass;
+	BlockHighPass _flow_gyro_y_high_pass;
+
+	// stats
+	BlockStats<float, 1> _baroStats;
+	BlockStats<float, 1> _sonarStats;
+	BlockStats<float, 1> _lidarStats;
+	BlockStats<float, 1> _flowQStats;
+	BlockStats<float, 3> _visionStats;
+	BlockStats<float, 3> _mocapStats;
+	BlockStats<double, 3> _gpsStats;
 
 	// misc
 	struct pollfd _polls[3];
 	uint64_t _timeStamp;
 	uint64_t _time_last_xy;
+	uint64_t _time_last_z;
+	uint64_t _time_last_tz;
 	uint64_t _time_last_flow;
 	uint64_t _time_last_baro;
 	uint64_t _time_last_gps;
@@ -253,23 +278,11 @@ private:
 	bool _visionInitialized;
 	bool _mocapInitialized;
 
-	// init counts
-	int _baroInitCount;
-	int _gpsInitCount;
-	int _lidarInitCount;
-	int _sonarInitCount;
-	int _flowInitCount;
-	int _visionInitCount;
-	int _mocapInitCount;
-
 	// reference altitudes
 	float _altHome;
 	bool _altHomeInitialized;
 	float _baroAltHome;
 	float _gpsAltHome;
-	float _lidarAltHome;
-	float _sonarAltHome;
-	float _flowAltHome;
 	Vector3f _visionHome;
 	Vector3f _mocapHome;
 
@@ -285,7 +298,10 @@ private:
 	// status
 	bool _canEstimateXY;
 	bool _canEstimateZ;
+	bool _canEstimateT;
 	bool _xyTimeout;
+	bool _zTimeout;
+	bool _tzTimeout;
 
 	// sensor faults
 	fault_t _baroFault;
@@ -296,9 +312,7 @@ private:
 	fault_t _visionFault;
 	fault_t _mocapFault;
 
-	bool _visionTimeout;
+	// performance counters
-	bool _mocapTimeout;
-
 	perf_counter_t _loop_perf;
 	perf_counter_t _interval_perf;
 	perf_counter_t _err_perf;

src/modules/local_position_estimator/params.c

@@ -7,6 +7,9 @@
  * Enable local position estimator.
  *
  * @group Local Position Estimator
+ * @min 0
+ * @max 1
+ * @decimal 0
  */
 PARAM_DEFINE_INT32(LPE_ENABLED, 1);
 
@@ -14,9 +17,23 @@ PARAM_DEFINE_INT32(LPE_ENABLED, 1);
  * Enable accelerometer integration for prediction.
  *
  * @group Local Position Estimator
+ * @min 0
+ * @max 1
+ * @decimal 0
  */
 PARAM_DEFINE_INT32(LPE_INTEGRATE, 1);
 
+/**
+ * Optical flow z offset from center
+ *
+ * @group Local Position Estimator
+ * @unit m
+ * @min -1
+ * @max 1
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_FLW_OFF_Z, 0.0f);
+
 /**
  * Optical flow xy standard deviation.
  *
@@ -24,9 +41,20 @@ PARAM_DEFINE_INT32(LPE_INTEGRATE, 1);
  * @unit m
  * @min 0.01
  * @max 1
+ * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
 
+/**
+ * Optical flow minimum quality threshold
+ *
+ * @group Local Position Estimator
+ * @min 0
+ * @max 255
+ * @decimal 0
+ */
+PARAM_DEFINE_INT32(LPE_FLW_QMIN, 75);
+
 /**
  * Sonar z standard deviation.
  *
@@ -34,8 +62,20 @@ PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f);
  * @unit m
  * @min 0.01
  * @max 1
+ * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f);
+PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.05f);
+
+/**
+ * Sonar z offset from center of vehicle +down
+ *
+ * @group Local Position Estimator
+ * @unit m
+ * @min -1
+ * @max 1
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_SNR_OFF_Z, 0.00f);
 
 /**
  * Lidar z standard deviation.
@@ -44,9 +84,21 @@ PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f);
  * @unit m
  * @min 0.01
  * @max 1
+ * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
 
+/**
+ * Lidar z offset from center of vehicle +down
+ *
+ * @group Local Position Estimator
+ * @unit m
+ * @min -1
+ * @max 1
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);
+
 /**
  * Accelerometer xy standard deviation
  *
@@ -60,6 +112,7 @@ PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
  * @unit m/s^2
  * @min 0.00001
  * @max 2
+ * @decimal 4
  */
 PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
 
@@ -72,6 +125,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
  * @unit m/s^2
  * @min 0.00001
  * @max 2
+ * @decimal 4
  */
 PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
 
@@ -82,6 +136,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
  * @unit m
  * @min 0.01
  * @max 3
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
 
@@ -92,6 +147,7 @@ PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
  * @unit m
  * @min 0.01
  * @max 5
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
 
@@ -102,6 +158,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
  * @unit m
  * @min 0.01
  * @max 20
+ * @decimal 2
  */
 PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f);
 
@@ -112,6 +169,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f);
  * @unit m/s
  * @min 0.01
  * @max 2
+ * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f);
 
@@ -122,6 +180,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f);
  * @unit m/s
  * @min 0.01
  * @max 2
+ * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f);
 
@@ -132,6 +191,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f);
  * @unit m
  * @min 1.0
  * @max 5.0
+ * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
 
@@ -144,6 +204,7 @@ PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
  * @unit m
  * @min 0.01
  * @max 1
+ * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
 
@@ -154,6 +215,7 @@ PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
  * @unit m
  * @min 0.01
  * @max 2
+ * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f);
 
@@ -165,6 +227,7 @@ PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f);
  * @group Local Position Estimator
  * @min 0
  * @max 1
+ * @decimal 0
  */
 PARAM_DEFINE_INT32(LPE_NO_VISION, 0);
 
@@ -175,6 +238,7 @@ PARAM_DEFINE_INT32(LPE_NO_VISION, 0);
  * @unit m
  * @min 0.01
  * @max 1
+ * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
 
@@ -185,6 +249,7 @@ PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
  * @unit (m/s^2)-s
  * @min 0
  * @max 1
+ * @decimal 8
  */
 PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
 
@@ -195,6 +260,7 @@ PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
  * @unit (m/s)-s
  * @min 0
  * @max 5
+ * @decimal 8
  */
 PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
 
@@ -205,18 +271,28 @@ PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
  * @unit (m/s)-s
  * @min 0
  * @max 1
+ * @decimal 8
  */
 PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f);
 
 /**
- * Fault detection threshold, for chi-squared dist.
+ * Terrain random walk noise power (variance*sampling rate).
  *
- * TODO add separate params for 1 dof, 3 dof, and 6 dof beta
+ * @group Local Position Estimator
- * or false alarm rate in false alarms/hr
+ * @unit m-s
+ * @min 0
+ * @max 1
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-3f);
+
+/**
+ * Flow gyro high pass filter cut off frequency
  *
  * @group Local Position Estimator
- * @unit
+ * @unit Hz
- * @min 3
+ * @min 0
- * @max 1000
+ * @max 2
+ * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_BETA_MAX, 1000.0f);
+PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);