diff --git a/src/lib/controllib/blocks.cpp b/src/lib/controllib/blocks.cpp
index aeef0f0977..0e0c3a6ccc 100644
--- a/src/lib/controllib/blocks.cpp
+++ b/src/lib/controllib/blocks.cpp
@@ -84,6 +84,7 @@ float BlockLowPass::update(float input)
 	return getState();
 }
 
+
 float BlockHighPass::update(float input)
 {
 	float b = 2 * float(M_PI) * getFCut() * getDt();
diff --git a/src/lib/controllib/blocks.hpp b/src/lib/controllib/blocks.hpp
index 28b73aa62d..a56a32b216 100644
--- a/src/lib/controllib/blocks.hpp
+++ b/src/lib/controllib/blocks.hpp
@@ -127,6 +127,45 @@ protected:
 	control::BlockParamFloat _fCut;
 };
 
+template<class Type, size_t M>
+class __EXPORT BlockLowPassVector: public Block
+{
+public:
+// methods
+	BlockLowPassVector(SuperBlock *parent,
+			   const char *name) :
+		Block(parent, name),
+		_state(),
+		_fCut(this, "") // only one parameter, no need to name
+	{
+		for (int i = 0; i < M; i++) {
+			_state(i) = 0.0f / 0.0f;
+		}
+	};
+	virtual ~BlockLowPassVector() {};
+	matrix::Vector<Type, M> update(const matrix::Matrix<Type, M, 1> &input)
+	{
+		for (int i = 0; i < M; i++) {
+			if (!PX4_ISFINITE(getState()(i))) {
+				setState(input);
+			}
+		}
+
+		float b = 2 * float(M_PI) * getFCut() * getDt();
+		float a = b / (1 + b);
+		setState(input * a + getState() * (1 - a));
+		return getState();
+	}
+// accessors
+	matrix::Vector<Type, M> getState() { return _state; }
+	float getFCut() { return _fCut.get(); }
+	void setState(const matrix::Vector<Type, M> &state) { _state = state; }
+private:
+// attributes
+	matrix::Vector<Type, M> _state;
+	control::BlockParamFloat _fCut;
+};
+
 /**
  * A high pass filter as described here:
  * http://en.wikipedia.org/wiki/High-pass_filter.
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index 9ffd4599fe..b2b6c6bdda 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -797,6 +797,7 @@ bool AttitudeEstimatorQ::update(float dt)
 
 	_q.normalize();
 
+
 	// Accelerometer correction
 	// Project 'k' unit vector of earth frame to body frame
 	// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
@@ -810,7 +811,9 @@ bool AttitudeEstimatorQ::update(float dt)
 	corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
 
 	// Gyro bias estimation
-	_gyro_bias += corr * (_w_gyro_bias * dt);
+	if (_gyro.length() < 1.0f) {
+		_gyro_bias += corr * (_w_gyro_bias * dt);
+	}
 
 	for (int i = 0; i < 3; i++) {
 		_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
index 12360a18a2..b9b4c21fa5 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
@@ -3,40 +3,45 @@
 #include <fcntl.h>
 #include <systemlib/err.h>
 #include <matrix/math.hpp>
+#include <cstdlib>
 
 orb_advert_t mavlink_log_pub = nullptr;
 
 // timeouts for sensors in microseconds
 static const uint32_t 		EST_SRC_TIMEOUT = 10000; // 0.01 s
 
+// required standard deviation of estimate for estimator to publish data
+static const uint32_t 		EST_STDDEV_XY_VALID = 2.0; // 2.0 m
+static const uint32_t 		EST_STDDEV_Z_VALID = 2.0; // 2.0 m
+static const uint32_t 		EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
+
 // minimum flow altitude
 static const float flow_min_agl = 0.3;
 
 BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// this block has no parent, and has name LPE
 	SuperBlock(NULL, "LPE"),
-
 	// subscriptions, set rate, add to list
-	// TODO topic speed limiting?
-	_sub_status(ORB_ID(vehicle_status), 0, 0, &getSubscriptions()),
-	_sub_armed(ORB_ID(actuator_armed), 0, 0, &getSubscriptions()),
-	_sub_control_mode(ORB_ID(vehicle_control_mode),
-			  0, 0, &getSubscriptions()),
-	_sub_att(ORB_ID(vehicle_attitude), 0, 0, &getSubscriptions()),
-	_sub_att_sp(ORB_ID(vehicle_attitude_setpoint),
-		    0, 0, &getSubscriptions()),
-	_sub_flow(ORB_ID(optical_flow), 0, 0, &getSubscriptions()),
-	_sub_sensor(ORB_ID(sensor_combined), 0, 0, &getSubscriptions()),
-	_sub_param_update(ORB_ID(parameter_update), 0, 0, &getSubscriptions()),
-	_sub_manual(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()),
-	_sub_home(ORB_ID(home_position), 0, 0, &getSubscriptions()),
-	_sub_gps(ORB_ID(vehicle_gps_position), 0, 0, &getSubscriptions()),
-	_sub_vision_pos(ORB_ID(vision_position_estimate), 0, 0, &getSubscriptions()),
-	_sub_mocap(ORB_ID(att_pos_mocap), 0, 0, &getSubscriptions()),
-	_sub_dist0(ORB_ID(distance_sensor), 0, 0, &getSubscriptions()),
-	_sub_dist1(ORB_ID(distance_sensor), 0, 1, &getSubscriptions()),
-	_sub_dist2(ORB_ID(distance_sensor), 0, 2, &getSubscriptions()),
-	_sub_dist3(ORB_ID(distance_sensor), 0, 3, &getSubscriptions()),
+	_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
+	_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
+	// flow 10 hz
+	_sub_flow(ORB_ID(optical_flow), 1000 / 10, 0, &getSubscriptions()),
+	// main prediction loop, 100 hz
+	_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
+	// status updates 2 hz
+	_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
+	_sub_manual(ORB_ID(manual_control_setpoint), 1000 / 2, 0, &getSubscriptions()),
+	_sub_home(ORB_ID(home_position), 1000 / 2, 0, &getSubscriptions()),
+	// gps 10 hz
+	_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
+	// vision 5 hz
+	_sub_vision_pos(ORB_ID(vision_position_estimate), 1000 / 5, 0, &getSubscriptions()),
+	// all distance sensors, 10 hz
+	_sub_mocap(ORB_ID(att_pos_mocap), 1000 / 10, 0, &getSubscriptions()),
+	_sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
+	_sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
+	_sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()),
+	_sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()),
 	_dist_subs(),
 	_sub_lidar(NULL),
 	_sub_sonar(NULL),
@@ -97,7 +102,10 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_mocapStats(this, ""),
 	_gpsStats(this, ""),
 
-	// stats
+	// low pass
+	_xLowPass(this, "X_LP"),
+
+	// delay
 	_xDelay(this, ""),
 	_tDelay(this, ""),
 
@@ -141,9 +149,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_flowMeanQual(0),
 
 	// status
-	_canEstimateXY(false),
-	_canEstimateZ(false),
-	_canEstimateT(false),
+	_validXY(false),
+	_validZ(false),
+	_validTZ(false),
 	_xyTimeout(true),
 	_zTimeout(true),
 	_tzTimeout(true),
@@ -182,12 +190,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
 	_polls[POLL_SENSORS].events = POLLIN;
 
-	// initialize P, x, u
-	initP();
+	// initialize A, B,  P, x, u
 	_x.setZero();
 	_u.setZero();
 	_flowX = 0;
 	_flowY = 0;
+	initSS();
 
 	// perf counters
 	_loop_perf = perf_alloc(PC_ELAPSED,
@@ -207,6 +215,14 @@ BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
 {
 }
 
+Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
+	float t,
+	const Vector<float, BlockLocalPositionEstimator::n_x> &x,
+	const Vector<float, BlockLocalPositionEstimator::n_u> &u)
+{
+	return _A * x + _B * u;
+}
+
 void BlockLocalPositionEstimator::update()
 {
 
@@ -254,6 +270,9 @@ void BlockLocalPositionEstimator::update()
 
 		// assume we are on the ground, so terrain alt is local alt
 		_x(X_tz) = _x(X_z);
+
+		// reset lowpass filter as well
+		_xLowPass.setState(_x);
 	}
 
 	_lastArmedState = armedState;
@@ -275,6 +294,7 @@ void BlockLocalPositionEstimator::update()
 	// update parameters
 	if (paramsUpdated) {
 		updateParams();
+		updateSSParams();
 	}
 
 	// update home position projection
@@ -282,27 +302,60 @@ void BlockLocalPositionEstimator::update()
 		updateHome();
 	}
 
-	// determine if we should start estimating
-	_canEstimateZ =
-		(_baroInitialized && _baroFault < fault_lvl_disable);
-	_canEstimateXY =
-		(_gpsInitialized && _gpsFault < fault_lvl_disable) ||
-		(_flowInitialized && _flowFault < fault_lvl_disable) ||
-		(_visionInitialized && _visionFault < fault_lvl_disable) ||
-		(_mocapInitialized && _mocapFault < fault_lvl_disable);
-	_canEstimateT =
-		(_lidarInitialized && _lidarFault < fault_lvl_disable) ||
-		(_sonarInitialized && _sonarFault < fault_lvl_disable);
+	// is xy valid?
+	bool xy_stddev_ok = sqrt(math::max(_P(X_x, X_x), _P(X_y, X_y))) < EST_STDDEV_XY_VALID;
 
-	if (_canEstimateXY) {
+	if (_validXY) {
+		// if valid and gps has timed out, set to not valid
+		if (!xy_stddev_ok && !_gpsInitialized) {
+			_validXY = false;
+		}
+
+	} else {
+		if (xy_stddev_ok) {
+			_validXY = true;
+		}
+	}
+
+	// is z valid?
+	bool z_stddev_ok = sqrt(_P(X_z, X_z)) < EST_STDDEV_Z_VALID;
+
+	if (_validZ) {
+		// if valid and baro has timed out, set to not valid
+		if (!z_stddev_ok && !_baroInitialized) {
+			_validZ = false;
+		}
+
+	} else {
+		if (z_stddev_ok) {
+			_validZ = true;
+		}
+	}
+
+	// is terrain valid?
+	bool tz_stddev_ok = sqrt(_P(X_tz, X_tz)) < EST_STDDEV_TZ_VALID;
+
+	if (_validTZ) {
+		if (!tz_stddev_ok) {
+			_validTZ = false;
+		}
+
+	} else {
+		if (tz_stddev_ok) {
+			_validTZ = true;
+		}
+	}
+
+	// timeouts
+	if (_validXY) {
 		_time_last_xy = _timeStamp;
 	}
 
-	if (_canEstimateZ) {
+	if (_validZ) {
 		_time_last_z = _timeStamp;
 	}
 
-	if (_canEstimateT) {
+	if (_validTZ) {
 		_time_last_tz = _timeStamp;
 	}
 
@@ -310,7 +363,7 @@ void BlockLocalPositionEstimator::update()
 	checkTimeouts();
 
 	// if we have no lat, lon initialize projection at 0,0
-	if (_canEstimateXY && !_map_ref.init_done) {
+	if (_validXY && !_map_ref.init_done) {
 		map_projection_init(&_map_ref,
 				    _init_home_lat.get(),
 				    _init_home_lon.get());
@@ -431,7 +484,7 @@ void BlockLocalPositionEstimator::update()
 		publishLocalPos();
 		publishEstimatorStatus();
 
-		if (_canEstimateXY) {
+		if (_validXY) {
 			publishGlobalPos();
 		}
 	}
@@ -495,7 +548,53 @@ void BlockLocalPositionEstimator::checkTimeouts()
 
 float BlockLocalPositionEstimator::agl()
 {
-	return _x(X_tz) - _x(X_z);
+	const Vector<float, n_x> &xLP = _xLowPass.getState();
+	return xLP(X_tz) - xLP(X_z);
+}
+
+void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
+{
+	// don't correct bias when rotating rapidly
+	float ang_speed = sqrt(
+				  _sub_att.get().rollspeed * _sub_att.get().rollspeed +
+				  _sub_att.get().pitchspeed * _sub_att.get().pitchspeed +
+				  _sub_att.get().yawspeed * _sub_att.get().yawspeed);
+
+	if (ang_speed > 1) {
+		dx(X_bx) = 0;
+		dx(X_by) = 0;
+		dx(X_bz) = 0;
+	}
+
+	if (!_validXY) {
+		dx(X_x) = 0;
+		dx(X_y) = 0;
+		dx(X_vx) = 0;
+		dx(X_vy) = 0;
+		dx(X_bx) = 0;
+		dx(X_by) = 0;
+	}
+
+	if (!_validZ) {
+		dx(X_z) = 0;
+		dx(X_vz) = 0;
+		dx(X_bz) = 0;
+	}
+
+	if (!_validTZ) {
+		dx(X_tz) = 0;
+	}
+
+	// saturate bias
+	float bx = dx(X_bx) + _x(X_bx);
+	float by = dx(X_by) + _x(X_by);
+	float bz = dx(X_bz) + _x(X_bz);
+
+	if (std::abs(bx) > BIAS_MAX) { bx = BIAS_MAX * bx / std::abs(bx); }
+
+	if (std::abs(by) > BIAS_MAX) { by = BIAS_MAX * by / std::abs(by); }
+
+	if (std::abs(bz) > BIAS_MAX) { bz = BIAS_MAX * bz / std::abs(bz); }
 }
 
 void BlockLocalPositionEstimator::detectDistanceSensors()
@@ -551,21 +650,23 @@ void BlockLocalPositionEstimator::updateHome()
 
 void BlockLocalPositionEstimator::publishLocalPos()
 {
+	const Vector<float, n_x> &xLP = _xLowPass.getState();
+
 	// publish local position
 	if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
 	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
 	    && PX4_ISFINITE(_x(X_vz))) {
 		_pub_lpos.get().timestamp = _timeStamp;
-		_pub_lpos.get().xy_valid = _canEstimateXY;
-		_pub_lpos.get().z_valid = _canEstimateZ;
-		_pub_lpos.get().v_xy_valid = _canEstimateXY;
-		_pub_lpos.get().v_z_valid = _canEstimateZ;
-		_pub_lpos.get().x = _x(X_x); 	// north
-		_pub_lpos.get().y = _x(X_y);  	// east
-		_pub_lpos.get().z = _x(X_z); 	// down
-		_pub_lpos.get().vx = _x(X_vx);  // north
-		_pub_lpos.get().vy = _x(X_vy);  // east
-		_pub_lpos.get().vz = _x(X_vz); 	// down
+		_pub_lpos.get().xy_valid = _validXY;
+		_pub_lpos.get().z_valid = _validZ;
+		_pub_lpos.get().v_xy_valid = _validXY;
+		_pub_lpos.get().v_z_valid = _validZ;
+		_pub_lpos.get().x = xLP(X_x); 	// north
+		_pub_lpos.get().y = xLP(X_y);  	// east
+		_pub_lpos.get().z = xLP(X_z); 	// down
+		_pub_lpos.get().vx = xLP(X_vx); // north
+		_pub_lpos.get().vy = xLP(X_vy); // east
+		_pub_lpos.get().vz = xLP(X_vz); // down
 		_pub_lpos.get().yaw = _sub_att.get().yaw;
 		_pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference
 		_pub_lpos.get().z_global = _baroInitialized;
@@ -574,9 +675,9 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
 		_pub_lpos.get().ref_alt = _sub_home.get().alt;
 		_pub_lpos.get().dist_bottom = agl();
-		_pub_lpos.get().dist_bottom_rate = -_x(X_vz);
+		_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
 		_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
-		_pub_lpos.get().dist_bottom_valid = _canEstimateZ;
+		_pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
 		_pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
 		_pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
 		_pub_lpos.update();
@@ -595,13 +696,13 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
 	_pub_est_status.get().n_states = n_x;
 	_pub_est_status.get().nan_flags = 0;
 	_pub_est_status.get().health_flags =
-		((_baroFault > fault_lvl_disable) << SENSOR_BARO)
-		+ ((_gpsFault > fault_lvl_disable) << SENSOR_GPS)
-		+ ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR)
-		+ ((_flowFault > fault_lvl_disable) << SENSOR_FLOW)
-		+ ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR)
-		+ ((_visionFault > fault_lvl_disable) << SENSOR_VISION)
-		+ ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP);
+		((_baroFault > FAULT_NONE) << SENSOR_BARO)
+		+ ((_gpsFault > FAULT_NONE) << SENSOR_GPS)
+		+ ((_lidarFault > FAULT_NONE) << SENSOR_LIDAR)
+		+ ((_flowFault > FAULT_NONE) << SENSOR_FLOW)
+		+ ((_sonarFault > FAULT_NONE) << SENSOR_SONAR)
+		+ ((_visionFault > FAULT_NONE) << SENSOR_VISION)
+		+ ((_mocapFault > FAULT_NONE) << SENSOR_MOCAP);
 	_pub_est_status.get().timeout_flags =
 		(_baroInitialized << SENSOR_BARO)
 		+ (_gpsInitialized << SENSOR_GPS)
@@ -618,26 +719,27 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 	// publish global position
 	double lat = 0;
 	double lon = 0;
-	map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon);
-	float alt = -_x(X_z) + _altHome;
+	const Vector<float, n_x> &xLP = _xLowPass.getState();
+	map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
+	float alt = -xLP(X_z) + _altHome;
 
 	if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
-	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) &&
-	    PX4_ISFINITE(_x(X_vz))) {
+	    PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
+	    PX4_ISFINITE(xLP(X_vz))) {
 		_pub_gpos.get().timestamp = _timeStamp;
 		_pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec;
 		_pub_gpos.get().lat = lat;
 		_pub_gpos.get().lon = lon;
 		_pub_gpos.get().alt = alt;
-		_pub_gpos.get().vel_n = _x(X_vx);
-		_pub_gpos.get().vel_e = _x(X_vy);
-		_pub_gpos.get().vel_d = _x(X_vz);
+		_pub_gpos.get().vel_n = xLP(X_vx);
+		_pub_gpos.get().vel_e = xLP(X_vy);
+		_pub_gpos.get().vel_d = xLP(X_vz);
 		_pub_gpos.get().yaw = _sub_att.get().yaw;
 		_pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
 		_pub_gpos.get().epv = sqrtf(_P(X_z, X_z));
-		_pub_gpos.get().terrain_alt = _altHome - _x(X_tz);
-		_pub_gpos.get().terrain_alt_valid = _canEstimateT;
-		_pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout;
+		_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
+		_pub_gpos.get().terrain_alt_valid = _validTZ;
+		_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
 		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
 		_pub_gpos.update();
 	}
@@ -646,23 +748,95 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 void BlockLocalPositionEstimator::initP()
 {
 	_P.setZero();
-	_P(X_x, X_x) = 1;
-	_P(X_y, X_y) = 1;
-	_P(X_z, X_z) = 1;
+	_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID; // initialize to twice valid condition
+	_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID;
+	_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID;
 	_P(X_vx, X_vx) = 1;
 	_P(X_vy, X_vy) = 1;
 	_P(X_vz, X_vz) = 1;
 	_P(X_bx, X_bx) = 1e-6;
 	_P(X_by, X_by) = 1e-6;
 	_P(X_bz, X_bz) = 1e-6;
-	_P(X_tz, X_tz) = 1;
+	_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID;
+}
+
+void BlockLocalPositionEstimator::initSS()
+{
+	initP();
+
+	// dynamics matrix
+	_A.setZero();
+	// derivative of position is velocity
+	_A(X_x, X_vx) = 1;
+	_A(X_y, X_vy) = 1;
+	_A(X_z, X_vz) = 1;
+
+	// input matrix
+	_B.setZero();
+	_B(X_vx, U_ax) = 1;
+	_B(X_vy, U_ay) = 1;
+	_B(X_vz, U_az) = 1;
+
+	// update components that depend on current state
+	updateSSStates();
+	updateSSParams();
+}
+
+void BlockLocalPositionEstimator::updateSSStates()
+{
+	// derivative of velocity is accelerometer acceleration
+	// (in input matrix) - bias (in body frame)
+	Matrix3f R_att(_sub_att.get().R);
+	_A(X_vx, X_bx) = -R_att(0, 0);
+	_A(X_vx, X_by) = -R_att(0, 1);
+	_A(X_vx, X_bz) = -R_att(0, 2);
+
+	_A(X_vy, X_bx) = -R_att(1, 0);
+	_A(X_vy, X_by) = -R_att(1, 1);
+	_A(X_vy, X_bz) = -R_att(1, 2);
+
+	_A(X_vz, X_bx) = -R_att(2, 0);
+	_A(X_vz, X_by) = -R_att(2, 1);
+	_A(X_vz, X_bz) = -R_att(2, 2);
+}
+
+void BlockLocalPositionEstimator::updateSSParams()
+{
+	// input noise covariance matrix
+	_R.setZero();
+	_R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
+	_R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
+	_R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
+
+	// process noise power matrix
+	_Q.setZero();
+	float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
+	float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
+	_Q(X_x, X_x) = pn_p_sq;
+	_Q(X_y, X_y) = pn_p_sq;
+	_Q(X_z, X_z) = pn_p_sq;
+	_Q(X_vx, X_vx) = pn_v_sq;
+	_Q(X_vy, X_vy) = pn_v_sq;
+	_Q(X_vz, X_vz) = pn_v_sq;
+
+	// technically, the noise is in the body frame,
+	// but the components are all the same, so
+	// ignoring for now
+	float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
+	_Q(X_bx, X_bx) = pn_b_sq;
+	_Q(X_by, X_by) = pn_b_sq;
+	_Q(X_bz, X_bz) = pn_b_sq;
+
+	// terrain random walk noise
+	float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get();
+	_Q(X_tz, X_tz) = pn_t_sq;
 }
 
 void BlockLocalPositionEstimator::predict()
 {
 	// if can't update anything, don't propagate
 	// state or covariance
-	if (!_canEstimateXY && !_canEstimateZ) { return; }
+	if (!_validXY && !_validZ) { return; }
 
 	if (_integrate.get() && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
@@ -674,86 +848,26 @@ void BlockLocalPositionEstimator::predict()
 		_u = Vector3f(0, 0, 0);
 	}
 
-	// dynamics matrix
-	Matrix<float, n_x, n_x>  A; // state dynamics matrix
-	A.setZero();
-	// derivative of position is velocity
-	A(X_x, X_vx) = 1;
-	A(X_y, X_vy) = 1;
-	A(X_z, X_vz) = 1;
-
-	// derivative of velocity is accelerometer acceleration
-	// (in input matrix) - bias (in body frame)
-	Matrix3f R_att(_sub_att.get().R);
-	A(X_vx, X_bx) = -R_att(0, 0);
-	A(X_vx, X_by) = -R_att(0, 1);
-	A(X_vx, X_bz) = -R_att(0, 2);
-
-	A(X_vy, X_bx) = -R_att(1, 0);
-	A(X_vy, X_by) = -R_att(1, 1);
-	A(X_vy, X_bz) = -R_att(1, 2);
-
-	A(X_vz, X_bx) = -R_att(2, 0);
-	A(X_vz, X_by) = -R_att(2, 1);
-	A(X_vz, X_bz) = -R_att(2, 2);
-
-	// input matrix
-	Matrix<float, n_x, n_u>  B; // input matrix
-	B.setZero();
-	B(X_vx, U_ax) = 1;
-	B(X_vy, U_ay) = 1;
-	B(X_vz, U_az) = 1;
-
-	// input noise covariance matrix
-	Matrix<float, n_u, n_u> R;
-	R.setZero();
-	R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
-	R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
-	R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
-
-	// process noise power matrix
-	Matrix<float, n_x, n_x>  Q;
-	Q.setZero();
-	float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
-	float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
-	Q(X_x, X_x) = pn_p_sq;
-	Q(X_y, X_y) = pn_p_sq;
-	Q(X_z, X_z) = pn_p_sq;
-	Q(X_vx, X_vx) = pn_v_sq;
-	Q(X_vy, X_vy) = pn_v_sq;
-	Q(X_vz, X_vz) = pn_v_sq;
-
-	// technically, the noise is in the body frame,
-	// but the components are all the same, so
-	// ignoring for now
-	float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
-	Q(X_bx, X_bx) = pn_b_sq;
-	Q(X_by, X_by) = pn_b_sq;
-	Q(X_bz, X_bz) = pn_b_sq;
-
-	// terrain random walk noise
-	float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get();
-	Q(X_tz, X_tz) = pn_t_sq;
+	// update state space based on new states
+	updateSSStates();
 
 	// continuous time kalman filter prediction
-	Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
-
-	// only predict for components we have
-	// valid measurements for
-	if (!_canEstimateXY) {
-		dx(X_x) = 0;
-		dx(X_y) = 0;
-		dx(X_vx) = 0;
-		dx(X_vy) = 0;
-	}
-
-	if (!_canEstimateZ) {
-		dx(X_z) = 0;
-		dx(X_vz) = 0;
-	}
+	// integrate runge kutta 4th order
+	// TODO move rk4 algorithm to matrixlib
+	// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
+	float h = getDt();
+	Vector<float, n_x> k1, k2, k3, k4;
+	k1 = dynamics(0, _x, _u);
+	k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
+	k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
+	k4 = dynamics(h, _x + k3 * h, _u);
+	Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
 
 	// propagate
+	correctionLogic(dx);
 	_x += dx;
-	_P += (A * _P + _P * A.transpose() +
-	       B * R * B.transpose() + Q) * getDt();
+	_P += (_A * _P + _P * _A.transpose() +
+	       _B * _R * _B.transpose() +
+	       _Q) * getDt();
+	_xLowPass.update(_x);
 }
diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
index eba9d61355..92a3a9fc14 100644
--- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
@@ -33,8 +33,9 @@
 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 float GPS_DELAY_MAX = 0.5f; // seconds
+static const float HIST_STEP = 0.05f; // 20 hz
+static const float BIAS_MAX = 1e-1f;
 static const size_t HIST_LEN = 10; // GPS_DELAY_MAX / HIST_STEP;
 static const size_t N_DIST_SUBS = 4;
 
@@ -134,6 +135,10 @@ public:
 
 	BlockLocalPositionEstimator();
 	void update();
+	Vector<float, n_x> dynamics(
+		float t,
+		const Vector<float, n_x> &x,
+		const Vector<float, n_u> &u);
 	virtual ~BlockLocalPositionEstimator();
 
 private:
@@ -144,6 +149,9 @@ private:
 	// methods
 	// ----------------------------
 	void initP();
+	void initSS();
+	void updateSSStates();
+	void updateSSParams();
 
 	// predict the next state
 	void predict();
@@ -195,6 +203,7 @@ private:
 
 	// misc
 	float agl();
+	void correctionLogic(Vector<float, n_x> &dx);
 	void detectDistanceSensors();
 	void updateHome();
 
@@ -207,11 +216,8 @@ private:
 	// ----------------------------
 
 	// subscriptions
-	uORB::Subscription<vehicle_status_s> _sub_status;
 	uORB::Subscription<actuator_armed_s> _sub_armed;
-	uORB::Subscription<vehicle_control_mode_s> _sub_control_mode;
 	uORB::Subscription<vehicle_attitude_s> _sub_att;
-	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<parameter_update_s> _sub_param_update;
@@ -302,6 +308,9 @@ private:
 	BlockStats<float, n_y_mocap> _mocapStats;
 	BlockStats<double, n_y_gps> _gpsStats;
 
+	// low pass
+	BlockLowPassVector<float, n_x> _xLowPass;
+
 	// delay blocks
 	BlockDelay<float, n_x, 1, HIST_LEN> _xDelay;
 	BlockDelay<uint64_t, 1, 1, HIST_LEN> _tDelay;
@@ -346,9 +355,9 @@ private:
 	float _flowMeanQual;
 
 	// status
-	bool _canEstimateXY;
-	bool _canEstimateZ;
-	bool _canEstimateT;
+	bool _validXY;
+	bool _validZ;
+	bool _validTZ;
 	bool _xyTimeout;
 	bool _zTimeout;
 	bool _tzTimeout;
@@ -372,4 +381,8 @@ private:
 	Vector<float, n_x>  _x; // state vector
 	Vector<float, n_u>  _u; // input vector
 	Matrix<float, n_x, n_x>  _P; // state covariance matrix
+	Matrix<float, n_x, n_x>  _A; // dynamics matrix
+	Matrix<float, n_x, n_u>  _B; // input matrix
+	Matrix<float, n_u, n_u>  _R; // input covariance
+	Matrix<float, n_x, n_x>  _Q; // process noise covariance
 };
diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt
index 7b118a18d0..1421be5969 100644
--- a/src/modules/local_position_estimator/CMakeLists.txt
+++ b/src/modules/local_position_estimator/CMakeLists.txt
@@ -35,7 +35,7 @@ px4_add_module(
 	MAIN local_position_estimator
 	COMPILE_FLAGS -Os
 	STACK_MAIN 5700
-	STACK_MAX 10000
+	STACK_MAX 13000
 	SRCS
 		local_position_estimator_main.cpp
 		BlockLocalPositionEstimator.cpp
diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp
index 27256fc8e7..4ba3946076 100644
--- a/src/modules/local_position_estimator/local_position_estimator_main.cpp
+++ b/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,
-						 12000,
+						 13000,
 						 local_position_estimator_thread_main,
 						 (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
 		return 0;
diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index 4b85db9447..fb8fdd6172 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -88,34 +88,32 @@ PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f);
 PARAM_DEFINE_FLOAT(LPE_LDR_OFF_Z, 0.00f);
 
 /**
- * Accelerometer xy standard deviation
+ * Accelerometer xy noise density
  *
- * Data sheet sqrt(Noise power) = 150ug/sqrt(Hz)
- * std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2
- * Since accels sampled at 1000 Hz.
+ * Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz)
  *
- * should be 0.0464
+ * Larger than data sheet to account for tilt error.
  *
  * @group Local Position Estimator
- * @unit m/s^2
+ * @unit m/s^2/srqt(Hz)
  * @min 0.00001
  * @max 2
  * @decimal 4
  */
-PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f);
+PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0015f);
 
 /**
- * Accelerometer z standard deviation
+ * Accelerometer z noise density
  *
- * (see Accel x comments)
+ * Data sheet noise density = 150ug/sqrt(Hz) = 0.0015 m/s^2/sqrt(Hz)
  *
  * @group Local Position Estimator
- * @unit m/s^2
+ * @unit m/s^2/srqt(Hz)
  * @min 0.00001
  * @max 2
  * @decimal 4
  */
-PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f);
+PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0015f);
 
 /**
  * Barometric presssure altitude z standard deviation.
@@ -145,11 +143,11 @@ PARAM_DEFINE_INT32(LPE_GPS_ON, 1);
  * @max 0.4
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
+PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.29f);
 
 
 /**
- * GPS xy standard deviation.
+ * Minimum GPS xy standard deviation, uses reported EPH if greater.
  *
  * @group Local Position Estimator
  * @unit m
@@ -157,10 +155,10 @@ PARAM_DEFINE_FLOAT(LPE_GPS_DELAY, 0.25f);
  * @max 5
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
+PARAM_DEFINE_FLOAT(LPE_GPS_XY, 1.0f);
 
 /**
- * GPS z standard deviation.
+ * Minimum GPS z standard deviation, uses reported EPV if greater.
  *
  * @group Local Position Estimator
  * @unit m
@@ -168,10 +166,11 @@ PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f);
  * @max 200
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(LPE_GPS_Z, 100.0f);
+PARAM_DEFINE_FLOAT(LPE_GPS_Z, 3.0f);
 
 /**
  * GPS xy velocity standard deviation.
+ * EPV used if greater than this value.
  *
  * @group Local Position Estimator
  * @unit m/s
@@ -193,7 +192,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.25f);
 PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
 
 /**
- * GPS max eph
+ * Max EPH allowed for GPS initialization
  *
  * @group Local Position Estimator
  * @unit m
@@ -204,7 +203,7 @@ PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.25f);
 PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
 
 /**
- * GPS max epv
+ * Max EPV allowed for GPS initialization
  *
  * @group Local Position Estimator
  * @unit m
@@ -258,24 +257,30 @@ PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f);
 /**
  * Position propagation noise density
  *
+ * Increase to trust measurements more.
+ * Decrease to trust model more.
+ *
  * @group Local Position Estimator
  * @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 noise density
  *
+ * Increase to trust measurements more.
+ * Decrease to trust model more.
+ *
  * @group Local Position Estimator
  * @unit (m/s)/s/sqrt(Hz)
  * @min 0
  * @max 1
  * @decimal 8
  */
-PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f);
+PARAM_DEFINE_FLOAT(LPE_PN_V, 0.1f);
 
 /**
  * Accel bias propagation noise density
@@ -332,3 +337,14 @@ PARAM_DEFINE_FLOAT(LPE_LAT, 40.430f);
  */
 PARAM_DEFINE_FLOAT(LPE_LON, -86.929);
 
+/**
+ * Cut frequency for state publication
+ *
+ * @group Local Position Estimator
+ * @unit Hz
+ * @min 5
+ * @max 1000
+ * @decimal 0
+ */
+PARAM_DEFINE_FLOAT(LPE_X_LP, 5.0);
+
diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp
index 351a134696..57fc87eb5f 100644
--- a/src/modules/local_position_estimator/sensors/baro.cpp
+++ b/src/modules/local_position_estimator/sensors/baro.cpp
@@ -76,8 +76,8 @@ void BlockLocalPositionEstimator::baroCorrect()
 
 	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));
+			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;
 		}
 
@@ -90,14 +90,7 @@ void BlockLocalPositionEstimator::baroCorrect()
 	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;
-		}
-
+		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp
index 31f29a1487..879da54b0a 100644
--- a/src/modules/local_position_estimator/sensors/flow.cpp
+++ b/src/modules/local_position_estimator/sensors/flow.cpp
@@ -49,7 +49,7 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
 	}
 
 	// calculate range to center of image for flow
