Merge pull request #1873 from Zefz/ekf-dead-reckoning-fix

AttPosEKF dead reckoning fix

4 files changed, 257 insertions, 185 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index f832f761e..cd1db4e51 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -133,7 +133,7 @@ extern struct system_load_s system_load;
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
-#define POSITION_TIMEOUT		(2 * 1000 * 1000)	/**< consider the local or global position estimate invalid after 600ms */
+#define POSITION_TIMEOUT		(1 * 1000 * 1000)	/**< consider the local or global position estimate invalid after 1000ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
index 228ffa853..ec9efe8ee 100644
--- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -143,6 +143,7 @@ private:
     int     _mission_sub;
     int     _home_sub;          /**< home position as defined by commander / user */
     int     _landDetectorSub;
+    int     _armedSub;
 
     orb_advert_t    _att_pub;           /**< vehicle attitude */
     orb_advert_t    _global_pos_pub;        /**< global position */
@@ -163,6 +164,7 @@ private:
     struct wind_estimate_s              _wind;          /**< wind estimate */
     struct range_finder_report          _distance;      /**< distance estimate */
     struct vehicle_land_detected_s      _landDetector;
+    struct actuator_armed_s             _armed;
 
     struct gyro_scale               _gyro_offsets[3];
     struct accel_scale              _accel_offsets[3];
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 903158129..aad3aa43d 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -96,13 +96,13 @@ uint64_t getMicros()
 namespace estimator
 {
 
-	/* oddly, ERROR is not defined for c++ */
-	#ifdef ERROR
-	# undef ERROR
-	#endif
-	static const int ERROR = -1;
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
-	AttitudePositionEstimatorEKF	*g_estimator = nullptr;
+AttitudePositionEstimatorEKF	*g_estimator = nullptr;
 }
 
 AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
@@ -110,7 +110,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_task_running(false),
 	_estimator_task(-1),
 
-	/* subscriptions */
+/* subscriptions */
 	_sensor_combined_sub(-1),
 	_distance_sub(-1),
 	_airspeed_sub(-1),
@@ -122,8 +122,9 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_mission_sub(-1),
 	_home_sub(-1),
 	_landDetectorSub(-1),
+	_armedSub(-1),
 
-	/* publications */
+/* publications */
 	_att_pub(-1),
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
@@ -131,71 +132,72 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_wind_pub(-1),
 
 	_att({}),
-	_gyro({}),
-	_accel({}),
-	_mag({}),
-	_airspeed({}),
-	_baro({}),
-	_vstatus({}),
-	_global_pos({}),
-	_local_pos({}),
-	_gps({}),
-	_wind({}),
-	_distance{},
-	_landDetector{},
-
-	_gyro_offsets({}),
-	_accel_offsets({}),
-	_mag_offsets({}),
-
-	_sensor_combined{},
-
-	_pos_ref{},
-	_baro_ref(0.0f),
-	_baro_ref_offset(0.0f),
-	_baro_gps_offset(0.0f),
-
-	/* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
-	_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
-	_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
-	_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
-	_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
-	_perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
-	_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
-	_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
-
-	/* states */
-	_gps_alt_filt(0.0f),
-	_baro_alt_filt(0.0f),
-	_covariancePredictionDt(0.0f),
-	_gpsIsGood(false),
-	_previousGPSTimestamp(0),
-	_baro_init(false),
-	_gps_initialized(false),
-	_filter_start_time(0),
-	_last_sensor_timestamp(0),
-	_last_run(0),
-	_distance_last_valid(0),
-	_gyro_valid(false),
-	_accel_valid(false),
-	_mag_valid(false),
-	_gyro_main(0),
-	_accel_main(0),
-	_mag_main(0),
-	_ekf_logging(true),
-	_debug(0),
-
-	_newDataGps(false),
-	_newHgtData(false),
-	_newAdsData(false),
-	_newDataMag(false),
-	_newRangeData(false),
-
-	_mavlink_fd(-1),
-	_parameters{},
-	_parameter_handles{},
-	_ekf(nullptr)
+     _gyro({}),
+     _accel({}),
+     _mag({}),
+     _airspeed({}),
+     _baro({}),
+     _vstatus({}),
+     _global_pos({}),
+     _local_pos({}),
+     _gps({}),
+     _wind({}),
+     _distance {},
+     _landDetector {},
+     _armed {},
+
+     _gyro_offsets({}),
+     _accel_offsets({}),
+     _mag_offsets({}),
+
+     _sensor_combined {},
+
+     _pos_ref {},
+     _baro_ref(0.0f),
+     _baro_ref_offset(0.0f),
+     _baro_gps_offset(0.0f),
+
+     /* performance counters */
+     _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
+     _loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
+     _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
+     _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
+     _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
+     _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")),
+     _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
+     _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
+
+     /* states */
+     _gps_alt_filt(0.0f),
+     _baro_alt_filt(0.0f),
+     _covariancePredictionDt(0.0f),
+     _gpsIsGood(false),
+     _previousGPSTimestamp(0),
+     _baro_init(false),
+     _gps_initialized(false),
+     _filter_start_time(0),
+     _last_sensor_timestamp(0),
+     _last_run(0),
+     _distance_last_valid(0),
+     _gyro_valid(false),
+     _accel_valid(false),
+     _mag_valid(false),
+     _gyro_main(0),
+     _accel_main(0),
+     _mag_main(0),
+     _ekf_logging(true),
+     _debug(0),
+
+     _newDataGps(false),
+     _newHgtData(false),
+     _newAdsData(false),
+     _newDataMag(false),
+     _newRangeData(false),
+
+     _mavlink_fd(-1),
+     _parameters {},
+     _parameter_handles {},
+     _ekf(nullptr)
 {
 	_last_run = hrt_absolute_time();
 
@@ -334,7 +336,7 @@ int AttitudePositionEstimatorEKF::parameters_update()
 		_ekf->posDSigma = _parameters.posd_noise;
 		_ekf->magMeasurementSigma = _parameters.mag_noise;
 		_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
-        	_ekf->accelProcessNoise = _parameters.acc_pnoise;
+		_ekf->accelProcessNoise = _parameters.acc_pnoise;
 		_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
 		_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
 	}
