Merge pull request #4267 from jgoppert/lpe_update

Updated lpe for separate sensor source files, also made more quiet.

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -5,14 +5,7 @@
 #include <mathlib/mathlib.h>
 #include <systemlib/perf_counter.h>
 #include <lib/geo/geo.h>
-
-#ifdef USE_MATRIX_LIB
-#include "matrix/Matrix.hpp"
-using namespace matrix;
-#else
-#include <Eigen/Eigen>
-using namespace Eigen;
-#endif
+#include <matrix/Matrix.hpp>
 
 // uORB Subscriptions
 #include <uORB/Subscription.hpp>
@@ -37,13 +30,13 @@ using namespace Eigen;
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/estimator_status.h>
 
-#define CBRK_NO_VISION_KEY	328754
-
+using namespace matrix;
 using namespace control;
 
 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);
+static const size_t HIST_LEN = 10; // GPS_DELAY_MAX / HIST_STEP;
+static const size_t N_DIST_SUBS = 4;
 
 enum fault_t {
 	FAULT_NONE = 0,
@@ -61,12 +54,26 @@ enum sensor_t {
 	SENSOR_MOCAP
 };
 
+// change this to set when
+// the system will abort correcting a measurement
+// given a fault has been detected
+static const fault_t fault_lvl_disable = FAULT_SEVERE;
+
+// for fault detection
+// chi squared distribution, false alarm probability 0.0001
+// see fault_table.py
+// note skip 0 index so we can use degree of freedom as index
+static const float BETA_TABLE[7] = {0,
+				    8.82050518214,
+				    12.094592431,
+				    13.9876612368,
+				    16.0875642296,
+				    17.8797700658,
+				    19.6465647819,
+				   };
+
 class BlockLocalPositionEstimator : public control::SuperBlock
 {
-//
-// The purpose of this estimator is to provide a robust solution for
-// indoor flight.
-//
 // dynamics:
 //
 //	x(+) = A * x(-) + B * u(+)
@@ -92,8 +99,8 @@ class BlockLocalPositionEstimator : public control::SuperBlock
 // states:
 // 	px, py, pz , ( position NED)
 // 	vx, vy, vz ( vel NED),
-// 	bx, by, bz ( TODO accelerometer bias)
-// 	tz (TODO terrain altitude)
+// 	bx, by, bz ( accel bias)
+// 	tz (terrain altitude, ASL)
 //
 // measurements:
 //
@@ -141,27 +148,55 @@ private:
 	// predict the next state
 	void predict();
 
-	// correct the state prediction with a measurement
-	void correctBaro();
-	void correctGps();
-	void correctLidar();
-	void correctFlow();
-	void correctSonar();
-	void correctVision();
-	void correctMocap();
+	// lidar
+	int  lidarMeasure(Vector<float, n_y_lidar> &y);
+	void lidarCorrect();
+	void lidarInit();
+	void lidarCheckTimeout();
 
-	// sensor timeout checks
+	// sonar
+	int  sonarMeasure(Vector<float, n_y_sonar> &y);
+	void sonarCorrect();
+	void sonarInit();
+	void sonarCheckTimeout();
+
+	// baro
+	int  baroMeasure(Vector<float, n_y_baro> &y);
+	void baroCorrect();
+	void baroInit();
+	void baroCheckTimeout();
+
+	// gps
+	int  gpsMeasure(Vector<double, n_y_gps> &y);
+	void gpsCorrect();
+	void gpsInit();
+	void gpsCheckTimeout();
+
+	// flow
+	int  flowMeasure(Vector<float, n_y_flow> &y);
+	void flowCorrect();
+	void flowInit();
+	void flowCheckTimeout();
+
+	// vision
+	int  visionMeasure(Vector<float, n_y_vision> &y);
+	void visionCorrect();
+	void visionInit();
+	void visionCheckTimeout();
+
+	// mocap
+	int  mocapMeasure(Vector<float, n_y_mocap> &y);
+	void mocapCorrect();
+	void mocapInit();
+	void mocapCheckTimeout();
+
+	// timeouts
 	void checkTimeouts();
 
-	// sensor initialization
+	// misc
+	float agl();
+	void detectDistanceSensors();
 	void updateHome();
-	void initBaro();
-	void initGps();
-	void initLidar();
-	void initSonar();
-	void initFlow();
-	void initVision();
-	void initMocap();
 
 	// publications
 	void publishLocalPos();
@@ -185,7 +220,11 @@ private:
 	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_dist0;
+	uORB::Subscription<distance_sensor_s> _sub_dist1;
+	uORB::Subscription<distance_sensor_s> _sub_dist2;
+	uORB::Subscription<distance_sensor_s> _sub_dist3;
+	uORB::Subscription<distance_sensor_s> *_dist_subs[N_DIST_SUBS];
 	uORB::Subscription<distance_sensor_s> *_sub_lidar;
 	uORB::Subscription<distance_sensor_s> *_sub_sonar;
 
@@ -216,6 +255,7 @@ private:
 	BlockParamFloat  _baro_stddev;
 
 	// gps parameters
+	BlockParamInt   _gps_on;
 	BlockParamFloat  _gps_delay;
 	BlockParamFloat  _gps_xy_stddev;
 	BlockParamFloat  _gps_z_stddev;
@@ -226,7 +266,7 @@ private:
 	// vision parameters
 	BlockParamFloat  _vision_xy_stddev;
 	BlockParamFloat  _vision_z_stddev;
-	BlockParamInt    _no_vision;
+	BlockParamInt   _vision_on;
 
 	// mocap parameters
 	BlockParamFloat  _mocap_p_stddev;
@@ -244,18 +284,22 @@ private:
 	BlockParamFloat  _pn_b_noise_density;
 	BlockParamFloat  _pn_t_noise_density;
 
+	// init home
+	BlockParamFloat  _init_home_lat;
+	BlockParamFloat  _init_home_lon;
+
 	// 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, n_y_baro> _baroStats;
+	BlockStats<float, n_y_sonar> _sonarStats;
+	BlockStats<float, n_y_lidar> _lidarStats;
 	BlockStats<float, 1> _flowQStats;
-	BlockStats<float, 3> _visionStats;
-	BlockStats<float, 3> _mocapStats;
-	BlockStats<double, 3> _gpsStats;
+	BlockStats<float, n_y_vision> _visionStats;
+	BlockStats<float, n_y_mocap> _mocapStats;
+	BlockStats<double, n_y_gps> _gpsStats;
 
 	// delay blocks
 	BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
@@ -275,9 +319,9 @@ private:
 	uint64_t _time_last_sonar;
 	uint64_t _time_last_vision_p;
 	uint64_t _time_last_mocap;
-	orb_advert_t _mavlink_log_pub;
 
 	// initialization flags
+	bool _receivedGps;
 	bool _baroInitialized;
 	bool _gpsInitialized;
 	bool _lidarInitialized;
@@ -299,18 +343,14 @@ private:
 	float _flowY;
 	float _flowMeanQual;
 
-	// referene lat/lon
-	double _gpsLatHome;
-	double _gpsLonHome;
-
 	// status
 	bool _canEstimateXY;
 	bool _canEstimateZ;
 	bool _canEstimateT;
-	bool _canEstimateGlobal;
 	bool _xyTimeout;
 	bool _zTimeout;
 	bool _tzTimeout;
+	bool _lastArmedState;
 
 	// sensor faults
 	fault_t _baroFault;

src/modules/local_position_estimator/CMakeLists.txt

@@ -39,6 +39,13 @@ px4_add_module(
 	SRCS
 		local_position_estimator_main.cpp
 		BlockLocalPositionEstimator.cpp
+		sensors/flow.cpp
+		sensors/lidar.cpp
+		sensors/sonar.cpp
+		sensors/gps.cpp
+		sensors/baro.cpp
+		sensors/vision.cpp
+		sensors/mocap.cpp
 		params.c
 	DEPENDS
 		platforms__common

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,
-						 11264,
+						 12000,
 						 local_position_estimator_thread_main,
 						 (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
 		return 0;

src/modules/local_position_estimator/params.c

@@ -3,14 +3,6 @@
 // 16 is max name length
 
 
-/**
- * Enable local position estimator.
- *
- * @boolean
- * @group Local Position Estimator
- */
-PARAM_DEFINE_INT32(LPE_ENABLED, 1);
-
 /**
  * Enable accelerometer integration for prediction.
  *
@@ -136,6 +128,13 @@ PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
  */
 PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f);
 
+/**
+ * Enable GPS
+ *
+ * @group Local Position Estimator
+ * @boolean
+ */
+PARAM_DEFINE_INT32(LPE_GPS_ON, 1);
 
 /**
  * GPS delay compensaton
@@ -227,16 +226,12 @@ PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f);
 PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f);
 
 /**
- * Circuit breaker to disable vision input.
- *
- * Set to the appropriate key (328754) to disable vision input.
+ * Enabled vision correction
  *
  * @group Local Position Estimator
- * @min 0
- * @max 1
- * @decimal 0
+ * @boolean
  */
-PARAM_DEFINE_INT32(LPE_NO_VISION, 0);
+PARAM_DEFINE_INT32(LPE_VIS_ON, 1);
 
 /**
  * Vicon position standard deviation.
@@ -303,3 +298,26 @@ PARAM_DEFINE_FLOAT(LPE_PN_T, 1e-3f);
  * @decimal 3
  */
 PARAM_DEFINE_FLOAT(LPE_FGYRO_HP, 0.1f);
+
+/**
+ * Home latitude for nav w/o GPS
+ *
+ * @group Local Position Estimator
+ * @unit deg
+ * @min -90
+ * @max 90
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
+
+/**
+ * Home longitude for nav w/o GPS
+ *
+ * @group Local Position Estimator
+ * @unit deg
+ * @min -180
+ * @max 180
+ * @decimal 8
+ */
+PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
+

src/modules/local_position_estimator/sensors/baro.cpp

@@ -0,0 +1,114 @@
+#include "../BlockLocalPositionEstimator.hpp"
+#include <systemlib/mavlink_log.h>
+#include <matrix/math.hpp>
+
+extern orb_advert_t mavlink_log_pub;
+
+// required number of samples for sensor
+// to initialize
+static const uint32_t 		REQ_BARO_INIT_COUNT = 100;
+static const uint32_t 		BARO_TIMEOUT =   	100000;	// 0.1 s
+
+void BlockLocalPositionEstimator::baroInit()
+{
+	// measure
+	Vector<float, n_y_baro> y;
+
+	if (baroMeasure(y) != OK) {
+		_baroStats.reset();
+		return;
+	}
+
+	// if finished
+	if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
+		_baroAltHome = _baroStats.getMean()(0);
+		mavlink_and_console_log_info(&mavlink_log_pub,
+					     "[lpe] baro init %d m std %d cm",
+					     (int)_baroStats.getMean()(0),
+					     (int)(100 * _baroStats.getStdDev()(0)));
+		_baroInitialized = true;
+		_baroFault = FAULT_NONE;
+
+		if (!_altHomeInitialized) {
+			_altHomeInitialized = true;
+			_altHome = _baroAltHome;
+		}
+	}
+}
+
+int BlockLocalPositionEstimator::baroMeasure(Vector<float, n_y_baro> &y)
+{
+	//measure
+	y.setZero();
+	y(0) = _sub_sensor.get().baro_alt_meter[0];
+	_baroStats.update(y);
+	_time_last_baro = _timeStamp;
+	return OK;
+}
+
+void BlockLocalPositionEstimator::baroCorrect()
+{
+	// measure
+	Vector<float, n_y_baro> y;
+
+	if (baroMeasure(y) != OK) { return; }
+
+	// subtract baro home alt
+	y -= _baroAltHome;
+
+	// baro measurement matrix
+	Matrix<float, n_y_baro, n_x> C;
+	C.setZero();
+	C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir.
+
+	Matrix<float, n_y_baro, n_y_baro> R;
+	R.setZero();
+	R(0, 0) = _baro_stddev.get() * _baro_stddev.get();
+
+	// residual
+	Matrix<float, n_y_baro, n_y_baro> S_I =
+		inv<float, n_y_baro>((C * _P * C.transpose()) + R);
+	Vector<float, n_y_baro> r = y - (C * _x);
+
+	// fault detection
+	float beta = (r.transpose() * (S_I * r))(0, 0);
+
+	if (beta > BETA_TABLE[n_y_baro]) {
+		if (_baroFault < FAULT_MINOR) {
+			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
+			//double(r(0)), double(beta));
+			_baroFault = FAULT_MINOR;
+		}
+
+	} else if (_baroFault) {
+		_baroFault = FAULT_NONE;
+		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK");
+	}
+
+	// kalman filter correction if no fault
+	if (_baroFault < fault_lvl_disable) {
+		Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
+		Vector<float, n_x> dx = K * r;
+
+		if (!_canEstimateXY) {
+			dx(X_x) = 0;
+			dx(X_y) = 0;
+			dx(X_vx) = 0;
+			dx(X_vy) = 0;
+		}
+
+		_x += dx;
+		_P -= K * C * _P;
+	}
+}
+
+void BlockLocalPositionEstimator::baroCheckTimeout()
+{
+	if (_timeStamp - _time_last_baro > BARO_TIMEOUT) {
+		if (_baroInitialized) {
+			_baroInitialized = false;
+			_baroStats.reset();
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro timeout ");
+		}
+	}
+}

src/modules/local_position_estimator/sensors/flow.cpp

@@ -0,0 +1,186 @@
+#include "../BlockLocalPositionEstimator.hpp"
+#include <systemlib/mavlink_log.h>
+#include <matrix/math.hpp>
+
+// mavlink pub
+extern orb_advert_t mavlink_log_pub;
+
+// required number of samples for sensor
+// to initialize
+static const uint32_t 		REQ_FLOW_INIT_COUNT = 10;
+static const uint32_t 		FLOW_TIMEOUT =   	1000000;	// 1 s
+
+// minimum flow altitude
+static const float flow_min_agl = 0.3;
+
+void BlockLocalPositionEstimator::flowInit()
+{
+	// measure
+	Vector<float, n_y_flow> y;
+
+	if (flowMeasure(y) != OK) {
+		_flowQStats.reset();
+		return;
+	}
+
+	// if finished
+	if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) {
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow init: "
+					     "quality %d std %d",
+					     int(_flowQStats.getMean()(0)),
+					     int(_flowQStats.getStdDev()(0)));
+		_flowInitialized = true;
+		_flowFault = FAULT_NONE;
+	}
+}
+
+int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
+{
+	// check for agl
+	if (agl() < flow_min_agl) {
+		return -1;
+	}
+
+	// check quality
+	float qual = _sub_flow.get().quality;
+
+	if (qual < _flow_min_q.get()) {
+		return -1;
+	}
+
+	// calculate range to center of image for flow
+	float d = 0;
+
+	if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) {
+		d = _sub_lidar->get().current_distance
+		    + (_lidar_z_offset.get() - _flow_z_offset.get());
+
+	} else if (_sonarInitialized && (_sonarFault < fault_lvl_disable)) {
+		d = _sub_sonar->get().current_distance
+		    + (_sonar_z_offset.get() - _flow_z_offset.get());
+
+	} else {
+		// no valid distance data
+		return -1;
+	}
+
+	// check for global accuracy
+	if (_gpsInitialized) {
+		double  lat = _sub_gps.get().lat * 1.0e-7;
+		double  lon = _sub_gps.get().lon * 1.0e-7;
+		float px = 0;
+		float py = 0;
+		map_projection_project(&_map_ref, lat, lon, &px, &py);
+		Vector2f delta(px - _flowX, py - _flowY);
+
+		if (delta.norm() > 3) {
+			mavlink_and_console_log_info(&mavlink_log_pub,
+						     "[lpe] flow too far from GPS, disabled");
+			_flowInitialized = false;
+			return -1;
+		}
+	}
+
+	// optical flow in x, y axis
+	float flow_x_rad = _sub_flow.get().pixel_flow_x_integral;
+	float flow_y_rad = _sub_flow.get().pixel_flow_y_integral;
+
+	// angular rotation in x, y axis
+	float gyro_x_rad = _flow_gyro_x_high_pass.update(
+				   _sub_flow.get().gyro_x_rate_integral);
+	float gyro_y_rad = _flow_gyro_y_high_pass.update(
+				   _sub_flow.get().gyro_y_rate_integral);
+
+	// compute velocities in camera frame using ground distance
+	// assume camera frame is body frame
+	Vector3f delta_b(
+		-(flow_x_rad - gyro_x_rad)*d,
+		-(flow_y_rad - gyro_y_rad)*d,
+		0);
+
+	// rotation of flow from body to nav frame
+	Matrix3f R_nb(_sub_att.get().R);
+	Vector3f delta_n = R_nb * delta_b;
+
+	// flow integration
+	_flowX += delta_n(0);
+	_flowY += delta_n(1);
+
+	// measurement
+	y(Y_flow_x) = _flowX;
+	y(Y_flow_y) = _flowY;
+
+	_flowQStats.update(Scalarf(_sub_flow.get().quality));
+
+	// imporant to timestamp flow even if distance is bad
+	_time_last_flow = _timeStamp;
+
+	return OK;
+}
+
+void BlockLocalPositionEstimator::flowCorrect()
+{
+	// measure flow
+	Vector<float, n_y_flow> y;
+
+	if (flowMeasure(y) != OK) { return; }
+
+	// flow measurement matrix and noise matrix
+	Matrix<float, n_y_flow, n_x> C;
+	C.setZero();
+	C(Y_flow_x, X_x) = 1;
+	C(Y_flow_y, X_y) = 1;
+
+	Matrix<float, n_y_flow, n_y_flow> R;
+	R.setZero();
+	R(Y_flow_x, Y_flow_x) =
+		_flow_xy_stddev.get() * _flow_xy_stddev.get();
+	R(Y_flow_y, Y_flow_y) =
+		_flow_xy_stddev.get() * _flow_xy_stddev.get();
+
+	// residual
+	Vector<float, 2> r = y - C * _x;
+
+	// residual covariance, (inverse)
+	Matrix<float, n_y_flow, n_y_flow> S_I =
+		inv<float, n_y_flow>(C * _P * C.transpose() + R);
+
+	// fault detection
+	float beta = (r.transpose() * (S_I * r))(0, 0);
+
+	if (beta > BETA_TABLE[n_y_flow]) {
+		if (_flowFault < FAULT_MINOR) {
+			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault,  beta %5.2f", double(beta));
+			_flowFault = FAULT_MINOR;
+		}
+
+	} else if (_flowFault) {
+		_flowFault = FAULT_NONE;
+		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
+	}
+
+	if (_flowFault < fault_lvl_disable) {
+		Matrix<float, n_x, n_y_flow> K =
+			_P * C.transpose() * S_I;
+		_x += K * r;
+		_P -= K * C * _P;
+
+	} else {
+		// reset flow integral to current estimate of position
+		// if a fault occurred
+		_flowX = _x(X_x);
+		_flowY = _x(X_y);
+	}
+
+}
+
+void BlockLocalPositionEstimator::flowCheckTimeout()
+{
+	if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
+		if (_flowInitialized) {
+			_flowInitialized = false;
+			_flowQStats.reset();
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow timeout ");
+		}
+	}
+}