-	if (!_canEstimateT) {
+	if (!_validTZ) {
 		return -1;
 	}
 
@@ -153,7 +153,9 @@ void BlockLocalPositionEstimator::flowCorrect()
 	if (_flowFault < fault_lvl_disable) {
 		Matrix<float, n_x, n_y_flow> K =
 			_P * C.transpose() * S_I;
-		_x += K * r;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 
 	} else {
diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp
index 617849b50f..e08119dddb 100644
--- a/src/modules/local_position_estimator/sensors/gps.cpp
+++ b/src/modules/local_position_estimator/sensors/gps.cpp
@@ -8,11 +8,25 @@ extern orb_advert_t mavlink_log_pub;
 // to initialize
 static const uint32_t 		REQ_GPS_INIT_COUNT = 10;
 static const uint32_t 		GPS_TIMEOUT =      1000000; // 1.0 s
-static const float 			GPS_EPH_MIN =      2.0; // m, min allowed by gps self reporting
-static const float 			GPS_EPV_MIN =      2.0; // m, min allowed by gps self reporting
 
 void BlockLocalPositionEstimator::gpsInit()
 {
+	// check for good gps signal
+	uint8_t nSat = _sub_gps.get().satellites_used;
+	float eph = _sub_gps.get().eph;
+	float epv = _sub_gps.get().epv;
+	uint8_t fix_type = _sub_gps.get().fix_type;
+
+	if (
+		nSat < 6 ||
+		eph > _gps_eph_max.get() ||
+		epv > _gps_epv_max.get() ||
+		fix_type < 3
+	) {
+		_gpsStats.reset();
+		return;
+	}
+
 	// measure
 	Vector<double, n_y_gps> y;
 
@@ -50,21 +64,6 @@ void BlockLocalPositionEstimator::gpsInit()
 
 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;
-	float epv = _sub_gps.get().epv;
-	uint8_t fix_type = _sub_gps.get().fix_type;
-
-	if (
-		nSat < 6 ||
-		eph > _gps_eph_max.get() ||
-		epv > _gps_epv_max.get() ||
-		fix_type < 3
-	) {
-		return -1;
-	}
-
 	// gps measurement
 	y.setZero();
 	y(0) = _sub_gps.get().lat * 1e-7;
@@ -125,11 +124,11 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
 
 	// if field is not below minimum, set it to the value provided
-	if (_sub_gps.get().eph > GPS_EPH_MIN) {
+	if (_sub_gps.get().eph > _gps_xy_stddev.get()) {
 		var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
 	}
 
-	if (_sub_gps.get().epv > GPS_EPV_MIN) {
+	if (_sub_gps.get().epv > _gps_z_stddev.get()) {
 		var_z = _sub_gps.get().epv * _sub_gps.get().epv;
 	}
 
@@ -170,13 +169,9 @@ void BlockLocalPositionEstimator::gpsCorrect()
 
 	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)));
+			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
+						     double(r(0)*r(0) / S_I(0, 0)),  double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
+						     double(r(3)*r(3) / S_I(3, 3)),  double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
 			_gpsFault = FAULT_MINOR;
 		}
 
@@ -188,7 +183,9 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	// 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;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 	}
 }
diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp
index ecbdb4d733..67f623e946 100644
--- a/src/modules/local_position_estimator/sensors/lidar.cpp
+++ b/src/modules/local_position_estimator/sensors/lidar.cpp
@@ -109,14 +109,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
 	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;
-		}
-
+		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp
index f3c26e4fde..8319fc403f 100644
--- a/src/modules/local_position_estimator/sensors/mocap.cpp
+++ b/src/modules/local_position_estimator/sensors/mocap.cpp
@@ -98,7 +98,9 @@ void BlockLocalPositionEstimator::mocapCorrect()
 	// 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;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 	}
 }
diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp
index d95ad5e346..e78dc45b0e 100644
--- a/src/modules/local_position_estimator/sensors/sonar.cpp
+++ b/src/modules/local_position_estimator/sensors/sonar.cpp
@@ -127,14 +127,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
 		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;
-		}
-
+		correctionLogic(dx);
 		_x += dx;
 		_P -= K * C * _P;
 	}
diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp
index 1b3c7ddbca..4a4fa6a2d4 100644
--- a/src/modules/local_position_estimator/sensors/vision.cpp
+++ b/src/modules/local_position_estimator/sensors/vision.cpp
@@ -96,7 +96,9 @@ void BlockLocalPositionEstimator::visionCorrect()
 	// 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;