@@ -365,14 +367,15 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 
 	int check = _ekf->CheckAndBound(&ekf_report);
 
-	const char* const feedback[] = { 0,
-					"NaN in states, resetting",
-					"stale sensor data, resetting",
-					"got initial position lock",
-					"excessive gyro offsets",
-					"velocity diverted, check accel config",
-					"excessive covariances",
-					"unknown condition, resetting"};
+	const char *const feedback[] = { 0,
+					 "NaN in states, resetting",
+					 "stale sensor data, resetting",
+					 "got initial position lock",
+					 "excessive gyro offsets",
+					 "velocity diverted, check accel config",
+					 "excessive covariances",
+					 "unknown condition, resetting"
+				       };
 
 	// Print out error condition
 	if (check) {
@@ -391,6 +394,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 	}
 
 	struct estimator_status_report rep;
+
 	memset(&rep, 0, sizeof(rep));
 
 	// If error flag is set, we got a filter reset
@@ -433,18 +437,18 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 
 			if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) {
 				warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s",
-					((rep.health_flags & (1 << 0)) ? "OK" : "ERR"),
-					((rep.health_flags & (1 << 1)) ? "OK" : "ERR"),
-					((rep.health_flags & (1 << 2)) ? "OK" : "ERR"),
-					((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
+				      ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"),
+				      ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"),
+				      ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"),
+				      ((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
 			}
 
 			if (rep.timeout_flags) {
 				warnx("timeout: %s%s%s%s",
-					((rep.timeout_flags & (1 << 0)) ? "VEL " : ""),
-					((rep.timeout_flags & (1 << 1)) ? "POS " : ""),
-					((rep.timeout_flags & (1 << 2)) ? "HGT " : ""),
-					((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
+				      ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""),
+				      ((rep.timeout_flags & (1 << 1)) ? "POS " : ""),
+				      ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""),
+				      ((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
 			}
 		}
 
@@ -461,6 +465,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 
 		if (_estimator_status_pub > 0) {
 			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
+
 		} else {
 			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
 		}
@@ -497,6 +502,7 @@ void AttitudePositionEstimatorEKF::task_main()
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_sub = orb_subscribe(ORB_ID(home_position));
 	_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
+	_armedSub = orb_subscribe(ORB_ID(actuator_armed));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
@@ -530,8 +536,9 @@ void AttitudePositionEstimatorEKF::task_main()
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -590,7 +597,7 @@ void AttitudePositionEstimatorEKF::task_main()
 			/**
 			 *    PART ONE: COLLECT ALL DATA
 			 **/
-			 pollData();
+			pollData();
 
 			/*
 			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
@@ -628,10 +635,10 @@ void AttitudePositionEstimatorEKF::task_main()
 				/* Initialize the filter first */
 				if (!_gps_initialized && _gpsIsGood) {
 					initializeGPS();
-				} 
-				else if (!_ekf->statesInitialised) {
+
+				} else if (!_ekf->statesInitialised) {
 					// North, East Down position (m)
-					float initVelNED[3] = {0.0f, 0.0f, 0.0f}; 
+					float initVelNED[3] = {0.0f, 0.0f, 0.0f};
 
 					_ekf->posNE[0] = 0.0f;
 					_ekf->posNE[1] = 0.0f;
@@ -643,8 +650,7 @@ void AttitudePositionEstimatorEKF::task_main()
 
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 
-				} 
-				else if (_ekf->statesInitialised) {
+				} else if (_ekf->statesInitialised) {
 
 					// Check if on ground - status is used by covariance prediction
 					_ekf->setOnGround(_landDetector.landed);
@@ -652,6 +658,7 @@ void AttitudePositionEstimatorEKF::task_main()
 					// We're apparently initialized in this case now
 					// check (and reset the filter as needed)
 					int check = check_filter_state();
+
 					if (check) {
 						// Let the system re-initialize itself
 						continue;
@@ -667,8 +674,7 @@ void AttitudePositionEstimatorEKF::task_main()
 					publishLocalPosition();
 
 					//Publish Global Position, but only if it's any good
-					if(_gps_initialized && _gpsIsGood)
-					{
+					if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) {
 						publishGlobalPosition();
 					}
 
@@ -736,12 +742,14 @@ void AttitudePositionEstimatorEKF::initializeGPS()
 	map_projection_init(&_pos_ref, lat, lon);
 	mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
 
-	#if 0
+#if 0
 	warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
-		(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
-	warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset);
-	warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
-	#endif
+	      (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
+	warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref,
+	      (double)_baro_ref_offset);
+	warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv,
+	      (double)math::degrees(declination));
+#endif
 
 	_gps_initialized = true;
 }
@@ -840,6 +848,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	if (_local_pos.v_xy_valid) {
 		_global_pos.vel_n = _local_pos.vx;
 		_global_pos.vel_e = _local_pos.vy;
+
 	} else {
 		_global_pos.vel_n = 0.0f;
 		_global_pos.vel_e = 0.0f;
@@ -855,7 +864,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	/* terrain altitude */
 	_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
 	_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
-		(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
+					(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
 
 	_global_pos.yaw = _local_pos.yaw;
 	_global_pos.eph = _gps.eph;
@@ -895,8 +904,9 @@ void AttitudePositionEstimatorEKF::publishWindEstimate()
 
 }
 
-void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, 
-	const bool fuseBaro, const bool fuseAirSpeed)
+void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const bool fuseMag,
+		const bool fuseRangeSensor,
+		const bool fuseBaro, const bool fuseAirSpeed)
 {
 	// Run the strapdown INS equations every IMU update
 	_ekf->UpdateStrapdownEquationsNED();
@@ -911,8 +921,8 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 
 	// perform a covariance prediction if the total delta angle has exceeded the limit
 	// or the time limit will be exceeded at the next IMU update
-	if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) 
-		|| (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
+	if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU))
+	    || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
 		_ekf->CovariancePrediction(_covariancePredictionDt);
 		_ekf->summedDelAng.zero();
 		_ekf->summedDelVel.zero();
@@ -934,8 +944,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 		// run the fusion step
 		_ekf->FuseVelposNED();
 
-	} 
-	else if (!_gps_initialized) {
+	} else if (!_gps_initialized) {
 
 		// force static mode
 		_ekf->staticMode = true;
@@ -959,8 +968,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 		// run the fusion step
 		_ekf->FuseVelposNED();
 
-	} 
-	else {
+	} else {
 		_ekf->fuseVelData = false;
 		_ekf->fusePosData = false;
 	}
@@ -976,34 +984,33 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 		// run the fusion step
 		_ekf->FuseVelposNED();
 
-	} 
-	else {
+	} else {
 		_ekf->fuseHgtData = false;
 	}
 
 	// Fuse Magnetometer Measurements
 	if (fuseMag) {
 		_ekf->fuseMagData = true;
-		_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
+		_ekf->RecallStates(_ekf->statesAtMagMeasTime,
+				   (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
 
 		_ekf->magstate.obsIndex = 0;
 		_ekf->FuseMagnetometer();
 		_ekf->FuseMagnetometer();
 		_ekf->FuseMagnetometer();
 
-	} 
-	else {
+	} else {
 		_ekf->fuseMagData = false;
 	}
 
 	// Fuse Airspeed Measurements
 	if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) {
 		_ekf->fuseVtasData = true;
-		_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+		_ekf->RecallStates(_ekf->statesAtVtasMeasTime,
+				   (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
 		_ekf->FuseAirspeed();
 
-	} 
-	else {
+	} else {
 		_ekf->fuseVtasData = false;
 	}
 
@@ -1061,46 +1068,67 @@ void AttitudePositionEstimatorEKF::print_status()
 
 	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
 	printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
-	printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset);
-	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
-	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
-	printf("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
-	printf("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
-	printf("states (pos m)      [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
-	printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
+	printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset,
+	       (double)_baro_gps_offset);
+	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y,
+	       (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
+	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y,
+	       (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
+	       (double)_ekf->correctedDelAng.z);
+	printf("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1],
+	       (double)_ekf->states[2], (double)_ekf->states[3]);
+	printf("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5],
+	       (double)_ekf->states[6]);
+	printf("states (pos m)      [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8],
+	       (double)_ekf->states[9]);
+	printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11],
+	       (double)_ekf->states[12]);
 
 	if (EKF_STATE_ESTIMATES == 23) {
 		printf("states (accel offs)   [13]: %8.4f\n", (double)_ekf->states[13]);
 		printf("states (wind)      [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]);
-		printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]);
-		printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]);
+		printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17],
+		       (double)_ekf->states[18]);
+		printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20],
+		       (double)_ekf->states[21]);
 		printf("states (terrain)      [22]: %8.4f\n", (double)_ekf->states[22]);
 
 	} else {
 		printf("states (wind)      [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
-		printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
-		printf("states (body mag)  [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
+		printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16],
+		       (double)_ekf->states[17]);
+		printf("states (body mag)  [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19],
+		       (double)_ekf->states[20]);
 	}