src/modules/local_position_estimator/sensors/gps.cpp

@@ -0,0 +1,196 @@
+#include "../BlockLocalPositionEstimator.hpp"
+#include <systemlib/mavlink_log.h>
+#include <matrix/math.hpp>
+
+extern orb_advert_t mavlink_log_pub;
+
+// required number of samples for sensor
+// to initialize
+static const uint32_t 		REQ_GPS_INIT_COUNT = 10;
+static const uint32_t 		GPS_TIMEOUT =      1000000; // 1.0 s
+
+void BlockLocalPositionEstimator::gpsInit()
+{
+	// measure
+	Vector<double, n_y_gps> y;
+
+	if (gpsMeasure(y) != OK) {
+		_gpsStats.reset();
+		return;
+	}
+
+	// if finished
+	if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
+		double gpsLatHome = _gpsStats.getMean()(0);
+		double gpsLonHome = _gpsStats.getMean()(1);
+
+		if (!_receivedGps) {
+			_receivedGps = true;
+			map_projection_init(&_map_ref, gpsLatHome, gpsLonHome);
+		}
+
+		_gpsAltHome = _gpsStats.getMean()(2);
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps init "
+					     "lat %6.2f lon %6.2f alt %5.1f m",
+					     gpsLatHome,
+					     gpsLonHome,
+					     double(_gpsAltHome));
+		_gpsInitialized = true;
+		_gpsFault = FAULT_NONE;
+		_gpsStats.reset();
+
+		if (!_altHomeInitialized) {
+			_altHomeInitialized = true;
+			_altHome = _gpsAltHome;
+		}
+	}
+}
+
+int BlockLocalPositionEstimator::gpsMeasure(Vector<double, n_y_gps> &y)
+{
+	// check for good gps signal
+	uint8_t nSat = _sub_gps.get().satellites_used;
+	float eph = _sub_gps.get().eph;
+
+	if (nSat < 6 || eph > _gps_eph_max.get()) {
+		return -1;
+	}
+
+	// gps measurement
+	y.setZero();
+	y(0) = _sub_gps.get().lat * 1e-7;
+	y(1) = _sub_gps.get().lon * 1e-7;
+	y(2) = _sub_gps.get().alt * 1e-3;
+	y(3) = _sub_gps.get().vel_n_m_s;
+	y(4) = _sub_gps.get().vel_e_m_s;
+	y(5) = _sub_gps.get().vel_d_m_s;
+
+	// increament sums for mean
+	_gpsStats.update(y);
+	_time_last_gps = _timeStamp;
+	return OK;
+}
+
+void BlockLocalPositionEstimator::gpsCorrect()
+{
+	// measure
+	Vector<double, n_y_gps> y_global;
+
+	if (gpsMeasure(y_global) != OK) { return; }
+
+	// gps measurement in local frame
+	double  lat = y_global(0);
+	double  lon = y_global(1);
+	float  alt = y_global(2);
+	float px = 0;
+	float py = 0;
+	float pz = -(alt - _gpsAltHome);
+	map_projection_project(&_map_ref, lat, lon, &px, &py);
+	Vector<float, 6> y;
+	y.setZero();
+	y(0) = px;
+	y(1) = py;
+	y(2) = pz;
+	y(3) = y_global(3);
+	y(4) = y_global(4);
+	y(5) = y_global(5);
+
+	// gps measurement matrix, measures position and velocity
+	Matrix<float, n_y_gps, n_x> C;
+	C.setZero();
+	C(Y_gps_x, X_x) = 1;
+	C(Y_gps_y, X_y) = 1;
+	C(Y_gps_z, X_z) = 1;
+	C(Y_gps_vx, X_vx) = 1;
+	C(Y_gps_vy, X_vy) = 1;
+	C(Y_gps_vz, X_vz) = 1;
+
+	// gps covariance matrix
+	Matrix<float, n_y_gps, n_y_gps> R;
+	R.setZero();
+
+	// default to parameter, use gps cov if provided
+	float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get();
+	float var_z = _gps_z_stddev.get() * _gps_z_stddev.get();
+	float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get();
+	float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
+
+	// if field is not zero, set it to the value provided
+	if (_sub_gps.get().eph > 1e-3f) {
+		var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
+	}
+
+	if (_sub_gps.get().epv > 1e-3f) {
+		var_z = _sub_gps.get().epv * _sub_gps.get().epv;
+	}
+
+	R(0, 0) = var_xy;
+	R(1, 1) = var_xy;
+	R(2, 2) = var_z;
+	R(3, 3) = var_vxy;
+	R(4, 4) = var_vxy;
+	R(5, 5) = var_vz;
+
+	// get delayed x and P
+	float t_delay = 0;
+	int i = 0;
+
+	for (i = 1; i < HIST_LEN; i++) {
+		t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i)(0, 0));
+
+		if (t_delay > _gps_delay.get()) {
+			break;
+		}
+	}
+
+	// if you are 3 steps past the delay you wanted, this
+	// data is probably too old to use
+	if (t_delay > GPS_DELAY_MAX) {
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps delayed data too old: %8.4f", double(t_delay));
+		return;
+	}
+
+	Vector<float, n_x> x0 = _xDelay.get(i);
+
+	// residual
+	Vector<float, n_y_gps> r = y - C * x0;
+	Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * _P * C.transpose() + R);
+
+	// fault detection
+	float beta = (r.transpose() * (S_I * r))(0, 0);
+
+	if (beta > BETA_TABLE[n_y_gps]) {
+		if (_gpsFault < FAULT_MINOR) {
+			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta));
+			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
+			//double(r(0)),  double(r(1)), double(r(2)),
+			//double(r(3)), double(r(4)), double(r(5)));
+			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
+			//double(S_I(0, 0)),  double(S_I(1, 1)), double(S_I(2, 2)),
+			//double(S_I(3, 3)),  double(S_I(4, 4)), double(S_I(5, 5)));
+			_gpsFault = FAULT_MINOR;
+		}
+
+	} else if (_gpsFault) {
+		_gpsFault = FAULT_NONE;
+		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
+	}
+
+	// kalman filter correction if no hard fault
+	if (_gpsFault < fault_lvl_disable) {
+		Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
+		_x += K * r;
+		_P -= K * C * _P;
+	}
+}
+
+void BlockLocalPositionEstimator::gpsCheckTimeout()
+{
+	if (_timeStamp - _time_last_gps > GPS_TIMEOUT) {
+		if (_gpsInitialized) {
+			_gpsInitialized = false;
+			_gpsStats.reset();
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS timeout ");
+		}
+	}
+}

