Updated lpe.

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.
- *
- * TODO add separate params for 1 dof, 3 dof, and 6 dof beta
- * or false alarm rate in false alarms/hr
+ * Terrain random walk noise power (variance*sampling rate).
  *
  * @group Local Position Estimator
- * @unit
- * @min 3
- * @max 1000
+ * @unit m-s
+ * @min 0
+ * @max 1
+ * @decimal 3
  */
-PARAM_DEFINE_FLOAT(LPE_BETA_MAX, 1000.0f);
+PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-3f);
+
+/**
+ * Flow gyro high pass filter cut off frequency
+ *
+ * @group Local Position Estimator
+ * @unit Hz
+ * @min 0
+ * @max 2
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);