+
 	printf("states: %s %s %s %s %s %s %s %s %s %s\n",
-		       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
-		       (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE",
-		       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
-		       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
-		       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
-		       (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
-		       (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
-		       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
-		       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
-		       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
+	       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
+	       (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE",
+	       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
+	       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
+	       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
+	       (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
+	       (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
+	       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
+	       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
+	       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
 }
 
 void AttitudePositionEstimatorEKF::pollData()
 {
+	//Update arming status
+	bool armedUpdate;
+	orb_check(_armedSub, &armedUpdate);
+
+	if (armedUpdate) {
+		orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
+	}
+
+	//Update Gyro and Accelerometer
 	static Vector3f lastAngRate;
 	static Vector3f lastAccel;
 	bool accel_updated = false;
 
-	//Update Gyro and Accelerometer
 	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
 	static hrt_abstime last_accel = 0;
@@ -1108,6 +1136,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	if (last_accel != _sensor_combined.accelerometer_timestamp) {
 		accel_updated = true;
+
 	} else {
 		accel_updated = false;
 	}
@@ -1136,9 +1165,9 @@ void AttitudePositionEstimatorEKF::pollData()
 	int last_gyro_main = _gyro_main;
 
 	if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
-		isfinite(_sensor_combined.gyro_rad_s[1]) &&
-		isfinite(_sensor_combined.gyro_rad_s[2]) &&
-		(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
+	    isfinite(_sensor_combined.gyro_rad_s[1]) &&
+	    isfinite(_sensor_combined.gyro_rad_s[2]) &&
+	    (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
 
 		_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
 		_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
@@ -1147,8 +1176,8 @@ void AttitudePositionEstimatorEKF::pollData()
 		_gyro_valid = true;
 
 	} else if (isfinite(_sensor_combined.gyro1_rad_s[0]) &&
-		isfinite(_sensor_combined.gyro1_rad_s[1]) &&
-		isfinite(_sensor_combined.gyro1_rad_s[2])) {
+		   isfinite(_sensor_combined.gyro1_rad_s[1]) &&
+		   isfinite(_sensor_combined.gyro1_rad_s[2])) {
 
 		_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
 		_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
@@ -1167,6 +1196,7 @@ void AttitudePositionEstimatorEKF::pollData()
 	if (!_gyro_valid) {
 		/* keep last value if he hit an out of band value */
 		lastAngRate = _ekf->angRate;
+
 	} else {
 		perf_count(_perf_gyro);
 	}
@@ -1181,6 +1211,7 @@ void AttitudePositionEstimatorEKF::pollData()
 			_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
 			_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
 			_accel_main = 0;
+
 		} else {
 			_ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0];
 			_ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1];
@@ -1218,12 +1249,14 @@ void AttitudePositionEstimatorEKF::pollData()
 	//Update Land Detector
 	bool newLandData;
 	orb_check(_landDetectorSub, &newLandData);
-	if(newLandData) {
+
+	if (newLandData) {
 		orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector);
 	}
 
 	//Update AirSpeed
 	orb_check(_airspeed_sub, &_newAdsData);
+
 	if (_newAdsData) {
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
 		perf_count(_perf_airspeed);
@@ -1233,6 +1266,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 
 	orb_check(_gps_sub, &_newDataGps);
+
 	if (_newDataGps) {
 
 		orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
@@ -1240,57 +1274,64 @@ void AttitudePositionEstimatorEKF::pollData()
 
 		//We are more strict for our first fix
 		float requiredAccuracy = _parameters.pos_stddev_threshold;
-		if(_gpsIsGood) {
+
+		if (_gpsIsGood) {
 			requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f;
 		}
 
 		//Check if the GPS fix is good enough for us to use
-		if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) {
+		if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) {
 			_gpsIsGood = true;
-		}
-		else {
+
+		} else {
 			_gpsIsGood = false;
 		}
 
 		if (_gpsIsGood) {
 
 			//Calculate time since last good GPS fix
-			const float dtGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
+			const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
 
-			_ekf->updateDtGpsFilt(math::constrain(dtGoodGPS, 0.01f, POS_RESET_THRESHOLD));
+			//Stop dead-reckoning mode
+			if (_global_pos.dead_reckoning) {
+				mavlink_log_info(_mavlink_fd, "[ekf] stop dead-reckoning");
+			}
 
-			/* fuse GPS updates */
+			_global_pos.dead_reckoning = false;
 
-			//_gps.timestamp / 1e3;
+			//Fetch new GPS data
 			_ekf->GPSstatus = _gps.fix_type;
 			_ekf->velNED[0] = _gps.vel_n_m_s;
 			_ekf->velNED[1] = _gps.vel_e_m_s;
 			_ekf->velNED[2] = _gps.vel_d_m_s;
 
-			// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
-
 			_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
 			_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
 			_ekf->gpsHgt = _gps.alt / 1e3f;
 
+			if (_previousGPSTimestamp != 0) {
+				//Calculate average time between GPS updates
+				_ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD));
+
+				// update LPF
+				_gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
+			}
+
 			//check if we had a GPS outage for a long time
-			if(_gps_initialized) {
+			if (_gps_initialized) {
 
 				//Convert from global frame to local frame
-				float posNED[3] = {0.0f, 0.0f, 0.0f}; 
+				float posNED[3] = {0.0f, 0.0f, 0.0f};
 				_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
 				_ekf->posNE[0] = posNED[0];
 				_ekf->posNE[1] = posNED[1];
 
-				if (dtGoodGPS > POS_RESET_THRESHOLD) {
+				if (dtLastGoodGPS > POS_RESET_THRESHOLD) {
 					_ekf->ResetPosition();
 					_ekf->ResetVelocity();
 				}
 			}
 
-			// update LPF
-			_gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
-
 			//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS);
 
 			// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
@@ -1308,8 +1349,8 @@ void AttitudePositionEstimatorEKF::pollData()
 			// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
 
 			_previousGPSTimestamp = _gps.timestamp_position;
-		}
-		else {
+
+		} else {
 			//Too poor GPS fix to use
 			_newDataGps = false;
 		}
@@ -1317,8 +1358,30 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	// If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update,
 	// then something is very wrong with the GPS (possibly a hardware failure or comlink error)
-	else if( (static_cast<float>(hrt_elapsed_time(&_gps.timestamp_position)) / 1e6f) > POS_RESET_THRESHOLD) {
+	const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
+
+	if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {
+
+		if (_global_pos.dead_reckoning) {
+			mavlink_log_info(_mavlink_fd, "[ekf] gave up dead-reckoning after long timeout");
+		}
+
 		_gpsIsGood = false;
+		_global_pos.dead_reckoning = false;
+	}
+
+	//If we have no good GPS fix for half a second, then enable dead-reckoning mode while armed (for up to POS_RESET_THRESHOLD seconds)
+	else if (dtLastGoodGPS >= 0.5f) {
+		if (_armed.armed) {
+			if (!_global_pos.dead_reckoning) {
+				mavlink_log_info(_mavlink_fd, "[ekf] dead-reckoning enabled");
+			}
+
+			_global_pos.dead_reckoning = true;
+
+		} else {
+			_global_pos.dead_reckoning = false;
+		}
 	}
 
 	//Update barometer
@@ -1331,18 +1394,20 @@ void AttitudePositionEstimatorEKF::pollData()
 
 		// init lowpass filters for baro and gps altitude
 		float baro_elapsed;
-		if(baro_last == 0) {
+
+		if (baro_last == 0) {
 			baro_elapsed = 0.0f;
-		}
-		else {
+
+		} else {
 			baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
 		}
+
 		baro_last = _baro.timestamp;
 
 		_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
 
 		_ekf->baroHgt = _baro.altitude;
-		_baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
+		_baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
 
 		if (!_baro_init) {
 			_baro_ref = _baro.altitude;
@@ -1377,6 +1442,7 @@ void AttitudePositionEstimatorEKF::pollData()
 			_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
 			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
 			_mag_main = 0;
+
 		} else {
 			_ekf->magData.x = _sensor_combined.magnetometer1_ga[0];
 			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
@@ -1396,31 +1462,30 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	//Update range data
 	orb_check(_distance_sub, &_newRangeData);
+
 	if (_newRangeData) {
 		orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
 
 		if (_distance.valid) {
 			_ekf->rngMea = _distance.distance;
 			_distance_last_valid = _distance.timestamp;
+
 		} else {
 			_newRangeData = false;
 		}
 	}
 }
 
-int AttitudePositionEstimatorEKF::trip_nan() {
+int AttitudePositionEstimatorEKF::trip_nan()
+{
 
 	int ret = 0;
 
 	// If system is not armed, inject a NaN value into the filter
-	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-
-	struct actuator_armed_s armed;
-	orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-
-	if (armed.armed) {
+	if (_armed.armed) {
 		warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM");
 		ret = 1;
+
 	} else {
 
 		float nan_val = 0.0f / 0.0f;
@@ -1453,24 +1518,26 @@ int AttitudePositionEstimatorEKF::trip_nan() {
 		print_status();
 	}
 
-	close(armed_sub);
 	return ret;
 }
 
 int ekf_att_pos_estimator_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (estimator::g_estimator != nullptr)
+		if (estimator::g_estimator != nullptr) {
 			errx(1, "already running");
+		}
 
 		estimator::g_estimator = new AttitudePositionEstimatorEKF();
 
-		if (estimator::g_estimator == nullptr)
+		if (estimator::g_estimator == nullptr) {
 			errx(1, "alloc failed");
+		}
 
 		if (OK != estimator::g_estimator->start()) {
 			delete estimator::g_estimator;
@@ -1484,14 +1551,16 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 			printf(".");
 			fflush(stdout);
 		}
+
 		printf("\n");
 
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (estimator::g_estimator == nullptr)
+		if (estimator::g_estimator == nullptr) {
 			errx(1, "not running");
+		}
 
 		delete estimator::g_estimator;
 		estimator::g_estimator = nullptr;
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index 137c86dd5..50c942961 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -74,6 +74,7 @@ struct vehicle_global_position_s {
 	float epv;			/**< Standard deviation of position vertically */
 	float terrain_alt;		/**< Terrain altitude in m, WGS84 */
 	bool terrain_alt_valid;		/**< Terrain altitude estimate is valid */
+	bool dead_reckoning;		/**< True if this position is estimated through dead-reckoning*/
 };
 
 /**