src/modules/local_position_estimator/sensors/lidar.cpp

@@ -0,0 +1,134 @@
+#include "../BlockLocalPositionEstimator.hpp"
+#include <systemlib/mavlink_log.h>
+#include <matrix/math.hpp>
+
+extern orb_advert_t mavlink_log_pub;
+
+// required number of samples for sensor
+// to initialize
+//
+static const uint32_t 		REQ_LIDAR_INIT_COUNT = 10;
+static const uint32_t 		LIDAR_TIMEOUT =   1000000; // 1.0 s
+
+void BlockLocalPositionEstimator::lidarInit()
+{
+	// measure
+	Vector<float, n_y_lidar> y;
+
+	if (lidarMeasure(y) != OK) {
+		_lidarStats.reset();
+	}
+
+	// if finished
+	if (_lidarStats.getCount() > REQ_LIDAR_INIT_COUNT) {
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar init: "
+					     "mean %d cm stddev %d cm",
+					     int(100 * _lidarStats.getMean()(0)),
+					     int(100 * _lidarStats.getStdDev()(0)));
+		_lidarInitialized = true;
+		_lidarFault = FAULT_NONE;
+	}
+}
+
+int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
+{
+	// measure
+	float d = _sub_lidar->get().current_distance + _lidar_z_offset.get();
+	warnx("d %10.2g, lidar z offset %10.2g\n", double(d), double(_lidar_z_offset.get()));
+	float eps = 0.01f;
+	float min_dist = _sub_lidar->get().min_distance + eps;
+	float max_dist = _sub_lidar->get().max_distance - eps;
+
+	// check for bad data
+	if (d > max_dist || d < min_dist) {
+		return -1;
+	}
+
+	// update stats
+	_lidarStats.update(Scalarf(d));
+	_time_last_lidar = _timeStamp;
+	y.setZero();
+	y(0) = d;
+	return OK;
+}
+
+void BlockLocalPositionEstimator::lidarCorrect()
+{
+	// measure lidar
+	Vector<float, n_y_lidar> y;
+
+	if (lidarMeasure(y) != OK) { return; }
+
+	// account for leaning
+	y(0) = y(0) *
+	       cosf(_sub_att.get().roll) *
+	       cosf(_sub_att.get().pitch);
+
+	// measurement matrix
+	Matrix<float, n_y_lidar, n_x> C;
+	C.setZero();
+	// y = -(z - tz)
+	// TODO could add trig to make this an EKF correction
+	C(Y_lidar_z, X_z) = -1; // measured altitude, negative down dir.
+	C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir.
+
+	// use parameter covariance unless sensor provides reasonable value
+	Matrix<float, n_y_lidar, n_y_lidar> R;
+	R.setZero();
+	float cov = _sub_lidar->get().covariance;
+
+	if (cov < 1.0e-3f) {
+		R(0, 0) = _lidar_z_stddev.get() * _lidar_z_stddev.get();
+
+	} else {
+		R(0, 0) = cov;
+	}
+
+	// residual
+	Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>((C * _P * C.transpose()) + R);
+	Vector<float, n_y_lidar> r = y - C * _x;
+
+	// fault detection
+	float beta = (r.transpose() * (S_I * r))(0, 0);
+
+	if (beta > BETA_TABLE[n_y_lidar]) {
+		if (_lidarFault < FAULT_MINOR) {
+			_lidarFault = FAULT_MINOR;
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar fault,  beta %5.2f", double(beta));
+		}
+
+		// abort correction
+		return;
+
+	} else if (_lidarFault) { // disable fault if ok
+		_lidarFault = FAULT_NONE;
+		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK");
+	}
+
+	// kalman filter correction if no fault
+	if (_lidarFault < fault_lvl_disable) {
+		Matrix<float, n_x, n_y_lidar> K = _P * C.transpose() * S_I;
+		Vector<float, n_x> dx = K * r;
+
+		if (!_canEstimateXY) {
+			dx(X_x) = 0;
+			dx(X_y) = 0;
+			dx(X_vx) = 0;
+			dx(X_vy) = 0;
+		}
+
+		_x += dx;
+		_P -= K * C * _P;
+	}
+}
+
+void BlockLocalPositionEstimator::lidarCheckTimeout()
+{
+	if (_timeStamp - _time_last_lidar > LIDAR_TIMEOUT) {
+		if (_lidarInitialized) {
+			_lidarInitialized = false;
+			_lidarStats.reset();
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar timeout ");
+		}
+	}
+}

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -0,0 +1,115 @@
+#include "../BlockLocalPositionEstimator.hpp"
+#include <systemlib/mavlink_log.h>
+#include <matrix/math.hpp>
+
+extern orb_advert_t mavlink_log_pub;
+
+// required number of samples for sensor
+// to initialize
+static const uint32_t 		REQ_MOCAP_INIT_COUNT = 20;
+static const uint32_t 		MOCAP_TIMEOUT =     200000;	// 0.2 s
+
+void BlockLocalPositionEstimator::mocapInit()
+{
+	// measure
+	Vector<float, n_y_mocap> y;
+
+	if (mocapMeasure(y) != OK) {
+		_mocapStats.reset();
+		return;
+	}
+
+	// if finished
+	if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
+		_mocapHome = _mocapStats.getMean();
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
+					     "%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
+					     double(_mocapStats.getMean()(0)),
+					     double(_mocapStats.getMean()(1)),
+					     double(_mocapStats.getMean()(2)),
+					     double(_mocapStats.getStdDev()(0)),
+					     double(_mocapStats.getStdDev()(1)),
+					     double(_mocapStats.getStdDev()(2)));
+		_mocapInitialized = true;
+		_mocapFault = FAULT_NONE;
+
+		if (!_altHomeInitialized) {
+			_altHomeInitialized = true;
+			_altHome = _mocapHome(2);
+		}
+	}
+}
+
+int BlockLocalPositionEstimator::mocapMeasure(Vector<float, n_y_mocap> &y)
+{
+	y.setZero();
+	y(Y_mocap_x) = _sub_mocap.get().x;
+	y(Y_mocap_y) = _sub_mocap.get().y;
+	y(Y_mocap_z) = _sub_mocap.get().z;
+	_mocapStats.update(y);
+	_time_last_mocap = _sub_mocap.get().timestamp_boot;
+	return OK;
+}
+
+void BlockLocalPositionEstimator::mocapCorrect()
+{
+	// measure
+	Vector<float, n_y_mocap> y;
+
+	if (mocapMeasure(y) != OK) { return; }
+
+	// make measurement relative to home
+	y -= _mocapHome;
+
+	// mocap measurement matrix, measures position
+	Matrix<float, n_y_mocap, n_x> C;
+	C.setZero();
+	C(Y_mocap_x, X_x) = 1;
+	C(Y_mocap_y, X_y) = 1;
+	C(Y_mocap_z, X_z) = 1;
+
+	// noise matrix
+	Matrix<float, n_y_mocap, n_y_mocap> R;
+	R.setZero();
+	float mocap_p_var = _mocap_p_stddev.get()* \
+			    _mocap_p_stddev.get();
+	R(Y_mocap_x, Y_mocap_x) = mocap_p_var;
+	R(Y_mocap_y, Y_mocap_y) = mocap_p_var;
+	R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
+
+	// residual
+	Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R);
+	Matrix<float, n_y_mocap, 1> r = y - C * _x;
+
+	// fault detection
+	float beta = (r.transpose() * (S_I * r))(0, 0);
+
+	if (beta > BETA_TABLE[n_y_mocap]) {
+		if (_mocapFault < FAULT_MINOR) {
+			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
+			_mocapFault = FAULT_MINOR;
+		}
+
+	} else if (_mocapFault) {
+		_mocapFault = FAULT_NONE;
+		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap OK");
+	}
+
+	// kalman filter correction if no fault
+	if (_mocapFault < fault_lvl_disable) {
+		Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
+		_x += K * r;
+		_P -= K * C * _P;
+	}
+}
+
+void BlockLocalPositionEstimator::mocapCheckTimeout()
+{
+	if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) {
+		if (_mocapInitialized) {
+			_mocapInitialized = false;
+			_mocapStats.reset();
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap timeout ");
+		}
+	}
+}