+		Vector<float, n_x> dx = K * r;
+		correctionLogic(dx);
+		_x += dx;
 		_P -= K * C * _P;
 	}
 }
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 7543560eac..cd17ae121c 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -2138,6 +2138,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2);
 				log_msg.body.log_EST2.gps_check_fail_flags = buf.estimator_status.gps_check_fail_flags;
 				log_msg.body.log_EST2.control_mode_flags = buf.estimator_status.control_mode_flags;
+				log_msg.body.log_EST2.health_flags = buf.estimator_status.health_flags;
 				LOGBUFFER_WRITE_AND_COUNT(EST2);
 
 				log_msg.msg_type = LOG_EST3_MSG;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index a6a253f921..58ae693b64 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -410,9 +410,10 @@ struct log_EST1_s {
 /* --- EST2 - ESTIMATOR STATUS --- */
 #define LOG_EST2_MSG 34
 struct log_EST2_s {
-    float cov[12];
-    uint16_t gps_check_fail_flags;
-    uint16_t control_mode_flags;
+	float cov[12];
+	uint16_t gps_check_fail_flags;
+	uint16_t control_mode_flags;
+	uint8_t health_flags;
 };
 
 /* --- EST3 - ESTIMATOR STATUS --- */
@@ -687,7 +688,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
 	LOG_FORMAT(EST0, "ffffffffffffBBHB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fFault,fTOut"),
 	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
-	LOG_FORMAT(EST2, "ffffffffffffHH",    "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL"),
+	LOG_FORMAT(EST2, "ffffffffffffHHB",    "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,GCHK,CTRL,fHealth"),
 	LOG_FORMAT(EST3, "ffffffffffffffff",    "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),
 	LOG_FORMAT(EST4, "ffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV"),
 	LOG_FORMAT(EST5, "ffffffffff", "MAGxI,MAGyI,MAGzI,MAGxIV,MAGyIV,MAGzIV,HeadI,HeadIV,AirI,AirIV"),