Merge pull request #4009 from jgoppert/lpe-gps-delay

Enabled gps delay compensation for lpe based on x hist.

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <px4_posix.h>
-#include <controllib/uorb/blocks.hpp>
+#include <controllib/blocks.hpp>
 #include <mathlib/mathlib.h>
 #include <systemlib/perf_counter.h>
 #include <lib/geo/geo.h>
@@ -35,14 +35,15 @@ 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/estimator_status.h>
 
 #define CBRK_NO_VISION_KEY	328754
 
 using namespace control;
 
-//static const size_t HIST_LEN = 2; // each step is 100 ms, gps has 200 ms delay
+static const float GPS_DELAY_MAX = 0.5; // seconds
+static const float HIST_STEP = 0.05; // 20 hz
+static const size_t HIST_LEN = (GPS_DELAY_MAX / HIST_STEP);
 
 enum fault_t {
 	FAULT_NONE = 0,
@@ -165,7 +166,6 @@ private:
 	// publications
 	void publishLocalPos();
 	void publishGlobalPos();
-	//void publishFilteredFlow();
 	void publishEstimatorStatus();
 
 	// attributes
@@ -192,7 +192,6 @@ private:
 	// 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<estimator_status_s> _pub_est_status;
 
 	// map projection
@@ -217,6 +216,7 @@ private:
 	BlockParamFloat  _baro_stddev;
 
 	// gps parameters
+	BlockParamFloat  _gps_delay;
 	BlockParamFloat  _gps_xy_stddev;
 	BlockParamFloat  _gps_z_stddev;
 	BlockParamFloat  _gps_vxy_stddev;
@@ -239,10 +239,10 @@ private:
 	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;
+	BlockParamFloat  _pn_p_noise_density;
+	BlockParamFloat  _pn_v_noise_density;
+	BlockParamFloat  _pn_b_noise_density;
+	BlockParamFloat  _pn_t_noise_density;
 
 	// flow gyro filter
 	BlockHighPass _flow_gyro_x_high_pass;
@@ -258,14 +258,13 @@ private:
 	BlockStats<double, 3> _gpsStats;
 
 	// delay blocks
-	//BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
-	//BlockDelay<float, n_x, n_x, HIST_LEN> _PDelay;
-	//BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
+	BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
+	BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
 
 	// misc
 	px4_pollfd_struct_t _polls[3];
 	uint64_t _timeStamp;
-	//uint64_t _time_last_hist;
+	uint64_t _time_last_hist;
 	uint64_t _time_last_xy;
 	uint64_t _time_last_z;
 	uint64_t _time_last_tz;
@@ -308,6 +307,7 @@ private:
 	bool _canEstimateXY;
 	bool _canEstimateZ;
 	bool _canEstimateT;
+	bool _canEstimateGlobal;
 	bool _xyTimeout;
 	bool _zTimeout;
 	bool _tzTimeout;

src/modules/local_position_estimator/CMakeLists.txt

@@ -31,7 +31,7 @@
 #
 ############################################################################
 if(${OS} STREQUAL "nuttx")
-	list(APPEND MODULE_CFLAGS -Wframe-larger-than=7000)
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=10000)
 elseif(${OS} STREQUAL "posix")
 	list(APPEND MODULE_CFLAGS -Wno-error)
 endif()

src/modules/local_position_estimator/fault_table.py

@@ -0,0 +1,13 @@
+#!/bin/bash
+from __future__ import print_function
+import pylab as pl
+import scipy.optimize
+from scipy.stats import chi2
+
+for fa_rate in 1.0/pl.array([1e1, 1e2, 1e4, 1e6, 1e9]):
+    print(fa_rate)
+    for df in range(1,7):
+        f_eq = lambda x: ((1- fa_rate) - chi2.cdf(x, df))**2
+        res = scipy.optimize.minimize(f_eq, df)
+        assert res['success']
+        print('\t', res.x[0])

src/modules/local_position_estimator/local_position_estimator_main.cpp

@@ -112,7 +112,7 @@ int local_position_estimator_main(int argc, char *argv[])
 		deamon_task = px4_task_spawn_cmd("lp_estimator",
 						 SCHED_DEFAULT,
 						 SCHED_PRIORITY_MAX - 5,
-						 10240,
+						 11264,
 						 local_position_estimator_thread_main,
 						 (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
 		return 0;

src/modules/local_position_estimator/params.c

@@ -105,7 +105,7 @@ PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);
  * should be 0.0464
  *
  * @group Local Position Estimator
- * @unit m/s/s
+ * @unit m/s^2
  * @min 0.00001
  * @max 2
  * @decimal 4
@@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
  * (see Accel x comments)
  *
  * @group Local Position Estimator
- * @unit m/s/s
+ * @unit m/s^2
  * @min 0.00001
  * @max 2
  * @decimal 4
@@ -136,6 +136,19 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
  */
 PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
 
+
+/**
+ * GPS delay compensaton
+ *
+ * @group Local Position Estimator
+ * @unit sec
+ * @min 0
+ * @max 0.4
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
+
+
 /**
  * GPS xy standard deviation.
  *
@@ -153,10 +166,10 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
  * @group Local Position Estimator
  * @unit m
  * @min 0.01
- * @max 20
+ * @max 200
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f);
+PARAM_DEFINE_FLOAT(LPE_GPS_Z, 100.0f);
 
 /**
  * GPS xy velocity standard deviation.
@@ -237,43 +250,43 @@ PARAM_DEFINE_INT32(LPE_NO_VISION, 0);
 PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
 
 /**
- * Position propagation process noise power (variance*sampling rate).
+ * Position propagation noise density
  *
  * @group Local Position Estimator
- * @unit (m/s/s)-s
+ * @unit m/s/sqrt(Hz)
  * @min 0
  * @max 1
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f);
+PARAM_DEFINE_FLOAT(LPE_PN_P, 0.1f);
 
 /**
- * Velocity propagation process noise power (variance*sampling rate).
+ * Velocity propagation noise density
  *
  * @group Local Position Estimator
- * @unit (m/s)-s
- * @min 0
- * @max 5
- * @decimal 8
- */
-PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
-
-/**
- * Accel bias propagation process noise power (variance*sampling rate).
- *
- * @group Local Position Estimator
- * @unit (m/s)-s
+ * @unit (m/s)/s/sqrt(Hz)
  * @min 0
  * @max 1
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f);
+PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
 
 /**
- * Terrain random walk noise power (variance*sampling rate).
+ * Accel bias propagation noise density
  *
  * @group Local Position Estimator
- * @unit m-s
+ * @unit (m/s^2)/s/sqrt(Hz)
+ * @min 0
+ * @max 1
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-3f);
+
+/**
+ * Terrain random walk noise density
+ *
+ * @group Local Position Estimator
+ * @unit m/s/sqrt(Hz)
  * @min 0
  * @max 1
  * @decimal 3