src/modules/local_position_estimator/sensors/sonar.cpp

@@ -0,0 +1,141 @@
+#include "../BlockLocalPositionEstimator.hpp"
+#include <systemlib/mavlink_log.h>
+#include <matrix/math.hpp>
+
+extern orb_advert_t mavlink_log_pub;
+
+// required number of samples for sensor
+// to initialize
+static const int 		REQ_SONAR_INIT_COUNT = 10;
+static const uint32_t 	SONAR_TIMEOUT =   1000000; // 1.0 s
+
+void BlockLocalPositionEstimator::sonarInit()
+{
+	// measure
+	Vector<float, n_y_sonar> y;
+
+	if (sonarMeasure(y) != OK) {
+		_sonarStats.reset();
+		return;
+	}
+
+	// if finished
+	if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) {
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init "
+					     "mean %d cm std %d cm",
+					     int(100 * _sonarStats.getMean()(0)),
+					     int(100 * _sonarStats.getStdDev()(0)));
+		_sonarInitialized = true;
+		_sonarFault = FAULT_NONE;
+	}
+}
+
+int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
+{
+	// measure
+	float d = _sub_sonar->get().current_distance + _sonar_z_offset.get();
+	float eps = 0.01f;
+	float min_dist = _sub_sonar->get().min_distance + eps;
+	float max_dist = _sub_sonar->get().max_distance - eps;
+
+	// check for bad data
+	if (d > max_dist || d < min_dist) {
+		return -1;
+	}
+
+	// update stats
+	_sonarStats.update(Scalarf(d));
+	_time_last_sonar = _timeStamp;
+	y.setZero();
+	y(0) = d *
+	       cosf(_sub_att.get().roll) *
+	       cosf(_sub_att.get().pitch);
+	return OK;
+}
+
+void BlockLocalPositionEstimator::sonarCorrect()
+{
+	// measure
+	Vector<float, n_y_sonar> y;
+
+	if (sonarMeasure(y) != OK) { return; }
+
+	// do not use sonar if lidar is active
+	//if (_lidarInitialized && (_lidarFault < fault_lvl_disable)) { return; }
+
+	// calculate covariance
+	float cov = _sub_sonar->get().covariance;
+
+	if (cov < 1.0e-3f) {
+		// use sensor value if reasoanble
+		cov = _sonar_z_stddev.get() * _sonar_z_stddev.get();
+	}
+
+	// sonar measurement matrix and noise matrix
+	Matrix<float, n_y_sonar, n_x> C;
+	C.setZero();
+	// y = -(z - tz)
+	// TODO could add trig to make this an EKF correction
+	C(Y_sonar_z, X_z) = -1; // measured altitude, negative down dir.
+	C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir.
+
+	// covariance matrix
+	Matrix<float, n_y_sonar, n_y_sonar> R;
+	R.setZero();
+	R(0, 0) = cov;
+
+	// residual
+	Vector<float, n_y_sonar> r = y - C * _x;
+
+	// residual covariance, (inverse)
+	Matrix<float, n_y_sonar, n_y_sonar> S_I =
+		inv<float, n_y_sonar>(C * _P * C.transpose() + R);
+
+	// fault detection
+	float beta = (r.transpose()  * (S_I * r))(0, 0);
+
+	if (beta > BETA_TABLE[n_y_sonar]) {
+		if (_sonarFault < FAULT_MINOR) {
+			_sonarFault = FAULT_MINOR;
+			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault,  beta %5.2f", double(beta));
+		}
+
+		// abort correction
+		return;
+
+	} else if (_sonarFault) {
+		_sonarFault = FAULT_NONE;
+		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar OK");
+	}
+
+	// kalman filter correction if no fault
+	if (_sonarFault < fault_lvl_disable) {
+		Matrix<float, n_x, n_y_sonar> K =
+			_P * C.transpose() * S_I;
+		Vector<float, n_x> dx = K * r;
+
+		if (!_canEstimateXY) {
+			dx(X_x) = 0;
+			dx(X_y) = 0;
+			dx(X_vx) = 0;
+			dx(X_vy) = 0;
+		}
+
+		_x += dx;
+		_P -= K * C * _P;
+	}
+
+}
+
+void BlockLocalPositionEstimator::sonarCheckTimeout()
+{
+	if (_timeStamp - _time_last_sonar > SONAR_TIMEOUT) {
+		if (_sonarInitialized) {
+			_sonarInitialized = false;
+			_sonarStats.reset();
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar timeout ");
+		}
+	}
+}
+
+

src/modules/local_position_estimator/sensors/vision.cpp

@@ -0,0 +1,113 @@
+#include "../BlockLocalPositionEstimator.hpp"
+#include <systemlib/mavlink_log.h>
+#include <matrix/math.hpp>
+
+extern orb_advert_t mavlink_log_pub;
+
+// required number of samples for sensor
+// to initialize
+static const uint32_t 		REQ_VISION_INIT_COUNT = 20;
+static const uint32_t 		VISION_TIMEOUT =    500000;	// 0.5 s
+
+void BlockLocalPositionEstimator::visionInit()
+{
+	// measure
+	Vector<float, n_y_vision> y;
+
+	if (visionMeasure(y) != OK) {
+		_visionStats.reset();
+		return;
+	}
+
+	// increament sums for mean
+	if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
+		_visionHome = _visionStats.getMean();
+		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: "
+					     "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
+					     double(_visionStats.getMean()(0)),
+					     double(_visionStats.getMean()(1)),
+					     double(_visionStats.getMean()(2)),
+					     double(_visionStats.getStdDev()(0)),
+					     double(_visionStats.getStdDev()(1)),
+					     double(_visionStats.getStdDev()(2)));
+		_visionInitialized = true;
+		_visionFault = FAULT_NONE;
+
+		if (!_altHomeInitialized) {
+			_altHomeInitialized = true;
+			_altHome = _visionHome(2);
+		}
+	}
+}
+
+int BlockLocalPositionEstimator::visionMeasure(Vector<float, n_y_vision> &y)
+{
+	y.setZero();
+	y(Y_vision_x) = _sub_vision_pos.get().x;
+	y(Y_vision_y) = _sub_vision_pos.get().y;
+	y(Y_vision_z) = _sub_vision_pos.get().z;
+	_visionStats.update(y);
+	_time_last_vision_p = _sub_vision_pos.get().timestamp_boot;
+	return OK;
+}
+
+void BlockLocalPositionEstimator::visionCorrect()
+{
+	// measure
+	Vector<float, n_y_vision> y;
+
+	if (visionMeasure(y) != OK) { return; }
+
+	// make measurement relative to home
+	y -= _visionHome;
+
+	// vision measurement matrix, measures position
+	Matrix<float, n_y_vision, n_x> C;
+	C.setZero();
+	C(Y_vision_x, X_x) = 1;
+	C(Y_vision_y, X_y) = 1;
+	C(Y_vision_z, X_z) = 1;
+
+	// noise matrix
+	Matrix<float, n_y_vision, n_y_vision> R;
+	R.setZero();
+	R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
+	R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
+	R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
+
+	// residual
+	Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
+	Matrix<float, n_y_vision, 1> r = y - C * _x;
+
+	// fault detection
+	float beta = (r.transpose() * (S_I * r))(0, 0);
+
+	if (beta > BETA_TABLE[n_y_vision]) {
+		if (_visionFault < FAULT_MINOR) {
+			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
+			_visionFault = FAULT_MINOR;
+		}
+
+	} else if (_visionFault) {
+		_visionFault = FAULT_NONE;
+		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK");
+	}
+
+	// kalman filter correction if no fault
+	if (_visionFault <  fault_lvl_disable) {
+		Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
+		_x += K * r;
+		_P -= K * C * _P;
+	}
+}
+
+void BlockLocalPositionEstimator::visionCheckTimeout()
+{
+	if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) {
+		if (_visionInitialized) {
+			_visionInitialized = false;
+			_visionStats.reset();
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position timeout ");
+		}
+	}
+}