Merge pull request #1794 from PX4/ekf-fixes

EKF Fixes from @Zefz

9 files changed, 1562 insertions, 1360 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index f2e72dd0c..242d8a486 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1388,30 +1388,24 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
 		}
 
-		/* update condition_global_position_valid */
-		/* hysteresis for EPH/EPV */
-		bool eph_good;
-
-		if (status.condition_global_position_valid) {
-			if (global_position.eph > eph_threshold * 2.5f) {
-				eph_good = false;
-
-			} else {
-				eph_good = true;
+		//update condition_global_position_valid
+		//Global positions are only published by the estimators if they are valid
+		if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
+			//We have had no good fix for POSITION_TIMEOUT amount of time
+			if(status.condition_global_position_valid) {
+				set_tune_override(TONE_GPS_WARNING_TUNE);
+				status_changed = true;
+				status.condition_global_position_valid = false;		
 			}
-
-		} else {
-			if (global_position.eph < eph_threshold) {
-				eph_good = true;
-
-			} else {
-				eph_good = false;
+		}
+		else if(global_position.timestamp != 0) {
+			//Got good global position estimate
+			if(!status.condition_global_position_valid) {
+				status_changed = true;
+				status.condition_global_position_valid = true;				
 			}
 		}
 
-		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
-			    &status_changed);
-
 		/* update condition_local_position_valid and condition_local_altitude_valid */
 		/* hysteresis for EPH */
 		bool local_eph_good;
@@ -2119,7 +2113,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 				/* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */
 
 			} else {
-				if (status_local->condition_local_position_valid) {
+				if (status_local->condition_global_position_valid) {
 					rgbled_set_color(RGBLED_COLOR_GREEN);
 
 				} else {
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 60e8be23e..b5ec6c4e6 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -115,6 +115,11 @@ void buzzer_deinit()
 	close(buzzer);
 }
 
+void set_tune_override(int tune)
+{
+	ioctl(buzzer, TONE_SET_ALARM, tune);
+}
+
 void set_tune(int tune)
 {
 	unsigned int new_tune_duration = tune_durations[tune];
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index 4a77fe487..cd3db7324 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -55,6 +55,7 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status);
 int buzzer_init(void);
 void buzzer_deinit(void);
 
+void set_tune_override(int tune);
 void set_tune(int tune);
 void tune_positive(bool use_buzzer);
 void tune_neutral(bool use_buzzer);
diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
new file mode 100644
index 000000000..228ffa853
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
@@ -0,0 +1,341 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file AttitudePositionEstimatorEKF.h
+ * Implementation of the attitude and position estimator. This is a PX4 wrapper around
+ * the EKF IntertialNav filter of Paul Riseborough (@see estimator_22states.cpp)
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/wind_estimate.h>
+#include <uORB/topics/sensor_combined.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
+#include <drivers/drv_range_finder.h>
+
+#include <geo/geo.h>
+#include <systemlib/perf_counter.h>
+
+//Forward declaration
+class AttPosEKF;
+
+class AttitudePositionEstimatorEKF
+{
+public:
+    /**
+     * Constructor
+     */
+    AttitudePositionEstimatorEKF();
+
+    /* we do not want people ever copying this class */
+    AttitudePositionEstimatorEKF(const AttitudePositionEstimatorEKF& that) = delete;
+    AttitudePositionEstimatorEKF operator=(const AttitudePositionEstimatorEKF&) = delete;
+
+    /**
+     * Destructor, also kills the sensors task.
+     */
+    ~AttitudePositionEstimatorEKF();
+
+    /**
+     * Start the sensors task.
+     *
+     * @return  OK on success.
+     */
+    int start();
+
+    /**
+     * Task status
+     *
+     * @return  true if the mainloop is running
+     */
+    bool task_running() { return _task_running; }
+
+    /**
+     * Print the current status.
+     */
+    void print_status();
+
+    /**
+     * Trip the filter by feeding it NaN values.
+     */
+    int trip_nan();
+
+    /**
+     * Enable logging.
+     *
+     * @param   enable Set to true to enable logging, false to disable
+     */
+    int enable_logging(bool enable);
+
+    /**
+     * Set debug level.
+     *
+     * @param   debug Desired debug level - 0 to disable.
+     */
+    int set_debuglevel(unsigned debug) { _debug = debug; return 0; }
+
+private:
+    bool        _task_should_exit;      /**< if true, sensor task should exit */
+    bool        _task_running;          /**< if true, task is running in its mainloop */
+    int     _estimator_task;        /**< task handle for sensor task */
+
+    int     _sensor_combined_sub;
+    int     _distance_sub;          /**< distance measurement */
+    int     _airspeed_sub;          /**< airspeed subscription */
+    int     _baro_sub;          /**< barometer subscription */
+    int     _gps_sub;           /**< GPS subscription */
+    int     _vstatus_sub;           /**< vehicle status subscription */
+    int     _params_sub;            /**< notification of parameter updates */
+    int     _manual_control_sub;        /**< notification of manual control updates */
+    int     _mission_sub;
+    int     _home_sub;          /**< home position as defined by commander / user */
+    int     _landDetectorSub;
+
+    orb_advert_t    _att_pub;           /**< vehicle attitude */
+    orb_advert_t    _global_pos_pub;        /**< global position */
+    orb_advert_t    _local_pos_pub;         /**< position in local frame */
+    orb_advert_t    _estimator_status_pub;      /**< status of the estimator */
+    orb_advert_t    _wind_pub;          /**< wind estimate */
+
+    struct vehicle_attitude_s           _att;           /**< vehicle attitude */
+    struct gyro_report                  _gyro;
+    struct accel_report                 _accel;
+    struct mag_report                   _mag;
+    struct airspeed_s                   _airspeed;      /**< airspeed */
+    struct baro_report                  _baro;          /**< baro readings */
+    struct vehicle_status_s             _vstatus;       /**< vehicle status */
+    struct vehicle_global_position_s    _global_pos;        /**< global vehicle position */
+    struct vehicle_local_position_s     _local_pos;     /**< local vehicle position */
+    struct vehicle_gps_position_s       _gps;           /**< GPS position */
+    struct wind_estimate_s              _wind;          /**< wind estimate */
+    struct range_finder_report          _distance;      /**< distance estimate */
+    struct vehicle_land_detected_s      _landDetector;
+
+    struct gyro_scale               _gyro_offsets[3];
+    struct accel_scale              _accel_offsets[3];
+    struct mag_scale                _mag_offsets[3];
+
+    struct sensor_combined_s            _sensor_combined;
+
+    struct map_projection_reference_s   _pos_ref;
+
+    float                       _baro_ref;      /**< barometer reference altitude */
+    float                       _baro_ref_offset;   /**< offset between initial baro reference and GPS init baro altitude */
+    float                       _baro_gps_offset;   /**< offset between baro altitude (at GPS init time) and GPS altitude */
+    hrt_abstime                 _last_debug_print = 0;
+
+    perf_counter_t  _loop_perf;         ///< loop performance counter
+    perf_counter_t  _loop_intvl;        ///< loop rate counter
+    perf_counter_t  _perf_gyro;         ///<local performance counter for gyro updates
+    perf_counter_t  _perf_mag;          ///<local performance counter for mag updates
+    perf_counter_t  _perf_gps;          ///<local performance counter for gps updates
+    perf_counter_t  _perf_baro;         ///<local performance counter for baro updates
+    perf_counter_t  _perf_airspeed;     ///<local performance counter for airspeed updates
+    perf_counter_t  _perf_reset;        ///<local performance counter for filter resets
+
+    float           _gps_alt_filt;
+    float           _baro_alt_filt;
+    float           _covariancePredictionDt;  ///< time lapsed since last covariance prediction
+    bool            _gpsIsGood;               ///< True if the current GPS fix is good enough for us to use
+    uint64_t        _previousGPSTimestamp;    ///< Timestamp of last good GPS fix we have received
+    bool            _baro_init;
+    bool            _gps_initialized;
+    hrt_abstime     _filter_start_time;
+    hrt_abstime     _last_sensor_timestamp;
+    hrt_abstime     _last_run;
+    hrt_abstime     _distance_last_valid;
+    bool            _gyro_valid;
+    bool            _accel_valid;
+    bool            _mag_valid;
+    int             _gyro_main;         ///< index of the main gyroscope
+    int             _accel_main;        ///< index of the main accelerometer
+    int             _mag_main;          ///< index of the main magnetometer
+    bool            _ekf_logging;       ///< log EKF state
+    unsigned        _debug;             ///< debug level - default 0
+
+    bool            _newDataGps;
+    bool            _newHgtData;
+    bool            _newAdsData;
+    bool            _newDataMag;
+    bool            _newRangeData;
+
+    int             _mavlink_fd;
+
+    struct {
+        int32_t vel_delay_ms;
+        int32_t pos_delay_ms;
+        int32_t height_delay_ms;
+        int32_t mag_delay_ms;
+        int32_t tas_delay_ms;
+        float velne_noise;
+        float veld_noise;
+        float posne_noise;
+        float posd_noise;
+        float mag_noise;
+        float gyro_pnoise;
+        float acc_pnoise;
+        float gbias_pnoise;
+        float abias_pnoise;
+        float mage_pnoise;
+        float magb_pnoise;
+        float eas_noise;
+        float pos_stddev_threshold;
+    }       _parameters;            /**< local copies of interesting parameters */
+
+    struct {
+        param_t vel_delay_ms;
+        param_t pos_delay_ms;
+        param_t height_delay_ms;
+        param_t mag_delay_ms;
+        param_t tas_delay_ms;
+        param_t velne_noise;
+        param_t veld_noise;
+        param_t posne_noise;
+        param_t posd_noise;
+        param_t mag_noise;
+        param_t gyro_pnoise;
+        param_t acc_pnoise;
+        param_t gbias_pnoise;
+        param_t abias_pnoise;
+        param_t mage_pnoise;
+        param_t magb_pnoise;
+        param_t eas_noise;
+        param_t pos_stddev_threshold;
+    }       _parameter_handles;     /**< handles for interesting parameters */
+
+    AttPosEKF                   *_ekf;
+
+private:
+    /**
+     * Update our local parameter cache.
+     */
+    int     parameters_update();
+
+    /**
+     * Update control outputs
+     *
+     */
+    void        control_update();
+
+    /**
+     * Check for changes in vehicle status.
+     */
+    void        vehicle_status_poll();
+
+    /**
+     * Shim for calling task_main from task_create.
+     */
+    static void task_main_trampoline(int argc, char *argv[]);
+
+    /**
+     * Main filter task.
+     */
+    void        task_main();
+
+    /**
+     * Check filter sanity state
+     *
+     * @return zero if ok, non-zero for a filter error condition.
+     */
+    int     check_filter_state();
+
+    /**
+    * @brief
+    *   Publish the euler and quaternions for attitude estimation
+    **/
+    void publishAttitude();
+
+    /**
+    * @brief
+    *   Publish local position relative to reference point where filter was initialized
+    **/
+    void publishLocalPosition();
+
+    /**
+    * @brief
+    *   Publish global position estimation (WSG84 and AMSL).
+    *   A global position estimate is only valid if we have a good GPS fix
+    **/
+    void publishGlobalPosition();
+
+    /**
+    * @brief
+    *   Publish wind estimates for north and east in m/s
+    **/
+    void publishWindEstimate();
+
+    /**
+    * @brief
+    *   Runs the sensor fusion step of the filter. The parameters determine which of the sensors
+    *   are fused with each other
+    **/
+    void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor, 
+            const bool fuseBaro, const bool fuseAirSpeed);
+
+    /**
+    * @brief
+    *   Initialize first time good GPS fix so we can get a reference point to calculate local position from
+    *   Should only be required to call once
+    **/
+    void initializeGPS();
+
+    /**
+    * @brief
+    *   Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate
+    *   flags to true (e.g newDataGps)
+    **/
+    void pollData();
+};
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 62965976d..1c79cb61d 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
@@ -37,8 +37,12 @@
  *
  * @author Paul Riseborough <p_riseborough@live.com.au>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Johan Jansen <jnsn.johan@gmail.com>
  */
 
+#include "AttitudePositionEstimatorEKF.h"
+#include "estimator_22states.h"
+
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -51,42 +55,22 @@
 #include <time.h>
 #include <float.h>
 
-#define SENSOR_COMBINED_SUB
-
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_accel.h>
-#include <drivers/drv_mag.h>
-#include <drivers/drv_baro.h>
-#include <drivers/drv_range_finder.h>
-#ifdef SENSOR_COMBINED_SUB
-#include <uORB/topics/sensor_combined.h>
-#endif
 #include <arch/board/board.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/airspeed.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/estimator_status.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/wind_estimate.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <geo/geo.h>
-#include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <mavlink/mavlink_log.h>
 #include <platforms/px4_defines.h>
 
-#include "estimator_22states.h"
+static uint64_t IMUmsec = 0;
+static uint64_t IMUusec = 0;
+
+//Constants
+static constexpr float rc = 10.0f;	// RC time constant of 1st order LPF in seconds
+static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; 	///< units: microseconds
+static constexpr float POS_RESET_THRESHOLD = 5.0f; 				///< Seconds before we signal a total GPS failure
 
 /**
  * estimator app start / stop handling function
@@ -99,10 +83,6 @@ __EXPORT uint32_t millis();
 
 __EXPORT uint64_t getMicros();
 
-static uint64_t IMUmsec = 0;
-static uint64_t IMUusec = 0;
-static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds
-
 uint32_t millis()
 {
 	return IMUmsec;
@@ -113,248 +93,25 @@ uint64_t getMicros()
 	return IMUusec;
 }
 
-class FixedwingEstimator
-{
-public:
-	/**
-	 * Constructor
-	 */
-	FixedwingEstimator();
-
-	/* we do not want people ever copying this class */
-	FixedwingEstimator(const FixedwingEstimator& that) = delete;
-	FixedwingEstimator operator=(const FixedwingEstimator&) = delete;
-
-	/**
-	 * Destructor, also kills the sensors task.
-	 */
-	~FixedwingEstimator();
-
-	/**
-	 * Start the sensors task.
-	 *
-	 * @return	OK on success.
-	 */
-	int		start();
-
-	/**
-	 * Task status
-	 *
-	 * @return	true if the mainloop is running
-	 */
-	bool		task_running() { return _task_running; }
-
-	/**
-	 * Print the current status.
-	 */
-	void		print_status();
-
-	/**
-	 * Trip the filter by feeding it NaN values.
-	 */
-	int		trip_nan();
-
-	/**
-	 * Enable logging.
-	 *
-	 * @param	enable Set to true to enable logging, false to disable
-	 */
-	int		enable_logging(bool enable);
-
-	/**
-	 * Set debug level.
-	 *
-	 * @param	debug Desired debug level - 0 to disable.
-	 */
-	int		set_debuglevel(unsigned debug) { _debug = debug; return 0; }
-
-private:
-
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
-	bool		_task_running;			/**< if true, task is running in its mainloop */
-	int		_estimator_task;		/**< task handle for sensor task */
-#ifndef SENSOR_COMBINED_SUB
-	int		_gyro_sub;			/**< gyro sensor subscription */
-	int		_accel_sub;			/**< accel sensor subscription */
-	int		_mag_sub;			/**< mag sensor subscription */
-#else
-	int		_sensor_combined_sub;
-#endif
-	int		_distance_sub;			/**< distance measurement */
-	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_baro_sub;			/**< barometer subscription */
-	int		_gps_sub;			/**< GPS subscription */
-	int		_vstatus_sub;			/**< vehicle status subscription */
-	int 		_params_sub;			/**< notification of parameter updates */
-	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_mission_sub;
-	int		_home_sub;			/**< home position as defined by commander / user */
-
-	orb_advert_t	_att_pub;			/**< vehicle attitude */
-	orb_advert_t	_global_pos_pub;		/**< global position */
-	orb_advert_t	_local_pos_pub;			/**< position in local frame */
-	orb_advert_t	_estimator_status_pub;		/**< status of the estimator */
-	orb_advert_t	_wind_pub;			/**< wind estimate */
-
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct gyro_report				_gyro;
-	struct accel_report				_accel;
-	struct mag_report				_mag;
-	struct airspeed_s				_airspeed;		/**< airspeed */
-	struct baro_report				_baro;			/**< baro readings */
-	struct vehicle_status_s				_vstatus;		/**< vehicle status */
-	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct vehicle_local_position_s			_local_pos;		/**< local vehicle position */
-	struct vehicle_gps_position_s			_gps;			/**< GPS position */
-	struct wind_estimate_s				_wind;			/**< wind estimate */
-	struct range_finder_report			_distance;		/**< distance estimate */
-
-	struct gyro_scale				_gyro_offsets[3];
-	struct accel_scale				_accel_offsets[3];
-	struct mag_scale				_mag_offsets[3];
-
-#ifdef SENSOR_COMBINED_SUB
-	struct sensor_combined_s			_sensor_combined;
-#endif
-
-	struct map_projection_reference_s	_pos_ref;
-
-	float						_baro_ref;		/**< barometer reference altitude */
-	float						_baro_ref_offset;	/**< offset between initial baro reference and GPS init baro altitude */
-	float						_baro_gps_offset;	/**< offset between baro altitude (at GPS init time) and GPS altitude */
-	hrt_abstime					_last_debug_print = 0;
-
-	perf_counter_t	_loop_perf;			///< loop performance counter
-	perf_counter_t	_loop_intvl;		///< loop rate counter
-	perf_counter_t	_perf_gyro;			///<local performance counter for gyro updates
-	perf_counter_t	_perf_mag;			///<local performance counter for mag updates
-	perf_counter_t	_perf_gps;			///<local performance counter for gps updates
-	perf_counter_t	_perf_baro;			///<local performance counter for baro updates
-	perf_counter_t	_perf_airspeed;			///<local performance counter for airspeed updates
-	perf_counter_t	_perf_reset;			///<local performance counter for filter resets
-
-	bool						_baro_init;
-	bool						_gps_initialized;
-	hrt_abstime					_gps_start_time;
-	hrt_abstime					_filter_start_time;
-	hrt_abstime					_last_sensor_timestamp;
-	hrt_abstime					_last_run;
-	hrt_abstime					_distance_last_valid;
-	bool						_gyro_valid;
-	bool						_accel_valid;
-	bool						_mag_valid;
-	int						_gyro_main;		///< index of the main gyroscope
-	int						_accel_main;		///< index of the main accelerometer
-	int						_mag_main;		///< index of the main magnetometer
-	bool						_ekf_logging;		///< log EKF state
-	unsigned					_debug;			///< debug level - default 0
-
-	int						_mavlink_fd;
-
-	struct {
-		int32_t	vel_delay_ms;
-		int32_t	pos_delay_ms;
-		int32_t	height_delay_ms;
-		int32_t	mag_delay_ms;
-		int32_t	tas_delay_ms;
-		float velne_noise;
-		float veld_noise;
-		float posne_noise;
-		float posd_noise;
-		float mag_noise;
-		float gyro_pnoise;
-		float acc_pnoise;
-		float gbias_pnoise;
-		float abias_pnoise;
-		float mage_pnoise;
-		float magb_pnoise;
-		float eas_noise;
-		float pos_stddev_threshold;
-	}		_parameters;			/**< local copies of interesting parameters */
-
-	struct {
-		param_t vel_delay_ms;
-		param_t	pos_delay_ms;
-		param_t	height_delay_ms;
-		param_t	mag_delay_ms;
-		param_t	tas_delay_ms;
-		param_t velne_noise;
-		param_t veld_noise;
-		param_t posne_noise;
-		param_t posd_noise;
-		param_t mag_noise;
-		param_t gyro_pnoise;
-		param_t acc_pnoise;
-		param_t gbias_pnoise;
-		param_t abias_pnoise;
-		param_t mage_pnoise;
-		param_t magb_pnoise;
-		param_t eas_noise;
-		param_t pos_stddev_threshold;
-	}		_parameter_handles;		/**< handles for interesting parameters */
-
-	AttPosEKF					*_ekf;
-
-	/**
-	 * Update our local parameter cache.
-	 */
-	int		parameters_update();
-
-	/**
-	 * Update control outputs
-	 *
-	 */
-	void		control_update();
-
-	/**
-	 * Check for changes in vehicle status.
-	 */
-	void		vehicle_status_poll();
-
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	static void	task_main_trampoline(int argc, char *argv[]);
-
-	/**
-	 * Main filter task.
-	 */
-	void		task_main();
-
-	/**
-	 * Check filter sanity state
-	 *
-	 * @return zero if ok, non-zero for a filter error condition.
-	 */
-	int		check_filter_state();
-};
-
 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;
 
-FixedwingEstimator	*g_estimator = nullptr;
+	AttitudePositionEstimatorEKF	*g_estimator = nullptr;
 }
 
-FixedwingEstimator::FixedwingEstimator() :
-
+AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_task_should_exit(false),
 	_task_running(false),
 	_estimator_task(-1),
 
-/* subscriptions */
-#ifndef SENSOR_COMBINED_SUB
-	_gyro_sub(-1),
-	_accel_sub(-1),
-	_mag_sub(-1),
-#else
+	/* subscriptions */
 	_sensor_combined_sub(-1),
-#endif
 	_distance_sub(-1),
 	_airspeed_sub(-1),
 	_baro_sub(-1),
@@ -364,8 +121,9 @@ FixedwingEstimator::FixedwingEstimator() :
 	_manual_control_sub(-1),
 	_mission_sub(-1),
 	_home_sub(-1),
+	_landDetectorSub(-1),
 
-/* publications */
+	/* publications */
 	_att_pub(-1),
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
@@ -384,21 +142,20 @@ FixedwingEstimator::FixedwingEstimator() :
 	_gps({}),
 	_wind({}),
 	_distance{},
+	_landDetector{},
 
 	_gyro_offsets({}),
 	_accel_offsets({}),
 	_mag_offsets({}),
 
-	#ifdef SENSOR_COMBINED_SUB
 	_sensor_combined{},
-	#endif
 
 	_pos_ref{},
 	_baro_ref(0.0f),
 	_baro_ref_offset(0.0f),
 	_baro_gps_offset(0.0f),
 
-/* performance counters */
+	/* 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")),
@@ -408,10 +165,14 @@ FixedwingEstimator::FixedwingEstimator() :
 	_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
 	_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
 
-/* states */
+	/* states */
+	_gps_alt_filt(0.0f),
+	_baro_alt_filt(0.0f),
+	_covariancePredictionDt(0.0f),
+	_gpsIsGood(false),
+	_previousGPSTimestamp(0),
 	_baro_init(false),
 	_gps_initialized(false),
-	_gps_start_time(0),
 	_filter_start_time(0),
 	_last_sensor_timestamp(0),
 	_last_run(0),
@@ -424,12 +185,18 @@ FixedwingEstimator::FixedwingEstimator() :
 	_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();
 
 	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
@@ -455,7 +222,6 @@ FixedwingEstimator::FixedwingEstimator() :
 	parameters_update();
 
 	/* get offsets */
-
 	int fd, res;
 
 	for (unsigned s = 0; s < 3; s++) {
@@ -498,7 +264,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	}
 }
 
-FixedwingEstimator::~FixedwingEstimator()
+AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
 {
 	if (_estimator_task != -1) {
 
@@ -525,16 +291,14 @@ FixedwingEstimator::~FixedwingEstimator()
 	estimator::g_estimator = nullptr;
 }
 
-int
-FixedwingEstimator::enable_logging(bool logging)
+int AttitudePositionEstimatorEKF::enable_logging(bool logging)
 {
 	_ekf_logging = logging;
 
 	return 0;
 }
 
-int
-FixedwingEstimator::parameters_update()
+int AttitudePositionEstimatorEKF::parameters_update()
 {
 
 	param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms));
@@ -578,8 +342,7 @@ FixedwingEstimator::parameters_update()
 	return OK;
 }
 
-void
-FixedwingEstimator::vehicle_status_poll()
+void AttitudePositionEstimatorEKF::vehicle_status_poll()
 {
 	bool vstatus_updated;
 
@@ -592,8 +355,7 @@ FixedwingEstimator::vehicle_status_poll()
 	}
 }
 
-int
-FixedwingEstimator::check_filter_state()
+int AttitudePositionEstimatorEKF::check_filter_state()
 {
 	/*
 	 *    CHECK IF THE INPUT DATA IS SANE
@@ -687,15 +449,15 @@ FixedwingEstimator::check_filter_state()
 		}
 
 		// Copy all states or at least all that we can fit
-		unsigned ekf_n_states = ekf_report.n_states;
-		unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
+		size_t ekf_n_states = ekf_report.n_states;
+		size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
 		rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
 
-		for (unsigned i = 0; i < rep.n_states; i++) {
+		for (size_t i = 0; i < rep.n_states; i++) {
 			rep.states[i] = ekf_report.states[i];
 		}
 
-		for (unsigned i = 0; i < rep.n_states; i++) {
+		for (size_t i = 0; i < rep.n_states; i++) {
 			rep.states[i] = ekf_report.states[i];
 		}
 
@@ -709,19 +471,17 @@ FixedwingEstimator::check_filter_state()
 	return check;
 }
 
-void
-FixedwingEstimator::task_main_trampoline(int argc, char *argv[])
+void AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[])
 {
 	estimator::g_estimator->task_main();
 }
 
-void
-FixedwingEstimator::task_main()
+void AttitudePositionEstimatorEKF::task_main()
 {
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 	_ekf = new AttPosEKF();
-	float dt = 0.0f; // time lapsed since last covariance prediction
+
 	_filter_start_time = hrt_absolute_time();
 
 	if (!_ekf) {
@@ -738,63 +498,32 @@ FixedwingEstimator::task_main()
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_sub = orb_subscribe(ORB_ID(home_position));
+	_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
 
-#ifndef SENSOR_COMBINED_SUB
-
-	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
-	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
-
-	/* rate limit gyro updates to 50 Hz */
-	/* XXX remove this!, BUT increase the data buffer size! */
-	orb_set_interval(_gyro_sub, 4);
-#else
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	/* XXX remove this!, BUT increase the data buffer size! */
 	orb_set_interval(_sensor_combined_sub, 9);
-#endif
 
 	/* sets also parameters in the EKF object */
 	parameters_update();
 
-	Vector3f lastAngRate;
-	Vector3f lastAccel;
-
 	/* wakeup source(s) */
 	struct pollfd fds[2];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
 	fds[0].events = POLLIN;
-#ifndef SENSOR_COMBINED_SUB
-	fds[1].fd = _gyro_sub;
-	fds[1].events = POLLIN;
-#else
+
 	fds[1].fd = _sensor_combined_sub;
 	fds[1].events = POLLIN;
-#endif
-
-	bool newDataGps = false;
-	bool newHgtData = false;
-	bool newAdsData = false;
-	bool newDataMag = false;
-	bool newRangeData = false;
-
-	float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
 
-	uint64_t last_gps = 0;
 	_gps.vel_n_m_s = 0.0f;
 	_gps.vel_e_m_s = 0.0f;
 	_gps.vel_d_m_s = 0.0f;
 
-	// init lowpass filters for baro and gps altitude
-	float _gps_alt_filt = 0, _baro_alt_filt = 0;
-	float rc = 10.0f;	// RC time constant of 1st order LPF in seconds
-	hrt_abstime baro_last = 0;
-
 	_task_running = true;
 
 	while (!_task_should_exit) {
@@ -832,9 +561,6 @@ FixedwingEstimator::task_main()
 			bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
 			vehicle_status_poll();
 
-			bool accel_updated;
-			bool mag_updated;
-
 			perf_count(_perf_gyro);
 
 			/* Reset baro reference if switching to HIL, reset sensor states */
@@ -842,14 +568,8 @@ FixedwingEstimator::task_main()
 				/* system is in HIL now, wait for measurements to come in one last round */
 				usleep(60000);
 
-#ifndef SENSOR_COMBINED_SUB
-				orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
-				orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
-				orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
-#else
 				/* now read all sensor publications to ensure all real sensor data is purged */
 				orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
-#endif
 
 				/* set sensors to de-initialized state */
 				_gyro_valid = false;
@@ -872,848 +592,825 @@ FixedwingEstimator::task_main()
 			/**
 			 *    PART ONE: COLLECT ALL DATA
 			 **/
+			 pollData();
 
-			/* load local copies */
-#ifndef SENSOR_COMBINED_SUB
-			orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
+			/*
+			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
+			 */
+			if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
+				continue;
+			}
 
+			/**
+			 *    PART TWO: EXECUTE THE FILTER
+			 *
+			 *    We run the filter only once all data has been fetched
+			 **/
 
-			orb_check(_accel_sub, &accel_updated);
+			if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
 
-			if (accel_updated) {
-				orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
-			}
+				// maintain filtered baro and gps altitudes to calculate weather offset
+				// baro sample rate is ~70Hz and measurement bandwidth is high
+				// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
+				// maintain heavily filtered values for both baro and gps altitude
+				// Assume the filtered output should be identical for both sensors
+				_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
+//				if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
+//					_last_debug_print = hrt_absolute_time();
+//					perf_print_counter(_perf_baro);
+//					perf_reset(_perf_baro);
+//					warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f",
+//							(double)_baro_gps_offset,
+//							(double)_baro_alt_filt,
+//							(double)_gps_alt_filt,
+//							(double)_global_pos.alt,
+//							(double)_local_pos.z);
+//				}
 
-			_last_sensor_timestamp = _gyro.timestamp;
-			IMUmsec = _gyro.timestamp / 1e3;
-			IMUusec = _gyro.timestamp;
+				/* Initialize the filter first */
+				if (!_gps_initialized && _gpsIsGood) {
+					initializeGPS();
+				} 
+				else if (!_ekf->statesInitialised) {
+					// North, East Down position (m)
+					float initVelNED[3] = {0.0f, 0.0f, 0.0f}; 
 
-			float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
-			_last_run = _gyro.timestamp;
+					_ekf->posNE[0] = 0.0f;
+					_ekf->posNE[1] = 0.0f;
 
-			/* guard against too large deltaT's */
-			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
-				deltaT = 0.01f;
-			}
+					_local_pos.ref_alt = _baro_ref;
+					_baro_ref_offset = 0.0f;
+					_baro_gps_offset = 0.0f;
+					_baro_alt_filt = _baro.altitude;
 
+					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 
-			// Always store data, independent of init status
-			/* fill in last data set */
-			_ekf->dtIMU = deltaT;
+				} 
+				else if (_ekf->statesInitialised) {
 
-			if (isfinite(_gyro.x) &&
-				isfinite(_gyro.y) &&
-				isfinite(_gyro.z)) {
-				_ekf->angRate.x = _gyro.x;
-				_ekf->angRate.y = _gyro.y;
-				_ekf->angRate.z = _gyro.z;
+					// Check if on ground - status is used by covariance prediction
+					_ekf->setOnGround(_landDetector.landed);
 
-				if (!_gyro_valid) {
-					lastAngRate = _ekf->angRate;
-				}
+					// 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;
+					}
 
-				_gyro_valid = true;
-			}
+					//Run EKF data fusion steps
+					updateSensorFusion(_newDataGps, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
 
-			if (accel_updated) {
-				_ekf->accel.x = _accel.x;
-				_ekf->accel.y = _accel.y;
-				_ekf->accel.z = _accel.z;
+					//Publish attitude estimations
+					publishAttitude();
 
-				if (!_accel_valid) {
-					lastAccel = _ekf->accel;
-				}
+					//Publish Local Position estimations
+					publishLocalPosition();
+
+					//Publish Global Position, but only if it's any good
+					if(_gps_initialized && _gpsIsGood)
+					{
+						publishGlobalPosition();
+					}
 
-				_accel_valid = true;
+					//Publish wind estimates
+					if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
+						publishWindEstimate();
+					}
+				}
 			}
 
-			_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
-			_ekf->lastAngRate = angRate;
-			_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
-			_ekf->lastAccel = accel;
+		}
 
+		perf_end(_loop_perf);
+	}
 
-#else
-			orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+	_task_running = false;
 
-			static hrt_abstime last_accel = 0;
-			static hrt_abstime last_mag = 0;
+	warnx("exiting.\n");
 
-			if (last_accel != _sensor_combined.accelerometer_timestamp) {
-				accel_updated = true;
-			} else {
-				accel_updated = false;
-			}
+	_estimator_task = -1;
+	_exit(0);
+}
 
-			last_accel = _sensor_combined.accelerometer_timestamp;
+void AttitudePositionEstimatorEKF::initializeGPS()
+{
+	// GPS is in scaled integers, convert
+	double lat = _gps.lat / 1.0e7;
+	double lon = _gps.lon / 1.0e7;
+	float gps_alt = _gps.alt / 1e3f;
 
+	// Set up height correctly
+	orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+	_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
 
-			// Copy gyro and accel
-			_last_sensor_timestamp = _sensor_combined.timestamp;
-			IMUmsec = _sensor_combined.timestamp / 1e3;
-			IMUusec = _sensor_combined.timestamp;
+	// init filtered gps and baro altitudes
+	_gps_alt_filt = gps_alt;
+	_baro_alt_filt = _baro.altitude;
 
-			float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
+	_ekf->baroHgt = _baro.altitude;
+	_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
 
-			/* guard against too large deltaT's */
-			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
-				deltaT = 0.01f;
-			}
+	// Set up position variables correctly
+	_ekf->GPSstatus = _gps.fix_type;
 
-			_last_run = _sensor_combined.timestamp;
+	_ekf->gpsLat = math::radians(lat);
+	_ekf->gpsLon = math::radians(lon) - M_PI;
+	_ekf->gpsHgt = gps_alt;
 
-			// Always store data, independent of init status
-			/* fill in last data set */
-			_ekf->dtIMU = deltaT;
+	// Look up mag declination based on current position
+	float declination = math::radians(get_mag_declination(lat, lon));
 
-			int last_gyro_main = _gyro_main;
+	float initVelNED[3];
+	initVelNED[0] = _gps.vel_n_m_s;
+	initVelNED[1] = _gps.vel_e_m_s;
+	initVelNED[2] = _gps.vel_d_m_s;
 
-			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)) {
+	_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
 
-				_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
-				_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
-				_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
-				_gyro_main = 0;
-				_gyro_valid = true;
+	// Initialize projection
+	_local_pos.ref_lat = lat;
+	_local_pos.ref_lon = lon;
+	_local_pos.ref_alt = gps_alt;
+	_local_pos.ref_timestamp = _gps.timestamp_position;
 
-			} else if (isfinite(_sensor_combined.gyro1_rad_s[0]) &&
-				isfinite(_sensor_combined.gyro1_rad_s[1]) &&
-				isfinite(_sensor_combined.gyro1_rad_s[2])) {
+	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);
 
-				_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
-				_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
-				_ekf->angRate.z = _sensor_combined.gyro1_rad_s[2];
-				_gyro_main = 1;
-				_gyro_valid = true;
+	#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
 
-			} else {
-				_gyro_valid = false;
-			}
+	_gps_initialized = true;
+}
 
-			if (last_gyro_main != _gyro_main) {
-				mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main);
-			}
+void AttitudePositionEstimatorEKF::publishAttitude()
+{
+	// Output results
+	math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+	math::Matrix<3, 3> R = q.to_dcm();
+	math::Vector<3> euler = R.to_euler();
 
-			if (!_gyro_valid) {
-				/* keep last value if he hit an out of band value */
-				lastAngRate = _ekf->angRate;
-			} else {
-				perf_count(_perf_gyro);
-			}
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			PX4_R(_att.R, i, j) = R(i, j);
+		}
+	}
 
-			if (accel_updated) {
-
-				int last_accel_main = _accel_main;
-
-				/* fail over to the 2nd accel if we know the first is down */
-				if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
-					_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
-					_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];
-					_ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2];
-					_accel_main = 1;
-				}
+	_att.timestamp = _last_sensor_timestamp;
+	_att.q[0] = _ekf->states[0];
+	_att.q[1] = _ekf->states[1];
+	_att.q[2] = _ekf->states[2];
+	_att.q[3] = _ekf->states[3];
+	_att.q_valid = true;
+	_att.R_valid = true;
+
+	_att.timestamp = _last_sensor_timestamp;
+	_att.roll = euler(0);
+	_att.pitch = euler(1);
+	_att.yaw = euler(2);
+
+	_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
+	_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
+	_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
+
+	// gyro offsets
+	_att.rate_offsets[0] = _ekf->states[10];
+	_att.rate_offsets[1] = _ekf->states[11];
+	_att.rate_offsets[2] = _ekf->states[12];
+
+	/* lazily publish the attitude only once available */
+	if (_att_pub > 0) {
+		/* publish the attitude setpoint */
+		orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
 
-				if (!_accel_valid) {
-					lastAccel = _ekf->accel;
-				}
+	} else {
+		/* advertise and publish */
+		_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
+	}
+}
 
-				if (last_accel_main != _accel_main) {
-					mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main);
-				}
+void AttitudePositionEstimatorEKF::publishLocalPosition()
+{
+	_local_pos.timestamp = _last_sensor_timestamp;
+	_local_pos.x = _ekf->states[7];
+	_local_pos.y = _ekf->states[8];
 
-				_accel_valid = true;
-			}
+	// XXX need to announce change of Z reference somehow elegantly
+	_local_pos.z = _ekf->states[9] - _baro_ref_offset;
 
-			_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
-			lastAngRate = _ekf->angRate;
-			_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
-			lastAccel = _ekf->accel;
+	_local_pos.vx = _ekf->states[4];
+	_local_pos.vy = _ekf->states[5];
+	_local_pos.vz = _ekf->states[6];
 
-			if (last_mag != _sensor_combined.magnetometer_timestamp) {
-				mag_updated = true;
-				newDataMag = true;
+	_local_pos.xy_valid = _gps_initialized && _gpsIsGood;
+	_local_pos.z_valid = true;
+	_local_pos.v_xy_valid = _gps_initialized && _gpsIsGood;
+	_local_pos.v_z_valid = true;
+	_local_pos.xy_global = _gps_initialized; //TODO: Handle optical flow mode here
 
-			} else {
-				newDataMag = false;
-			}
+	_local_pos.z_global = false;
+	_local_pos.yaw = _att.yaw;
 
-			last_mag = _sensor_combined.magnetometer_timestamp;
+	/* lazily publish the local position only once available */
+	if (_local_pos_pub > 0) {
+		/* publish the attitude setpoint */
+		orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
 
-#endif
+	} else {
+		/* advertise and publish */
+		_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
+	}
+}
 
-			//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
+void AttitudePositionEstimatorEKF::publishGlobalPosition()
+{
+	_global_pos.timestamp = _local_pos.timestamp;
+
+	if (_local_pos.xy_global) {
+		double est_lat, est_lon;
+		map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
+		_global_pos.lat = est_lat;
+		_global_pos.lon = est_lon;
+		_global_pos.time_utc_usec = _gps.time_utc_usec;
+	}
 
-			bool airspeed_updated;
-			orb_check(_airspeed_sub, &airspeed_updated);
+	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;
+	}
 
-			if (airspeed_updated) {
-				orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
-				perf_count(_perf_airspeed);
+	/* local pos alt is negative, change sign and add alt offsets */
+	_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
 
-				_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
-				newAdsData = true;
+	if (_local_pos.v_z_valid) {
+		_global_pos.vel_d = _local_pos.vz;
+	}
 
-			} else {
-				newAdsData = false;
-			}
+	/* 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);
 
-			bool gps_updated;
-			orb_check(_gps_sub, &gps_updated);
+	_global_pos.yaw = _local_pos.yaw;
+	_global_pos.eph = _gps.eph;
+	_global_pos.epv = _gps.epv;
 
-			if (gps_updated) {
+	/* lazily publish the global position only once available */
+	if (_global_pos_pub > 0) {
+		/* publish the global position */
+		orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
 
-				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
-				perf_count(_perf_gps);
+	} else {
+		/* advertise and publish */
+		_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
+	}
+}
 
-				if (_gps.fix_type < 3) {
-					newDataGps = false;
+void AttitudePositionEstimatorEKF::publishWindEstimate()
+{
+	_wind.timestamp = _global_pos.timestamp;
+	_wind.windspeed_north = _ekf->states[14];
+	_wind.windspeed_east = _ekf->states[15];
 
-				} else {
+	// XXX we need to do something smart about the covariance here
+	// but we default to the estimate covariance for now
+	_wind.covariance_north = _ekf->P[14][14];
+	_wind.covariance_east = _ekf->P[15][15];
 
-					/* store time of valid GPS measurement */
-					_gps_start_time = hrt_absolute_time();
+	/* lazily publish the wind estimate only once available */
+	if (_wind_pub > 0) {
+		/* publish the wind estimate */
+		orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
 
-					/* check if we had a GPS outage for a long time */
-					float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f;
+	} else {
+		/* advertise and publish */
+		_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
+	}
 
-					const float pos_reset_threshold = 5.0f; // seconds
+}
 
-					/* timeout of 5 seconds */
-					if (gps_elapsed > pos_reset_threshold) {
-						_ekf->ResetPosition();
-						_ekf->ResetVelocity();
-						_ekf->ResetStoredStates();
-					}
-					_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold));
+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();
+
+	// store the predicted states for subsequent use by measurement fusion
+	_ekf->StoreStates(IMUmsec);
+
+	// sum delta angles and time used by covariance prediction
+	_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
+	_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
+	_covariancePredictionDt += _ekf->dtIMU;
+
+	// 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)) {
+		_ekf->CovariancePrediction(_covariancePredictionDt);
+		_ekf->summedDelAng.zero();
+		_ekf->summedDelVel.zero();
+		_covariancePredictionDt = 0.0f;
+	}
 
-					/* fuse GPS updates */
+	// Fuse GPS Measurements
+	if (fuseGPS && _gps_initialized) {
+		// Convert GPS measurements to Pos NE, hgt and Vel NED
 
-					//_gps.timestamp / 1e3;
-					_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;
+		// set fusion flags
+		_ekf->fuseVelData = true;
+		_ekf->fusePosData = true;
 
-					// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
+		// recall states stored at time of measurement after adjusting for delays
+		_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+		_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
 
-					_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
-					_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
-					_ekf->gpsHgt = _gps.alt / 1e3f;
+		// run the fusion step
+		_ekf->FuseVelposNED();
 
-					// update LPF
-					_gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt);
+	} 
+	else if (!_gps_initialized) {
 
-					//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed);
+		// force static mode
+		_ekf->staticMode = true;
 
-					// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
-					// 	_ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
-					// } else {
-					// 	_ekf->vneSigma = _parameters.velne_noise;
-					// }
+		// Convert GPS measurements to Pos NE, hgt and Vel NED
+		_ekf->velNED[0] = 0.0f;
+		_ekf->velNED[1] = 0.0f;
+		_ekf->velNED[2] = 0.0f;
 
-					// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
-					// 	_ekf->posNeSigma = sqrtf(_gps.p_variance_m);
-					// } else {
-					// 	_ekf->posNeSigma = _parameters.posne_noise;
-					// }
+		_ekf->posNE[0] = 0.0f;
+		_ekf->posNE[1] = 0.0f;
 
-					// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
+		// set fusion flags
+		_ekf->fuseVelData = true;
+		_ekf->fusePosData = true;
 
-					newDataGps = true;
-					last_gps = _gps.timestamp_position;
+		// recall states stored at time of measurement after adjusting for delays
+		_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+		_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
 
-				}
+		// run the fusion step
+		_ekf->FuseVelposNED();
 
-			}
+	} 
+	else {
+		_ekf->fuseVelData = false;
+		_ekf->fusePosData = false;
+	}
 
-			bool baro_updated;
-			orb_check(_baro_sub, &baro_updated);
+	if (fuseBaro) {
+		// Could use a blend of GPS and baro alt data if desired
+		_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
+		_ekf->fuseHgtData = true;
 
-			if (baro_updated) {
+		// recall states stored at time of measurement after adjusting for delays
+		_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
 
-				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+		// run the fusion step
+		_ekf->FuseVelposNED();
 
-				float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
-				baro_last = _baro.timestamp;
+	} 
+	else {
+		_ekf->fuseHgtData = false;
+	}
 
-				_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
+	// 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->baroHgt = _baro.altitude;
-				_baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
+		_ekf->magstate.obsIndex = 0;
+		_ekf->FuseMagnetometer();
+		_ekf->FuseMagnetometer();
+		_ekf->FuseMagnetometer();
 
-				if (!_baro_init) {
-					_baro_ref = _baro.altitude;
-					_baro_init = true;
-					warnx("ALT REF INIT");
-				}
+	} 
+	else {
+		_ekf->fuseMagData = false;
+	}
 
-				perf_count(_perf_baro);
+	// 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->FuseAirspeed();
 
-				newHgtData = true;
-			} else {
-				newHgtData = false;
-			}
+	} 
+	else {
+		_ekf->fuseVtasData = false;
+	}
 
-#ifndef SENSOR_COMBINED_SUB
-			orb_check(_mag_sub, &mag_updated);
-#endif
+	// Fuse Rangefinder Measurements
+	if (fuseRangeSensor) {
+		if (_ekf->Tnb.z.z > 0.9f) {
+			// _ekf->rngMea is set in sensor readout already
+			_ekf->fuseRngData = true;
+			_ekf->fuseOptFlowData = false;
+			_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
+			_ekf->OpticalFlowEKF();
+			_ekf->fuseRngData = false;
+		}
+	}
+}
 
-			if (mag_updated) {
+int AttitudePositionEstimatorEKF::start()
+{
+	ASSERT(_estimator_task == -1);
 
-				_mag_valid = true;
+	/* start the task */
+	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 40,
+					 7500,
+					 (main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
+					 nullptr);
 
-				perf_count(_perf_mag);
+	if (_estimator_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
 
-#ifndef SENSOR_COMBINED_SUB
-				orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
+	return OK;
+}
 
-				// XXX we compensate the offsets upfront - should be close to zero.
-				// 0.001f
-				_ekf->magData.x = _mag.x;
-				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+void AttitudePositionEstimatorEKF::print_status()
+{
+	math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+	math::Matrix<3, 3> R = q.to_dcm();
+	math::Vector<3> euler = R.to_euler();
 
-				_ekf->magData.y = _mag.y;
-				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+	printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n",
+	       (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
 
-				_ekf->magData.z = _mag.z;
-				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+	// State vector:
+	// 0-3: quaternions (q0, q1, q2, q3)
+	// 4-6: Velocity - m/sec (North, East, Down)
+	// 7-9: Position - m (North, East, Down)
+	// 10-12: Delta Angle bias - rad (X,Y,Z)
+	// 13:    Accelerometer offset
+	// 14-15: Wind Vector  - m/sec (North,East)
+	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
+	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
-#else
-				int last_mag_main = _mag_main;
+	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]);
 
-				// XXX we compensate the offsets upfront - should be close to zero.
+	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 (terrain)      [22]: %8.4f\n", (double)_ekf->states[22]);
 
-				/* fail over to the 2nd mag if we know the first is down */
-				if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) {
-					_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
-					_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+	} 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: %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->magData.y = _sensor_combined.magnetometer_ga[1];
-					_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+void AttitudePositionEstimatorEKF::pollData()
+{
+	static Vector3f lastAngRate;
+	static Vector3f lastAccel;
+	bool accel_updated = false;
 
-					_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
+	//Update Gyro and Accelerometer
+	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
-					_ekf->magData.y = _sensor_combined.magnetometer1_ga[1];
-					_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+	static hrt_abstime last_accel = 0;
+	static hrt_abstime last_mag = 0;
 
-					_ekf->magData.z = _sensor_combined.magnetometer1_ga[2];
-					_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
-					_mag_main = 1;
-				}
+	if (last_accel != _sensor_combined.accelerometer_timestamp) {
+		accel_updated = true;
+	} else {
+		accel_updated = false;
+	}
 
-				if (last_mag_main != _mag_main) {
-					mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main);
-				}
-#endif
+	last_accel = _sensor_combined.accelerometer_timestamp;
 
-				newDataMag = true;
 
-			} else {
-				newDataMag = false;
-			}
+	// Copy gyro and accel
+	_last_sensor_timestamp = _sensor_combined.timestamp;
+	IMUmsec = _sensor_combined.timestamp / 1e3;
+	IMUusec = _sensor_combined.timestamp;
 
-			orb_check(_distance_sub, &newRangeData);
+	float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
 
-			if (newRangeData) {
-				orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
+	/* guard against too large deltaT's */
+	if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
+		deltaT = 0.01f;
+	}
 
-				if (_distance.valid) {
-					_ekf->rngMea = _distance.distance;
-					_distance_last_valid = _distance.timestamp;
-				} else {
-					newRangeData = false;
-				}
-			}
+	_last_run = _sensor_combined.timestamp;
 
-			/*
-			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
-			 */
-			if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
-				continue;
-			}
+	// Always store data, independent of init status
+	/* fill in last data set */
+	_ekf->dtIMU = deltaT;
 
-			/**
-			 *    PART TWO: EXECUTE THE FILTER
-			 *
-			 *    We run the filter only once all data has been fetched
-			 **/
+	int last_gyro_main = _gyro_main;
 
-			if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
+	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)) {
 
-				float initVelNED[3];
+		_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
+		_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
+		_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
+		_gyro_main = 0;
+		_gyro_valid = true;
 
-				// maintain filtered baro and gps altitudes to calculate weather offset
-				// baro sample rate is ~70Hz and measurement bandwidth is high
-				// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
-				// maintain heavily filtered values for both baro and gps altitude
-				// Assume the filtered output should be identical for both sensors
-				_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
-//				if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
-//					_last_debug_print = hrt_absolute_time();
-//					perf_print_counter(_perf_baro);
-//					perf_reset(_perf_baro);
-//					warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f",
-//							(double)_baro_gps_offset,
-//							(double)_baro_alt_filt,
-//							(double)_gps_alt_filt,
-//							(double)_global_pos.alt,
-//							(double)_local_pos.z);
-//				}
+	} else if (isfinite(_sensor_combined.gyro1_rad_s[0]) &&
+		isfinite(_sensor_combined.gyro1_rad_s[1]) &&
+		isfinite(_sensor_combined.gyro1_rad_s[2])) {
 
-				/* Initialize the filter first */
-				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
+		_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
+		_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
+		_ekf->angRate.z = _sensor_combined.gyro1_rad_s[2];
+		_gyro_main = 1;
+		_gyro_valid = true;
 
-					// GPS is in scaled integers, convert
-					double lat = _gps.lat / 1.0e7;
-					double lon = _gps.lon / 1.0e7;
-					float gps_alt = _gps.alt / 1e3f;
+	} else {
+		_gyro_valid = false;
+	}
 
-					initVelNED[0] = _gps.vel_n_m_s;
-					initVelNED[1] = _gps.vel_e_m_s;
-					initVelNED[2] = _gps.vel_d_m_s;
+	if (last_gyro_main != _gyro_main) {
+		mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main);
+	}
 
-					// Set up height correctly
-					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-					_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
+	if (!_gyro_valid) {
+		/* keep last value if he hit an out of band value */
+		lastAngRate = _ekf->angRate;
+	} else {
+		perf_count(_perf_gyro);
+	}
 
-					// init filtered gps and baro altitudes
-					_gps_alt_filt = gps_alt;
-					_baro_alt_filt = _baro.altitude;
+	if (accel_updated) {
 
-					_ekf->baroHgt = _baro.altitude;
-					_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
+		int last_accel_main = _accel_main;
 
-					// Set up position variables correctly
-					_ekf->GPSstatus = _gps.fix_type;
+		/* fail over to the 2nd accel if we know the first is down */
+		if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
+			_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
+			_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];
+			_ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2];
+			_accel_main = 1;
+		}
 
-					_ekf->gpsLat = math::radians(lat);
-					_ekf->gpsLon = math::radians(lon) - M_PI;
-					_ekf->gpsHgt = gps_alt;
+		if (!_accel_valid) {
+			lastAccel = _ekf->accel;
+		}
 
-					// Look up mag declination based on current position
-					float declination = math::radians(get_mag_declination(lat, lon));
+		if (last_accel_main != _accel_main) {
+			mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main);
+		}
 
-					_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
+		_accel_valid = true;
+	}
 
-					// Initialize projection
-					_local_pos.ref_lat = lat;
-					_local_pos.ref_lon = lon;
-					_local_pos.ref_alt = gps_alt;
-					_local_pos.ref_timestamp = _gps.timestamp_position;
+	_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
+	lastAngRate = _ekf->angRate;
+	_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
+	lastAccel = _ekf->accel;
 
-					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 (last_mag != _sensor_combined.magnetometer_timestamp) {
+		_newDataMag = true;
 
-					#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
+	} else {
+		_newDataMag = false;
+	}
 
-					_gps_initialized = true;
+	last_mag = _sensor_combined.magnetometer_timestamp;
 
-				} else if (!_ekf->statesInitialised) {
+	//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
 
-					initVelNED[0] = 0.0f;
-					initVelNED[1] = 0.0f;
-					initVelNED[2] = 0.0f;
-					_ekf->posNE[0] = posNED[0];
-					_ekf->posNE[1] = posNED[1];
+	//Update Land Detector
+	bool newLandData;
+	orb_check(_landDetectorSub, &newLandData);
+	if(newLandData) {
+		orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector);
+	}
 
-					_local_pos.ref_alt = _baro_ref;
-					_baro_ref_offset = 0.0f;
-					_baro_gps_offset = 0.0f;
+	//Update AirSpeed
+	orb_check(_airspeed_sub, &_newAdsData);
+	if (_newAdsData) {
+		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+		perf_count(_perf_airspeed);
 
-					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
+		_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
+	}
 
-				} else if (_ekf->statesInitialised) {
 
-					// We're apparently initialized in this case now
-					// check (and reset the filter as needed)
-					int check = check_filter_state();
+	orb_check(_gps_sub, &_newDataGps);
+	if (_newDataGps) {
 
-					if (check) {
-						// Let the system re-initialize itself
-						continue;
-					}
+		orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
+		perf_count(_perf_gps);
 
-					// Run the strapdown INS equations every IMU update
-					_ekf->UpdateStrapdownEquationsNED();
+		//We are more strict for our first fix
+		float requiredAccuracy = _parameters.pos_stddev_threshold;
+		if(_gpsIsGood) {
+			requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f;
+		}
 
-					// store the predicted states for subsequent use by measurement fusion
-					_ekf->StoreStates(IMUmsec);
-					// Check if on ground - status is used by covariance prediction
-					_ekf->OnGroundCheck();
-					// sum delta angles and time used by covariance prediction
-					_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
-					_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
-					dt += _ekf->dtIMU;
-
-					// 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 ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
-						_ekf->CovariancePrediction(dt);
-						_ekf->summedDelAng.zero();
-						_ekf->summedDelVel.zero();
-						dt = 0.0f;
-					}
+		//Check if the GPS fix is good enough for us to use
+		if(_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) {
+			_gpsIsGood = true;
+		}
+		else {
+			_gpsIsGood = false;
+		}
 
-					// Fuse GPS Measurements
-					if (newDataGps && _gps_initialized) {
-						// Convert GPS measurements to Pos NE, hgt and Vel NED
-
-						float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f;
-
-						// Calculate acceleration predicted by GPS velocity change
-						if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) ||
-							(fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) ||
-							(fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) {
-
-							_ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt;
-							_ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt;
-							_ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt;
-						}
-
-						_ekf->velNED[0] = _gps.vel_n_m_s;
-						_ekf->velNED[1] = _gps.vel_e_m_s;
-						_ekf->velNED[2] = _gps.vel_d_m_s;
-						_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];
-						// set fusion flags
-						_ekf->fuseVelData = true;
-						_ekf->fusePosData = true;
-						// recall states stored at time of measurement after adjusting for delays
-						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
-						// run the fusion step
-						_ekf->FuseVelposNED();
-
-					} else if (!_gps_initialized) {
-
-						// force static mode
-						_ekf->staticMode = true;
-
-						// Convert GPS measurements to Pos NE, hgt and Vel NED
-						_ekf->velNED[0] = 0.0f;
-						_ekf->velNED[1] = 0.0f;
-						_ekf->velNED[2] = 0.0f;
-
-						_ekf->posNE[0] = 0.0f;
-						_ekf->posNE[1] = 0.0f;
-						// set fusion flags
-						_ekf->fuseVelData = true;
-						_ekf->fusePosData = true;
-						// recall states stored at time of measurement after adjusting for delays
-						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
-						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
-						// run the fusion step
-						_ekf->FuseVelposNED();
-
-					} else {
-						_ekf->fuseVelData = false;
-						_ekf->fusePosData = false;
-					}
+		if (_gpsIsGood) {
 
-					if (newHgtData) {
-						// Could use a blend of GPS and baro alt data if desired
-						_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
-						_ekf->fuseHgtData = true;
-						// recall states stored at time of measurement after adjusting for delays
-						_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
-						// run the fusion step
-						_ekf->FuseVelposNED();
-
-					} else {
-						_ekf->fuseHgtData = false;
-					}
+			//Calculate time since last good GPS fix
+			const float dtGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
 
-					// Fuse Magnetometer Measurements
-					if (newDataMag) {
-						_ekf->fuseMagData = true;
-						_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
+			_ekf->updateDtGpsFilt(math::constrain(dtGoodGPS, 0.01f, POS_RESET_THRESHOLD));
 
-						_ekf->magstate.obsIndex = 0;
-						_ekf->FuseMagnetometer();
-						_ekf->FuseMagnetometer();
-						_ekf->FuseMagnetometer();
+			/* fuse GPS updates */
 
-					} else {
-						_ekf->fuseMagData = false;
-					}
+			//_gps.timestamp / 1e3;
+			_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;
 
-					// Fuse Airspeed Measurements
-					if (newAdsData && _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->FuseAirspeed();
+			// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
 
-					} else {
-						_ekf->fuseVtasData = false;
-					}
+			_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
+			_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
+			_ekf->gpsHgt = _gps.alt / 1e3f;
 
-					if (newRangeData) {
+			//check if we had a GPS outage for a long time
+			if(_gps_initialized) {
 
-						if (_ekf->Tnb.z.z > 0.9f) {
-							// _ekf->rngMea is set in sensor readout already
-							_ekf->fuseRngData = true;
-							_ekf->fuseOptFlowData = false;
-							_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
-							_ekf->OpticalFlowEKF();
-							_ekf->fuseRngData = false;
-						}
-					}
+				//Convert from global frame to local frame
+				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) {
+					_ekf->ResetPosition();
+					_ekf->ResetVelocity();
+				}
+			}
 
-					// Output results
-					math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
-					math::Matrix<3, 3> R = q.to_dcm();
-					math::Vector<3> euler = R.to_euler();
-
-					for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-							PX4_R(_att.R, i, j) = R(i, j);
-
-					_att.timestamp = _last_sensor_timestamp;
-					_att.q[0] = _ekf->states[0];
-					_att.q[1] = _ekf->states[1];
-					_att.q[2] = _ekf->states[2];
-					_att.q[3] = _ekf->states[3];
-					_att.q_valid = true;
-					_att.R_valid = true;
-
-					_att.timestamp = _last_sensor_timestamp;
-					_att.roll = euler(0);
-					_att.pitch = euler(1);
-					_att.yaw = euler(2);
-
-					_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
-					_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
-					_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
-					// gyro offsets
-					_att.rate_offsets[0] = _ekf->states[10];
-					_att.rate_offsets[1] = _ekf->states[11];
-					_att.rate_offsets[2] = _ekf->states[12];
-
-					/* lazily publish the attitude only once available */
-					if (_att_pub > 0) {
-						/* publish the attitude setpoint */
-						orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
-
-					} else {
-						/* advertise and publish */
-						_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
-					}
+			// update LPF
+			_gps_alt_filt += (dtGoodGPS / (rc + dtGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
 
-					if (_gps_initialized) {
-						_local_pos.timestamp = _last_sensor_timestamp;
-						_local_pos.x = _ekf->states[7];
-						_local_pos.y = _ekf->states[8];
-						// XXX need to announce change of Z reference somehow elegantly
-						_local_pos.z = _ekf->states[9] - _baro_ref_offset;
-
-						_local_pos.vx = _ekf->states[4];
-						_local_pos.vy = _ekf->states[5];
-						_local_pos.vz = _ekf->states[6];
-
-						_local_pos.xy_valid = _gps_initialized;
-						_local_pos.z_valid = true;
-						_local_pos.v_xy_valid = _gps_initialized;
-						_local_pos.v_z_valid = true;
-						_local_pos.xy_global = true;
-
-						_local_pos.z_global = false;
-						_local_pos.yaw = _att.yaw;
-
-						/* lazily publish the local position only once available */
-						if (_local_pos_pub > 0) {
-							/* publish the attitude setpoint */
-							orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
-
-						} else {
-							/* advertise and publish */
-							_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
-						}
-
-						_global_pos.timestamp = _local_pos.timestamp;
-
-						if (_local_pos.xy_global) {
-							double est_lat, est_lon;
-							map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
-							_global_pos.lat = est_lat;
-							_global_pos.lon = est_lon;
-							_global_pos.time_utc_usec = _gps.time_utc_usec;
-							_global_pos.eph = _gps.eph;
-							_global_pos.epv = _gps.epv;
-						}
-
-						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;
-						}
-
-						/* local pos alt is negative, change sign and add alt offsets */
-						_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;
-
-						if (_local_pos.v_z_valid) {
-							_global_pos.vel_d = _local_pos.vz;
-						}
-
-						/* 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);
-
-						_global_pos.yaw = _local_pos.yaw;
-
-						_global_pos.eph = _gps.eph;
-						_global_pos.epv = _gps.epv;
-
-						_global_pos.timestamp = _local_pos.timestamp;
-
-						/* lazily publish the global position only once available */
-						if (_global_pos_pub > 0) {
-							/* publish the global position */
-							orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
-
-						} else {
-							/* advertise and publish */
-							_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
-						}
-
-						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
-							_wind.timestamp = _global_pos.timestamp;
-							_wind.windspeed_north = _ekf->states[14];
-							_wind.windspeed_east = _ekf->states[15];
-							// XXX we need to do something smart about the covariance here
-							// but we default to the estimate covariance for now
-							_wind.covariance_north = _ekf->P[14][14];
-							_wind.covariance_east = _ekf->P[15][15];
-
-							/* lazily publish the wind estimate only once available */
-							if (_wind_pub > 0) {
-								/* publish the wind estimate */
-								orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
-
-							} else {
-								/* advertise and publish */
-								_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
-							}
-						}
+			//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) {
+			// 	_ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
+			// } else {
+			// 	_ekf->vneSigma = _parameters.velne_noise;
+			// }
 
-				}
+			// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
+			// 	_ekf->posNeSigma = sqrtf(_gps.p_variance_m);
+			// } else {
+			// 	_ekf->posNeSigma = _parameters.posne_noise;
+			// }
 
-			}
+			// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
 
+			_previousGPSTimestamp = _gps.timestamp_position;
 		}
+		else {
+			//Too poor GPS fix to use
+			_newDataGps = false;
+		}
+	}
 
-		perf_end(_loop_perf);
+	// 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) {
+		_gpsIsGood = false;
 	}
 
-	_task_running = false;
+	//Update barometer
+	orb_check(_baro_sub, &_newHgtData);
 
-	warnx("exiting.\n");
+	if (_newHgtData) {
+		static hrt_abstime baro_last = 0;
 
-	_estimator_task = -1;
-	_exit(0);
-}
+		orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
 
-int
-FixedwingEstimator::start()
-{
-	ASSERT(_estimator_task == -1);
+		// init lowpass filters for baro and gps altitude
+		float baro_elapsed;
+		if(baro_last == 0) {
+			baro_elapsed = 0.0f;
+		}
+		else {
+			baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
+		}
+		baro_last = _baro.timestamp;
 
-	/* start the task */
-	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
-					 7500,
-					 (main_t)&FixedwingEstimator::task_main_trampoline,
-					 nullptr);
+		_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
 
-	if (_estimator_task < 0) {
-		warn("task start failed");
-		return -errno;
+		_ekf->baroHgt = _baro.altitude;
+		_baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
+
+		if (!_baro_init) {
+			_baro_ref = _baro.altitude;
+			_baro_init = true;
+			warnx("ALT REF INIT");
+		}
+
+		perf_count(_perf_baro);
+
+		_newHgtData = true;
 	}
 
-	return OK;
-}
+	//Update Magnetometer
+	if (_newDataMag) {
 
-void
-FixedwingEstimator::print_status()
-{
-	math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
-	math::Matrix<3, 3> R = q.to_dcm();
-	math::Vector<3> euler = R.to_euler();
+		_mag_valid = true;
 
-	printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n",
-	       (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
+		perf_count(_perf_mag);
 
-	// State vector:
-	// 0-3: quaternions (q0, q1, q2, q3)
-	// 4-6: Velocity - m/sec (North, East, Down)
-	// 7-9: Position - m (North, East, Down)
-	// 10-12: Delta Angle bias - rad (X,Y,Z)
-	// 13:    Accelerometer offset
-	// 14-15: Wind Vector  - m/sec (North,East)
-	// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
-	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
+		int last_mag_main = _mag_main;
 
-	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]);
+		// XXX we compensate the offsets upfront - should be close to zero.
 
-	if (n_states == 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 (terrain)      [22]: %8.4f\n", (double)_ekf->states[22]);
+		/* fail over to the 2nd mag if we know the first is down */
+		if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) {
+			_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
+			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
 
-	} 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]);
+			_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
+			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+			_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
+
+			_ekf->magData.y = _sensor_combined.magnetometer1_ga[1];
+			_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+			_ekf->magData.z = _sensor_combined.magnetometer1_ga[2];
+			_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+			_mag_main = 1;
+		}
+
+		if (last_mag_main != _mag_main) {
+			mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main);
+		}
+	}
+
+	//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;
+		}
 	}
-	printf("states: %s %s %s %s %s %s %s %s %s %s\n",
-		       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
-		       (_ekf->onGround) ? "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");
 }
 
-int FixedwingEstimator::trip_nan() {
+int AttitudePositionEstimatorEKF::trip_nan() {
 
 	int ret = 0;
 
@@ -1772,7 +1469,7 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 		if (estimator::g_estimator != nullptr)
 			errx(1, "already running");
 
-		estimator::g_estimator = new FixedwingEstimator;
+		estimator::g_estimator = new AttitudePositionEstimatorEKF();
 
 		if (estimator::g_estimator == nullptr)
 			errx(1, "alloc failed");
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index 15d018ab6..d8e3116b9 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -1,14 +1,54 @@
+/****************************************************************************
+* Copyright (c) 2014, Paul Riseborough All rights reserved.
+* 
+* Redistribution and use in source and binary forms, with or without 
+* modification, are permitted provided that the following conditions are met:
+* 
+* Redistributions of source code must retain the above copyright notice, this 
+* list of conditions and the following disclaimer.
+* 
+* Redistributions in binary form must reproduce the above copyright notice, 
+* this list of conditions and the following disclaimer in the documentation 
+* and/or other materials provided with the distribution.
+* 
+* Neither the name of the {organization} nor the names of its contributors 
+* may be used to endorse or promote products derived from this software without 
+* specific prior written permission.
+* 
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
+* POSSIBILITY OF SUCH DAMAGE.
+****************************************************************************/
+
+/**
+ * @file estimator_22states.cpp
+ *
+ * Implementation of the attitude and position estimator.
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
 #include "estimator_22states.h"
 #include <string.h>
 #include <stdio.h>
 #include <stdarg.h>
 #include <math.h>
+#include <algorithm>
 
 #ifndef M_PI_F
-#define M_PI_F ((float)M_PI)
+#define M_PI_F static_cast<float>(M_PI)
 #endif
 
-#define EKF_COVARIANCE_DIVERGED 1.0e8f
+constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;
 
 AttPosEKF::AttPosEKF() :
     covTimeStepMax(0.0f),
@@ -72,7 +112,6 @@ AttPosEKF::AttPosEKF() :
     innovVelPos{},
     varInnovVelPos{},
     velNED{},
-    accelGPSNED{},
     posNE{},
     hgtMea(0.0f),
     baroHgtOffset(0.0f),
@@ -115,7 +154,6 @@ AttPosEKF::AttPosEKF() :
     inhibitGndState(true),
     inhibitScaleState(true),
 
-    onGround(true),
     staticMode(true),
     useGPS(false),
     useAirspeed(true),
@@ -145,7 +183,9 @@ AttPosEKF::AttPosEKF() :
     auxFlowInnovGate(0.0f),
     rngInnovGate(0.0f),
     minFlowRng(0.0f),
-    moCompR_LOS(0.0f)
+    moCompR_LOS(0.0f),
+
+    _onGround(true)    
 {
 
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
@@ -158,6 +198,40 @@ AttPosEKF::~AttPosEKF()
 {
 }
 
+void AttPosEKF::InitialiseParameters()
+{
+    covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+    covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
+    rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+    EAS2TAS = 1.0f;
+
+    yawVarScale = 1.0f;
+    windVelSigma = 0.1f;
+    dAngBiasSigma = 5.0e-7f;
+    dVelBiasSigma = 1e-4f;
+    magEarthSigma = 3.0e-4f;
+    magBodySigma  = 3.0e-4f;
+
+    vneSigma = 0.2f;
+    vdSigma = 0.3f;
+    posNeSigma = 2.0f;
+    posDSigma = 2.0f;
+
+    magMeasurementSigma = 0.05;
+    airspeedMeasurementSigma = 1.4f;
+    gyroProcessNoise = 1.4544411e-2f;
+    accelProcessNoise = 0.5f;
+
+    gndHgtSigma  = 0.1f; // terrain gradient 1-sigma
+    R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2
+    flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
+    auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
+    rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check
+    minFlowRng = 0.3f; //minimum range between ground and flow sensor
+    moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate
+}
+
+
 void AttPosEKF::UpdateStrapdownEquationsNED()
 {
     Vector3f delVelNav;
@@ -177,7 +251,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     float deltaQuat[4];
     const Vector3f gravityNED(0.0f, 0.0f, GRAVITY_MSS);
 
-// Remove sensor bias errors
+    // Remove sensor bias errors
     correctedDelAng.x = dAngIMU.x - states[10];
     correctedDelAng.y = dAngIMU.y - states[11];
     correctedDelAng.z = dAngIMU.z - states[12];
@@ -192,14 +266,14 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     delAngTotal.y += correctedDelAng.y;
     delAngTotal.z += correctedDelAng.z;
 
-// Save current measurements
+    // Save current measurements
     Vector3f  prevDelAng = correctedDelAng;
 
-// Apply corrections for earths rotation rate and coning errors
-// * and + operators have been overloaded
+    // Apply corrections for earths rotation rate and coning errors
+    // * and + operators have been overloaded
     correctedDelAng   = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
 
-// Convert the rotation vector to its equivalent quaternion
+    // Convert the rotation vector to its equivalent quaternion
     rotationMag = correctedDelAng.length();
     if (rotationMag < 1e-12f)
     {
@@ -221,14 +295,14 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
         deltaQuat[3] = correctedDelAng.z*rotScaler;
     }
 
-// Update the quaternions by rotating from the previous attitude through
-// the delta angle rotation quaternion
+    // Update the quaternions by rotating from the previous attitude through
+    // the delta angle rotation quaternion
     qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
     qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
     qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
     qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
 
-// Normalise the quaternions and update the quaternion states
+    // Normalise the quaternions and update the quaternion states
     quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
     if (quatMag > 1e-16f)
     {
@@ -239,7 +313,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
         states[3] = quatMagInv*qUpdated[3];
     }
 
-// Calculate the body to nav cosine matrix
+    // Calculate the body to nav cosine matrix
     q00 = sq(states[0]);
     q11 = sq(states[1]);
     q22 = sq(states[2]);
@@ -263,29 +337,29 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
 
     Tnb = Tbn.transpose();
 
-// transform body delta velocities to delta velocities in the nav frame
-// * and + operators have been overloaded
+    // transform body delta velocities to delta velocities in the nav frame
+    // * and + operators have been overloaded
     //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
     delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU;
     delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU;
     delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU;
 
-// calculate the magnitude of the nav acceleration (required for GPS
-// variance estimation)
+    // calculate the magnitude of the nav acceleration (required for GPS
+    // variance estimation)
     accNavMag = delVelNav.length()/dtIMU;
 
-// If calculating position save previous velocity
+    // If calculating position save previous velocity
     float lastVelocity[3];
     lastVelocity[0] = states[4];
     lastVelocity[1] = states[5];
     lastVelocity[2] = states[6];
 
-// Sum delta velocities to get velocity
+    // Sum delta velocities to get velocity
     states[4] = states[4] + delVelNav.x;
     states[5] = states[5] + delVelNav.y;
     states[6] = states[6] + delVelNav.z;
 
-// If calculating postions, do a trapezoidal integration for position
+    // If calculating postions, do a trapezoidal integration for position
     states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
     states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
     states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
@@ -322,12 +396,12 @@ void AttPosEKF::CovariancePrediction(float dt)
     float dvz_b;
 
     // arrays
-    float processNoise[n_states];
+    float processNoise[EKF_STATE_ESTIMATES];
     float SF[15];
     float SG[8];
     float SQ[11];
     float SPP[8] = {0};
-    float nextP[n_states][n_states];
+    float nextP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES];
 
     // calculate covariance prediction process noise
     for (uint8_t i= 0; i<4;  i++) processNoise[i] = 1.0e-9f;
@@ -343,13 +417,13 @@ void AttPosEKF::CovariancePrediction(float dt)
     }
     if (!inhibitMagStates) {
         for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma;
-        for (uint8_t i=19; i < n_states; i++) processNoise[i] = dt * magBodySigma;
+        for (uint8_t i=19; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = dt * magBodySigma;
     } else {
-        for (uint8_t i=16; i < n_states; i++) processNoise[i] = 0;
+        for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = 0;
     }
 
     // square all sigmas
-    for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]);
+    for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) processNoise[i] = sq(processNoise[i]);
 
     // set variables used to calculate covariance growth
     dvx = summedDelVel.x;
@@ -370,7 +444,7 @@ void AttPosEKF::CovariancePrediction(float dt)
     daxCov = sq(dt*gyroProcessNoise);
     dayCov = sq(dt*gyroProcessNoise);
     dazCov = sq(dt*gyroProcessNoise);
-    if (onGround) dazCov = dazCov * sq(yawVarScale);
+    if (_onGround) dazCov = dazCov * sq(yawVarScale);
     accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f);
     dvxCov = sq(dt*accelProcessNoise);
     dvyCov = sq(dt*accelProcessNoise);
@@ -908,7 +982,7 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[21][20] = P[21][20];
     nextP[21][21] = P[21][21];
 
-    for (unsigned i = 0; i < n_states; i++)
+    for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++)
     {
         nextP[i][i] = nextP[i][i] + processNoise[i];
     }
@@ -921,7 +995,7 @@ void AttPosEKF::CovariancePrediction(float dt)
     {
         for (uint8_t i=7; i<=8; i++)
         {
-            for (unsigned j = 0; j < n_states; j++)
+            for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++)
             {
                 nextP[i][j] = P[i][j];
                 nextP[j][i] = P[j][i];
@@ -930,12 +1004,12 @@ void AttPosEKF::CovariancePrediction(float dt)
     }
 
     // Copy covariance
-    for (unsigned i = 0; i < n_states; i++) {
+    for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
         P[i][i] = nextP[i][i];
     }
 
     // force symmetry for observable states
-    for (unsigned i = 1; i < n_states; i++)
+    for (size_t i = 1; i < EKF_STATE_ESTIMATES; i++)
     {
         for (uint8_t j = 0; j < i; j++)
         {
@@ -965,24 +1039,24 @@ void AttPosEKF::updateDtVelPosFilt(float dt)
 void AttPosEKF::FuseVelposNED()
 {
 
-// declare variables used by fault isolation logic
+    // declare variables used by fault isolation logic
     uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure
     uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available
     uint32_t hgtRetryTime = 500; // height measurement retry time
     uint32_t horizRetryTime;
 
-// declare variables used to check measurement errors
+    // declare variables used to check measurement errors
     float velInnov[3] = {0.0f,0.0f,0.0f};
     float posInnov[2] = {0.0f,0.0f};
     float hgtInnov = 0.0f;
 
-// declare variables used to control access to arrays
+    // declare variables used to control access to arrays
     bool fuseData[6] = {false,false,false,false,false,false};
     uint8_t stateIndex;
     uint8_t obsIndex;
     uint8_t indexLimit = 21;
 
-// declare variables used by state and covariance update calculations
+    // declare variables used by state and covariance update calculations
     float velErr;
     float posErr;
     float R_OBS[6];
@@ -990,12 +1064,12 @@ void AttPosEKF::FuseVelposNED()
     float SK;
     float quatMag;
 
-// Perform sequential fusion of GPS measurements. This assumes that the
-// errors in the different velocity and position components are
-// uncorrelated which is not true, however in the absence of covariance
-// data from the GPS receiver it is the only assumption we can make
-// so we might as well take advantage of the computational efficiencies
-// associated with sequential fusion
+    // Perform sequential fusion of GPS measurements. This assumes that the
+    // errors in the different velocity and position components are
+    // uncorrelated which is not true, however in the absence of covariance
+    // data from the GPS receiver it is the only assumption we can make
+    // so we might as well take advantage of the computational efficiencies
+    // associated with sequential fusion
     if (fuseVelData || fusePosData || fuseHgtData)
     {
         uint64_t tNow = getMicros();
@@ -1015,8 +1089,8 @@ void AttPosEKF::FuseVelposNED()
         else horizRetryTime = gpsRetryTimeNoTAS;
 
         // Form the observation vector
-        for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
-        for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
+        for (uint8_t i=0; i <=2; i++) observation[i] = velNED[i];
+        for (uint8_t i=3; i <=4; i++) observation[i] = posNE[i-3];
         observation[5] = -(hgtMea);
 
         // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
@@ -1048,10 +1122,9 @@ void AttPosEKF::FuseVelposNED()
                 current_ekf_state.velHealth = true;
                 current_ekf_state.velFailTime = millis();
             } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) {
-                // XXX check
                 current_ekf_state.velHealth = true;
                 ResetVelocity();
-                ResetStoredStates();
+
                 // do not fuse bad data
                 fuseVelData = false;
             }
@@ -1060,6 +1133,7 @@ void AttPosEKF::FuseVelposNED()
                 current_ekf_state.velHealth = false;
             }
         }
+
         if (fusePosData)
         {
             // test horizontal position measurements
@@ -1078,9 +1152,6 @@ void AttPosEKF::FuseVelposNED()
                 if (current_ekf_state.posTimeout) {
                     ResetPosition();
                     
-                    // XXX cross-check the state reset
-                    ResetStoredStates();
-
                     // do not fuse position data on this time
                     // step
                     fusePosData = false;
@@ -1091,6 +1162,7 @@ void AttPosEKF::FuseVelposNED()
                 current_ekf_state.posHealth = false;
             }
         }
+
         // test height measurements
         if (fuseHgtData)
         {
@@ -1108,7 +1180,6 @@ void AttPosEKF::FuseVelposNED()
                 // the height data, but reset height and stored states
                 if (current_ekf_state.hgtTimeout) {
                     ResetHeight();
-                    ResetStoredStates();
                     fuseHgtData = false;
                 }
             }
@@ -1179,7 +1250,7 @@ void AttPosEKF::FuseVelposNED()
                 }
                 // Don't update magnetic field states if inhibited
                 if (inhibitMagStates) {
-                    for (uint8_t i = 16; i < n_states; i++)
+                    for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++)
                     {
                         Kfusion[i] = 0;
                     }
@@ -1190,7 +1261,7 @@ void AttPosEKF::FuseVelposNED()
                 {
                     states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
                 }
-                quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+                quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
                 if (quatMag > 1e-12f) // divide by  0 protection
                 {
                     for (uint8_t i = 0; i<=3; i++)
@@ -1246,17 +1317,17 @@ void AttPosEKF::FuseMagnetometer()
     float SK_MX[6];
     float SK_MY[5];
     float SK_MZ[6];
-    float H_MAG[n_states];
-    for (uint8_t i = 0; i < n_states; i++) {
+    float H_MAG[EKF_STATE_ESTIMATES];
+    for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
         H_MAG[i] = 0.0f;
     }
 
-// Perform sequential fusion of Magnetometer measurements.
-// This assumes that the errors in the different components are
-// uncorrelated which is not true, however in the absence of covariance
-// data fit is the only assumption we can make
-// so we might as well take advantage of the computational efficiencies
-// associated with sequential fusion
+    // Perform sequential fusion of Magnetometer measurements.
+    // This assumes that the errors in the different components are
+    // uncorrelated which is not true, however in the absence of covariance
+    // data fit is the only assumption we can make
+    // so we might as well take advantage of the computational efficiencies
+    // associated with sequential fusion
     if (useCompass && fuseMagData && (obsIndex < 3))
     {
         // Calculate observation jacobians and Kalman gains
@@ -1303,7 +1374,7 @@ void AttPosEKF::FuseMagnetometer()
             SH_MAG[7] = 2*magN*q0;
             SH_MAG[8] = 2*magE*q3;
 
-            for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0;
+            for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0;
             H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
             H_MAG[1] = SH_MAG[0];
             H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;
@@ -1360,7 +1431,7 @@ void AttPosEKF::FuseMagnetometer()
                 Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
                 Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
             } else {
-                for (uint8_t i=16; i < n_states; i++) {
+                for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) {
                     Kfusion[i] = 0;
                 }
             }
@@ -1370,7 +1441,7 @@ void AttPosEKF::FuseMagnetometer()
         else if (obsIndex == 1) // we are now fusing the Y measurement
         {
             // Calculate observation jacobians
-            for (unsigned int i = 0; i < n_states; i++) H_MAG[i] = 0;
+            for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0;
             H_MAG[0] = SH_MAG[2];
             H_MAG[1] = SH_MAG[1];
             H_MAG[2] = SH_MAG[0];
@@ -1439,7 +1510,7 @@ void AttPosEKF::FuseMagnetometer()
         else if (obsIndex == 2) // we are now fusing the Z measurement
         {
             // Calculate observation jacobians
-            for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0;
+            for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_MAG[i] = 0;
             H_MAG[0] = SH_MAG[1];
             H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
             H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
@@ -1512,12 +1583,12 @@ void AttPosEKF::FuseMagnetometer()
         if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f)
         {
             // correct the state vector
-            for (uint8_t j= 0; j < n_states; j++)
+            for (uint8_t j= 0; j < EKF_STATE_ESTIMATES; j++)
             {
                 states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
             }
             // normalise the quaternion states
-            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
             if (quatMag > 1e-12f)
             {
                 for (uint8_t j= 0; j<=3; j++)
@@ -1529,40 +1600,40 @@ void AttPosEKF::FuseMagnetometer()
             // correct the covariance P = (I - K*H)*P
             // take advantage of the empty columns in KH to reduce the
             // number of operations
-            for (uint8_t i = 0; i < n_states; i++)
+            for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
             {
                 for (uint8_t j = 0; j <= 3; j++)
                 {
                     KH[i][j] = Kfusion[i] * H_MAG[j];
                 }
                 for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f;
-                if (!onGround)
+                if (!_onGround)
                 {
-                    for (uint8_t j = 16; j < n_states; j++)
+                    for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++)
                     {
                         KH[i][j] = Kfusion[i] * H_MAG[j];
                     }
                 }
                 else
                 {
-                    for (uint8_t j = 16; j < n_states; j++)
+                    for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++)
                     {
                         KH[i][j] = 0.0f;
                     }
                 }
             }
-            for (uint8_t i = 0; i < n_states; i++)
+            for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
             {
-                for (uint8_t j = 0; j < n_states; j++)
+                for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
                 {
                     KHP[i][j] = 0.0f;
                     for (uint8_t k = 0; k <= 3; k++)
                     {
                         KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
                     }
-                    if (!onGround)
+                    if (!_onGround)
                     {
-                        for (uint8_t k = 16; k < n_states; k++)
+                        for (uint8_t k = 16; k < EKF_STATE_ESTIMATES; k++)
                         {
                             KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
                         }
@@ -1570,9 +1641,9 @@ void AttPosEKF::FuseMagnetometer()
                 }
             }
         }
-        for (uint8_t i = 0; i < n_states; i++)
+        for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
         {
-            for (uint8_t j = 0; j < n_states; j++)
+            for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
             {
                 P[i][j] = P[i][j] - KHP[i][j];
             }
@@ -1610,12 +1681,12 @@ void AttPosEKF::FuseAirspeed()
     if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
     {
         // Calculate observation jacobians
-        SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
+        SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
         SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
         SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
 
-        float H_TAS[n_states];
-        for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f;
+        float H_TAS[EKF_STATE_ESTIMATES];
+        for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_TAS[i] = 0.0f;
         H_TAS[4] = SH_TAS[2];
         H_TAS[5] = SH_TAS[1];
         H_TAS[6] = vd*SH_TAS[0];
@@ -1664,7 +1735,7 @@ void AttPosEKF::FuseAirspeed()
             Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
             Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
         } else {
-            for (uint8_t i=16; i < n_states; i++) {
+            for (uint8_t i=16; i < EKF_STATE_ESTIMATES; i++) {
                 Kfusion[i] = 0;
             }
         }
@@ -1676,12 +1747,12 @@ void AttPosEKF::FuseAirspeed()
         if ((innovVtas*innovVtas*SK_TAS) < 25.0f)
         {
             // correct the state vector
-            for (uint8_t j=0; j < n_states; j++)
+            for (uint8_t j=0; j < EKF_STATE_ESTIMATES; j++)
             {
                 states[j] = states[j] - Kfusion[j] * innovVtas;
             }
             // normalise the quaternion states
-            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
             if (quatMag > 1e-12f)
             {
                 for (uint8_t j= 0; j <= 3; j++)
@@ -1693,7 +1764,7 @@ void AttPosEKF::FuseAirspeed()
             // correct the covariance P = (I - K*H)*P
             // take advantage of the empty columns in H to reduce the
             // number of operations
-            for (uint8_t i = 0; i < n_states; i++)
+            for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
             {
                 for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0;
                 for (uint8_t j = 4; j <= 6; j++)
@@ -1705,11 +1776,11 @@ void AttPosEKF::FuseAirspeed()
                 {
                     KH[i][j] = Kfusion[i] * H_TAS[j];
                 }
-                for (uint8_t j = 16; j < n_states; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 16; j < EKF_STATE_ESTIMATES; j++) KH[i][j] = 0.0;
             }
-            for (uint8_t i = 0; i < n_states; i++)
+            for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
             {
-                for (uint8_t j = 0; j < n_states; j++)
+                for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
                 {
                     KHP[i][j] = 0.0;
                     for (uint8_t k = 4; k <= 6; k++)
@@ -1722,9 +1793,9 @@ void AttPosEKF::FuseAirspeed()
                     }
                 }
             }
-            for (uint8_t i = 0; i < n_states; i++)
+            for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
             {
-                for (uint8_t j = 0; j < n_states; j++)
+                for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
                 {
                     P[i][j] = P[i][j] - KHP[i][j];
                 }
@@ -1736,13 +1807,13 @@ void AttPosEKF::FuseAirspeed()
     ConstrainVariances();
 }
 
-void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+void AttPosEKF::zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last)
 {
     uint8_t row;
     uint8_t col;
     for (row=first; row<=last; row++)
     {
-        for (col=0; col<n_states; col++)
+        for (col=0; col<EKF_STATE_ESTIMATES; col++)
         {
             covMat[row][col] = 0.0;
         }
@@ -1765,13 +1836,13 @@ void AttPosEKF::FuseOptFlow()
     static float losPred[2];
 
     // Transformation matrix from nav to body axes
-    float H_LOS[2][n_states];
-    float K_LOS[2][n_states];
+    float H_LOS[2][EKF_STATE_ESTIMATES];
+    float K_LOS[2][EKF_STATE_ESTIMATES];
     Vector3f velNED_local;
     Vector3f relVelSensor;
 
     // Perform sequential fusion of optical flow measurements only with valid tilt and height
-    flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng);
+    flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
     float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9];
     bool validTilt = Tnb.z.z > 0.71f;
     if (validTilt)
@@ -1836,7 +1907,7 @@ void AttPosEKF::FuseOptFlow()
             tempVar[8] = (SK_LOS[4] + q0*tempVar[2]);
 
             // calculate observation jacobians for X LOS rate
-            for (uint8_t i = 0; i < n_states; i++) H_LOS[0][i] = 0;
+            for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_LOS[0][i] = 0;
             H_LOS[0][0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3];
             H_LOS[0][1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn);
             H_LOS[0][2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn);
@@ -1877,7 +1948,7 @@ void AttPosEKF::FuseOptFlow()
                 K_LOS[0][20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]);
                 K_LOS[0][21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]);
             } else {
-                for (uint8_t i = 16; i < n_states; i++) {
+                for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) {
                     K_LOS[0][i] = 0.0f;
                 }
             }
@@ -1898,7 +1969,7 @@ void AttPosEKF::FuseOptFlow()
             tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8];
 
             // Calculate observation jacobians for Y LOS rate
-            for (uint8_t i = 0; i < n_states; i++) H_LOS[1][i] = 0;
+            for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++) H_LOS[1][i] = 0;
             H_LOS[1][0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3];
             H_LOS[1][1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3];
             H_LOS[1][2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3];
@@ -1939,7 +2010,7 @@ void AttPosEKF::FuseOptFlow()
             K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]);
             K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]);
             } else {
-                for (uint8_t i = 16; i < n_states; i++) {
+                for (uint8_t i = 16; i < EKF_STATE_ESTIMATES; i++) {
                     K_LOS[1][i] = 0.0f;
                 }
             }
@@ -1955,12 +2026,12 @@ void AttPosEKF::FuseOptFlow()
             // Check the innovation for consistency and don't fuse if > 5Sigma
             if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) {
                 // correct the state vector
-                for (uint8_t j = 0; j < n_states; j++)
+                for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
                 {
                     states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex];
                 }
                 // normalise the quaternion states
-                float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+                float quatMag = sqrtf(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
                 if (quatMag > 1e-12f)
                 {
                     for (uint8_t j= 0; j<=3; j++)
@@ -1972,7 +2043,7 @@ void AttPosEKF::FuseOptFlow()
                 // correct the covariance P = (I - K*H)*P
                 // take advantage of the empty columns in KH to reduce the
                 // number of operations
-                for (uint8_t i = 0; i < n_states; i++)
+                for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
                 {
                     for (uint8_t j = 0; j <= 6; j++)
                     {
@@ -1983,14 +2054,14 @@ void AttPosEKF::FuseOptFlow()
                         KH[i][j] = 0.0f;
                     }
                     KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9];
-                    for (uint8_t j = 10; j < n_states; j++)
+                    for (uint8_t j = 10; j < EKF_STATE_ESTIMATES; j++)
                     {
                         KH[i][j] = 0.0f;
                     }
                 }
-                for (uint8_t i = 0; i < n_states; i++)
+                for (uint8_t i = 0; i < EKF_STATE_ESTIMATES; i++)
                 {
-                    for (uint8_t j = 0; j < n_states; j++)
+                    for (uint8_t j = 0; j < EKF_STATE_ESTIMATES; j++)
                     {
                         KHP[i][j] = 0.0f;
                         for (uint8_t k = 0; k <= 6; k++)
@@ -2000,9 +2071,9 @@ void AttPosEKF::FuseOptFlow()
                         KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
                     }
                 }
-                for (uint8_t i = 0; i <  n_states; i++)
+                for (uint8_t i = 0; i <  EKF_STATE_ESTIMATES; i++)
                 {
-                    for (uint8_t j = 0; j <  n_states; j++)
+                    for (uint8_t j = 0; j <  EKF_STATE_ESTIMATES; j++)
                     {
                         P[i][j] = P[i][j] - KHP[i][j];
                     }
@@ -2014,11 +2085,6 @@ void AttPosEKF::FuseOptFlow()
     }
 }
 
-/*
-Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
-This fiter requires optical flow rates that are not motion compensated
-Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
-*/
 void AttPosEKF::OpticalFlowEKF()
 {
     // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
@@ -2036,7 +2102,7 @@ void AttPosEKF::OpticalFlowEKF()
         } else {
             return;
         }
-        distanceTravelledSq = min(distanceTravelledSq, 100.0f);
+        distanceTravelledSq = std::min(distanceTravelledSq, 100.0f);
         Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
     }
 
@@ -2076,7 +2142,7 @@ void AttPosEKF::OpticalFlowEKF()
         varInnovRng = 1.0f/SK_RNG[1];
 
         // constrain terrain height to be below the vehicle
-        flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
+        flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
 
         // estimate range to centre of image
         range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
@@ -2096,7 +2162,7 @@ void AttPosEKF::OpticalFlowEKF()
             }
             // constrain the states
             flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
-            flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
+            flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
 
             // correct the covariance matrix
             float nextPopt[2][2];
@@ -2105,8 +2171,8 @@ void AttPosEKF::OpticalFlowEKF()
             nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
             nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
             // prevent the state variances from becoming negative and maintain symmetry
-            Popt[0][0] = maxf(nextPopt[0][0],0.0f);
-            Popt[1][1] = maxf(nextPopt[1][1],0.0f);
+            Popt[0][0] = std::max(nextPopt[0][0],0.0f);
+            Popt[1][1] = std::max(nextPopt[1][1],0.0f);
             Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
             Popt[1][0] = Popt[0][1];
         }
@@ -2145,7 +2211,7 @@ void AttPosEKF::OpticalFlowEKF()
         vel.z          = statesAtFlowTime[6];
 
         // constrain terrain height to be below the vehicle
-        flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng);
+        flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
 
         // estimate range to centre of image
         range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z;
@@ -2213,7 +2279,7 @@ void AttPosEKF::OpticalFlowEKF()
                 }
                 // constrain the states
                 flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
-                flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng);
+                flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
 
                 // correct the covariance matrix
                 for (uint8_t i = 0; i < 2 ; i++) {
@@ -2229,8 +2295,8 @@ void AttPosEKF::OpticalFlowEKF()
                 }
 
                 // prevent the state variances from becoming negative and maintain symmetry
-                Popt[0][0] = maxf(nextPopt[0][0],0.0f);
-                Popt[1][1] = maxf(nextPopt[1][1],0.0f);
+                Popt[0][0] = std::max(nextPopt[0][0],0.0f);
+                Popt[1][1] = std::max(nextPopt[1][1],0.0f);
                 Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
                 Popt[1][0] = Popt[0][1];
             }
@@ -2239,53 +2305,30 @@ void AttPosEKF::OpticalFlowEKF()
 
 }
 
-void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+void AttPosEKF::zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last)
 {
     uint8_t row;
     uint8_t col;
     for (col=first; col<=last; col++)
     {
-        for (row=0; row < n_states; row++)
+        for (row=0; row < EKF_STATE_ESTIMATES; row++)
         {
             covMat[row][col] = 0.0;
         }
     }
 }
 
-float AttPosEKF::sq(float valIn)
-{
-    return valIn*valIn;
-}
-
-float AttPosEKF::maxf(float valIn1, float valIn2)
-{
-    if (valIn1 >= valIn2) {
-        return valIn1;
-    } else {
-        return valIn2;
-    }
-}
-
-float AttPosEKF::min(float valIn1, float valIn2)
-{
-    if (valIn1 <= valIn2) {
-        return valIn1;
-    } else {
-        return valIn2;
-    }
-}
-
 // Store states in a history array along with time stamp
 void AttPosEKF::StoreStates(uint64_t timestamp_ms)
 {
-    for (unsigned i=0; i<n_states; i++)
+    for (size_t i=0; i<EKF_STATE_ESTIMATES; i++)
         storedStates[i][storeIndex] = states[i];
     storedOmega[0][storeIndex] = angRate.x;
     storedOmega[1][storeIndex] = angRate.y;
     storedOmega[2][storeIndex] = angRate.z;
     statetimeStamp[storeIndex] = timestamp_ms;
     storeIndex++;
-    if (storeIndex == data_buffer_size)
+    if (storeIndex == EKF_DATA_BUFFER_SIZE)
         storeIndex = 0;
 }
 
@@ -2311,8 +2354,8 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
     int ret = 0;
 
     int64_t bestTimeDelta = 200;
-    unsigned bestStoreIndex = 0;
-    for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++)
+    size_t bestStoreIndex = 0;
+    for (size_t storeIndexLocal = 0; storeIndexLocal < EKF_DATA_BUFFER_SIZE; storeIndexLocal++)
     {
         // Work around a GCC compiler bug - we know 64bit support on ARM is
         // sketchy in GCC.
@@ -2332,7 +2375,7 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
     }
     if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
     {
-        for (unsigned i=0; i < n_states; i++) {
+        for (size_t i=0; i < EKF_STATE_ESTIMATES; i++) {
             if (isfinite(storedStates[i][bestStoreIndex])) {
                 statesForFusion[i] = storedStates[i][bestStoreIndex];
             } else if (isfinite(states[i])) {
@@ -2346,7 +2389,7 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
     }
     else // otherwise output current state
     {
-        for (unsigned i = 0; i < n_states; i++) {
+        for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
             if (isfinite(states[i])) {
                 statesForFusion[i] = states[i];
             } else {
@@ -2361,25 +2404,25 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
 void AttPosEKF::RecallOmega(float* omegaForFusion, uint64_t msec)
 {
     // work back in time and calculate average angular rate over the time interval
-    for (unsigned i=0; i < 3; i++) {
+    for (size_t i=0; i < 3; i++) {
         omegaForFusion[i] = 0.0f;
     }
     uint8_t sumIndex = 0;
     int64_t timeDelta;
-    for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++)
+    for (size_t storeIndexLocal = 0; storeIndexLocal < EKF_DATA_BUFFER_SIZE; storeIndexLocal++)
     {
         // calculate the average of all samples younger than msec
         timeDelta = statetimeStamp[storeIndexLocal] - msec;
         if (timeDelta > 0)
         {
-            for (unsigned i=0; i < 3; i++) {
+            for (size_t i=0; i < 3; i++) {
                 omegaForFusion[i] += storedOmega[i][storeIndexLocal];
             }
             sumIndex += 1;
         }
     }
     if (sumIndex >= 1) {
-        for (unsigned i=0; i < 3; i++) {
+        for (size_t i=0; i < 3; i++) {
             omegaForFusion[i] = omegaForFusion[i] / float(sumIndex);
         }
     } else {
@@ -2389,6 +2432,7 @@ void AttPosEKF::RecallOmega(float* omegaForFusion, uint64_t msec)
     }
 }
 
+#if 0
 void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
 {
     // Calculate the nav to body cosine matrix
@@ -2413,6 +2457,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
     Tnb.x.z = 2*(q13 - q02);
     Tnb.y.z = 2*(q23 + q01);
 }
+#endif
 
 void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4])
 {
@@ -2481,37 +2526,41 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d
     hgt = hgtRef - posNED[2];
 }
 
-void AttPosEKF::OnGroundCheck()
+void AttPosEKF::setOnGround(const bool isLanded)
 {
-    onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
+    _onGround = isLanded;
 
     if (staticMode) {
         staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
     }
     // don't update wind states if there is no airspeed measurement
-    if (onGround || !useAirspeed) {
+    if (_onGround || !useAirspeed) {
         inhibitWindStates = true;
     } else {
         inhibitWindStates =false;
     }
+
     // don't update magnetic field states if on ground or not using compass
-    if (onGround || !useCompass) {
+    if (_onGround || !useCompass) {
         inhibitMagStates = true;
     } else {
         inhibitMagStates = false;
     }
+
     // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS
-    if ((onGround || !useGPS) && !useRangeFinder) {
+    if ((_onGround || !useGPS) && !useRangeFinder) {
         inhibitGndState = true;
     } else {
         inhibitGndState = false;
     }
+
     // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable
-    if ((onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) {
+    if ((_onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) {
         inhibitGndState = true;
     } else {
         inhibitGndState = false;
     }
+
     // Don't update focal length offset state if there is no range finder or optical flow sensor
     // we need both sensors to do this estimation
     if (!useRangeFinder || !useOpticalFlow) {
@@ -2596,22 +2645,22 @@ void AttPosEKF::ConstrainVariances()
     // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
     // Constrain quaternion variances
-    for (unsigned i = 0; i <= 3; i++) {
+    for (size_t i = 0; i <= 3; i++) {
         P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
     }
 
     // Constrain velocity variances
-    for (unsigned i = 4; i <= 6; i++) {
+    for (size_t i = 4; i <= 6; i++) {
         P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
     }
 
     // Constrain position variances
-    for (unsigned i = 7; i <= 9; i++) {
+    for (size_t i = 7; i <= 9; i++) {
         P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f);
     }
 
     // Constrain delta angle bias variances
-    for (unsigned i = 10; i <= 12; i++) {
+    for (size_t i = 10; i <= 12; i++) {
         P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU));
     }
 
@@ -2619,17 +2668,17 @@ void AttPosEKF::ConstrainVariances()
     P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU));
 
     // Wind velocity variances
-    for (unsigned i = 14; i <= 15; i++) {
+    for (size_t i = 14; i <= 15; i++) {
         P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
     }
 
     // Earth magnetic field variances
-    for (unsigned i = 16; i <= 18; i++) {
+    for (size_t i = 16; i <= 18; i++) {
         P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
     }
 
     // Body magnetic field variances
-    for (unsigned i = 19; i <= 21; i++) {
+    for (size_t i = 19; i <= 21; i++) {
         P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
     }
 
@@ -2652,17 +2701,17 @@ void AttPosEKF::ConstrainStates()
     // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
     // Constrain quaternion
-    for (unsigned i = 0; i <= 3; i++) {
+    for (size_t i = 0; i <= 3; i++) {
         states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
     }
 
     // Constrain velocities to what GPS can do for us
-    for (unsigned i = 4; i <= 6; i++) {
+    for (size_t i = 4; i <= 6; i++) {
         states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f);
     }
 
     // Constrain position to a reasonable vehicle range (in meters)
-    for (unsigned i = 7; i <= 8; i++) {
+    for (size_t i = 7; i <= 8; i++) {
         states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f);
     }
 
@@ -2671,7 +2720,7 @@ void AttPosEKF::ConstrainStates()
     states[9] = ConstrainFloat(states[9], -4.0e4f, 4.0e4f);
 
     // Angle bias limit - set to 8 degrees / sec
-    for (unsigned i = 10; i <= 12; i++) {
+    for (size_t i = 10; i <= 12; i++) {
         states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU);
     }
 
@@ -2679,18 +2728,18 @@ void AttPosEKF::ConstrainStates()
     states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU);
 
     // Wind velocity limits - assume 120 m/s max velocity
-    for (unsigned i = 14; i <= 15; i++) {
+    for (size_t i = 14; i <= 15; i++) {
         states[i] = ConstrainFloat(states[i], -120.0f, 120.0f);
     }
 
     // Earth magnetic field limits (in Gauss)
-    for (unsigned i = 16; i <= 18; i++) {
+    for (size_t i = 16; i <= 18; i++) {
         states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
     }
 
     // Body magnetic field variances (in Gauss).
     // the max offset should be in this range.
-    for (unsigned i = 19; i <= 21; i++) {
+    for (size_t i = 19; i <= 21; i++) {
         states[i] = ConstrainFloat(states[i], -0.5f, 0.5f);
     }
 
@@ -2704,7 +2753,7 @@ void AttPosEKF::ForceSymmetry()
 
     // Force symmetry on the covariance matrix to prevent ill-conditioning
     // of the matrix which would cause the filter to blow-up
-    for (unsigned i = 1; i < n_states; i++)
+    for (size_t i = 1; i < EKF_STATE_ESTIMATES; i++)
     {
         for (uint8_t j = 0; j < i; j++)
         {
@@ -2792,12 +2841,6 @@ bool AttPosEKF::FilterHealthy()
     return true;
 }
 
-/**
- * Reset the filter position states
- *
- * This resets the position to the last GPS measurement
- * or to zero in case of static position.
- */
 void AttPosEKF::ResetPosition(void)
 {
     if (staticMode) {
@@ -2808,23 +2851,26 @@ void AttPosEKF::ResetPosition(void)
         // reset the states from the GPS measurements
         states[7] = posNE[0];
         states[8] = posNE[1];
+
+        // stored horizontal position states to prevent subsequent GPS measurements from being rejected
+        for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
+            storedStates[7][i] = states[7];
+            storedStates[8][i] = states[8];
+        }        
     }
 }
 
-/**
- * Reset the height state.
- *
- * This resets the height state with the last altitude measurements
- */
 void AttPosEKF::ResetHeight(void)
 {
     // write to the state vector
     states[9]   = -hgtMea;
+
+    // stored horizontal position states to prevent subsequent Barometer measurements from being rejected
+    for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
+        storedStates[9][i] = states[9];
+    }    
 }
 
-/**
- * Reset the velocity state.
- */
 void AttPosEKF::ResetVelocity(void)
 {
     if (staticMode) {
@@ -2832,10 +2878,16 @@ void AttPosEKF::ResetVelocity(void)
         states[5] = 0.0f;
         states[6] = 0.0f;
     } else if (GPSstatus >= GPS_FIX_3D) {
+        //Do not use Z velocity, we trust the Barometer history more
 
         states[4]  = velNED[0]; // north velocity from last reading
         states[5]  = velNED[1]; // east velocity from last reading
-        states[6]  = velNED[2]; // down velocity from last reading
+
+        // stored horizontal position states to prevent subsequent GPS measurements from being rejected
+        for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; ++i){
+            storedStates[4][i] = states[4];
+            storedStates[5][i] = states[5];
+        }                
     }
 }
 
@@ -2865,8 +2917,8 @@ bool AttPosEKF::StatesNaN() {
     } // delta velocities
 
     // check all states and covariance matrices
-    for (unsigned i = 0; i < n_states; i++) {
-        for (unsigned j = 0; j < n_states; j++) {
+    for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
+        for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) {
             if (!isfinite(KH[i][j])) {
 
                 current_ekf_state.KHNaN = true;
@@ -2917,15 +2969,6 @@ out:
 
 }
 
-/**
- * Check the filter inputs and bound its operational state
- *
- * This check will reset the filter states if required
- * due to a failure of consistency or timeout checks.
- * it should be run after the measurement data has been
- * updated, but before any of the fusion steps are
- * executed.
- */
 int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
 {
 
@@ -2944,9 +2987,6 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
 
     int ret = 0;
 
-    // Check if we're on ground - this also sets static mode.
-    OnGroundCheck();
-
     // Reset the filter if the states went NaN
     if (StatesNaN()) {
         ekf_debug("re-initializing dynamic");
@@ -2965,10 +3005,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
         // Fill error report
         GetFilterState(&last_ekf_error);
 
+        ResetStoredStates();
         ResetVelocity();
         ResetPosition();
         ResetHeight();
-        ResetStoredStates();
 
         // Timeout cleared with this reset
         current_ekf_state.imuTimeout = false;
@@ -2982,10 +3022,10 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
         // Fill error report, but not setting error flag
         GetFilterState(&last_ekf_error);
 
+        ResetStoredStates();
         ResetVelocity();
         ResetPosition();
         ResetHeight();
-        ResetStoredStates();
 
         ret = 0;
     }
@@ -3155,6 +3195,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     states[20] = magBias.y; // Magnetic Field Bias Y
     states[21] = magBias.z; // Magnetic Field Bias Z
 
+    ResetStoredStates();
     ResetVelocity();
     ResetPosition();
     ResetHeight();
@@ -3206,8 +3247,8 @@ void AttPosEKF::ZeroVariables()
     lastVelPosFusion = millis();
 
     // Do the data structure init
-    for (unsigned i = 0; i < n_states; i++) {
-        for (unsigned j = 0; j < n_states; j++) {
+    for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
+        for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) {
             KH[i][j] = 0.0f; //  intermediate result used for covariance updates
             KHP[i][j] = 0.0f; // intermediate result used for covariance updates
             P[i][j] = 0.0f; // covariance matrix
@@ -3231,9 +3272,9 @@ void AttPosEKF::ZeroVariables()
     flowStates[0] = 1.0f;
     flowStates[1] = 0.0f;
 
-    for (unsigned i = 0; i < data_buffer_size; i++) {
+    for (size_t i = 0; i < EKF_DATA_BUFFER_SIZE; i++) {
 
-        for (unsigned j = 0; j < n_states; j++) {
+        for (size_t j = 0; j < EKF_STATE_ESTIMATES; j++) {
             storedStates[j][i] = 0.0f;
         }
 
@@ -3252,11 +3293,11 @@ void AttPosEKF::GetFilterState(struct ekf_status_report *err)
 {
 
     // Copy states
-    for (unsigned i = 0; i < n_states; i++) {
+    for (size_t i = 0; i < EKF_STATE_ESTIMATES; i++) {
         current_ekf_state.states[i] = states[i];
     }
-    current_ekf_state.n_states = n_states;
-    current_ekf_state.onGround = onGround;
+    current_ekf_state.n_states = EKF_STATE_ESTIMATES;
+    current_ekf_state.onGround = _onGround;
     current_ekf_state.staticMode = staticMode;
     current_ekf_state.useCompass = useCompass;
     current_ekf_state.useAirspeed = useAirspeed;
diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index b1d71bd74..e15ded977 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_22states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -1,9 +1,49 @@
+/****************************************************************************
+* Copyright (c) 2014, Paul Riseborough All rights reserved.
+* 
+* Redistribution and use in source and binary forms, with or without 
+* modification, are permitted provided that the following conditions are met:
+* 
+* Redistributions of source code must retain the above copyright notice, this 
+* list of conditions and the following disclaimer.
+* 
+* Redistributions in binary form must reproduce the above copyright notice, 
+* this list of conditions and the following disclaimer in the documentation 
+* and/or other materials provided with the distribution.
+* 
+* Neither the name of the {organization} nor the names of its contributors 
+* may be used to endorse or promote products derived from this software without 
+* specific prior written permission.
+* 
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
+* POSSIBILITY OF SUCH DAMAGE.
+****************************************************************************/
+
+/**
+ * @file estimator_22states.h
+ *
+ * Definition of the attitude and position estimator.
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
 #pragma once
 
 #include "estimator_utilities.h"
+#include <cstddef>
 
-const unsigned int n_states = 22;
-const unsigned int data_buffer_size = 50;
+constexpr size_t EKF_STATE_ESTIMATES = 22;
+constexpr size_t EKF_DATA_BUFFER_SIZE = 50;
 
 class AttPosEKF {
 
@@ -22,7 +62,7 @@ public:
 
     /*
      * parameters are defined here and initialised in
-     * the InitialiseParameters() (which is just 20 lines down)
+     * the InitialiseParameters()
      */
 
     float covTimeStepMax; // maximum time allowed between covariance predictions
@@ -49,40 +89,6 @@ public:
 
     float EAS2TAS; // ratio f true to equivalent airspeed
 
-    void InitialiseParameters()
-    {
-        covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
-        covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
-        rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
-        EAS2TAS = 1.0f;
-
-        yawVarScale = 1.0f;
-        windVelSigma = 0.1f;
-        dAngBiasSigma = 5.0e-7f;
-        dVelBiasSigma = 1e-4f;
-        magEarthSigma = 3.0e-4f;
-        magBodySigma  = 3.0e-4f;
-
-        vneSigma = 0.2f;
-        vdSigma = 0.3f;
-        posNeSigma = 2.0f;
-        posDSigma = 2.0f;
-
-        magMeasurementSigma = 0.05;
-        airspeedMeasurementSigma = 1.4f;
-        gyroProcessNoise = 1.4544411e-2f;
-        accelProcessNoise = 0.5f;
-
-        gndHgtSigma  = 0.1f; // terrain gradient 1-sigma
-        R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2
-        flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
-        auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
-        rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check
-        minFlowRng = 0.3f; //minimum range between ground and flow sensor
-        moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate
-
-    }
-
     struct mag_state_struct {
         unsigned obsIndex;
         float MagPred[3];
@@ -108,25 +114,25 @@ public:
 
 
     // Global variables
-    float KH[n_states][n_states]; //  intermediate result used for covariance updates
-    float KHP[n_states][n_states]; // intermediate result used for covariance updates
-    float P[n_states][n_states]; // covariance matrix
-    float Kfusion[n_states]; // Kalman gains
-    float states[n_states]; // state matrix
-    float resetStates[n_states];
-    float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
-    uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
+    float KH[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; //  intermediate result used for covariance updates
+    float KHP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates
+    float P[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // covariance matrix
+    float Kfusion[EKF_STATE_ESTIMATES]; // Kalman gains
+    float states[EKF_STATE_ESTIMATES]; // state matrix
+    float resetStates[EKF_STATE_ESTIMATES];
+    float storedStates[EKF_STATE_ESTIMATES][EKF_DATA_BUFFER_SIZE]; // state vectors stored for the last 50 time steps
+    uint32_t statetimeStamp[EKF_DATA_BUFFER_SIZE]; // time stamp for each state vector stored
 
     // Times
     uint64_t lastVelPosFusion;  // the time of the last velocity fusion, in the standard time unit of the filter
 
-    float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
-    float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
-    float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
-    float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
-    float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
-    float statesAtRngTime[n_states]; // filter states at the effective measurement time
-    float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
+    float statesAtVelTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtPosTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtHgtTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for the hgtMea measurement
+    float statesAtMagMeasTime[EKF_STATE_ESTIMATES]; // filter satates at the effective measurement time
+    float statesAtVtasMeasTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time
+    float statesAtRngTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time
+    float statesAtFlowTime[EKF_STATE_ESTIMATES]; // States at the effective optical flow measurement time
     float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time
 
     Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
@@ -156,7 +162,6 @@ public:
     float varInnovVelPos[6]; // innovation variance output
 
     float velNED[3]; // North, East, Down velocity obs (m/s)
-    float accelGPSNED[3];   // Acceleration predicted by GPS in earth frame
     float posNE[2]; // North, East position obs (m)
     float hgtMea; //  measured height (m)
     float baroHgtOffset;        ///< the baro (weather) offset from normalized altitude
@@ -208,7 +213,6 @@ public:
     bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
     bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
 
-    bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
     bool staticMode;    ///< boolean true if no position feedback is fused
     bool useGPS; // boolean true if GPS data is being used
     bool useAirspeed;    ///< boolean true if airspeed data is being used
@@ -227,7 +231,7 @@ public:
     unsigned storeIndex;
 
     // Optical Flow error estimation
-    float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
+    float storedOmega[3][EKF_DATA_BUFFER_SIZE]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
 
     // Two state EKF used to estimate focal length scale factor and terrain position
     float Popt[2][2];                       // state covariance matrix
@@ -247,115 +251,157 @@ public:
     float minFlowRng;                       // minimum range over which to fuse optical flow measurements
     float moCompR_LOS;                      // scaler from sensor gyro rate to uncertainty in LOS rate
 
-void updateDtGpsFilt(float dt);
+    void updateDtGpsFilt(float dt);
 
-void updateDtHgtFilt(float dt);
+    void updateDtHgtFilt(float dt);
 
-void  UpdateStrapdownEquationsNED();
+    void UpdateStrapdownEquationsNED();
 
-void CovariancePrediction(float dt);
+    void CovariancePrediction(float dt);
 
-void FuseVelposNED();
+    void FuseVelposNED();
 
-void FuseMagnetometer();
+    void FuseMagnetometer();
 
-void FuseAirspeed();
+    void FuseAirspeed();
 
-void FuseOptFlow();
+    void FuseOptFlow();
 
-void OpticalFlowEKF();
+    /**
+    * @brief
+    *    Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
+    *    This fiter requires optical flow rates that are not motion compensated
+    *    Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
+    **/
+    void OpticalFlowEKF();
 
-void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+    void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
 
-void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+    void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
 
-void quatNorm(float (&quatOut)[4], const float quatIn[4]);
+    void quatNorm(float (&quatOut)[4], const float quatIn[4]);
 
-// store staes along with system time stamp in msces
-void StoreStates(uint64_t timestamp_ms);
-
-/**
- * Recall the state vector.
- *
- * Recalls the vector stored at closest time to the one specified by msec
- *FuseOptFlow
- * @return zero on success, integer indicating the number of invalid states on failure.
- *         Does only copy valid states, if the statesForFusion vector was initialized
- *         correctly by the caller, the result can be safely used, but is a mixture
- *         time-wise where valid states were updated and invalid remained at the old
- *         value.
- */
-int RecallStates(float *statesForFusion, uint64_t msec);
+    // store staes along with system time stamp in msces
+    void StoreStates(uint64_t timestamp_ms);
 
-void RecallOmega(float *omegaForFusion, uint64_t msec);
+    /**
+     * Recall the state vector.
+     *
+     * Recalls the vector stored at closest time to the one specified by msec
+     *FuseOptFlow
+     * @return zero on success, integer indicating the number of invalid states on failure.
+     *         Does only copy valid states, if the statesForFusion vector was initialized
+     *         correctly by the caller, the result can be safely used, but is a mixture
+     *         time-wise where valid states were updated and invalid remained at the old
+     *         value.
+     */
+    int RecallStates(float *statesForFusion, uint64_t msec);
 
-void ResetStoredStates();
+    void RecallOmega(float *omegaForFusion, uint64_t msec);
 
-void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
+    void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
 
-void calcEarthRateNED(Vector3f &omega, float latitude);
+    void calcEarthRateNED(Vector3f &omega, float latitude);
 
-static void eul2quat(float (&quat)[4], const float (&eul)[3]);
+    static void eul2quat(float (&quat)[4], const float (&eul)[3]);
 
-static void quat2eul(float (&eul)[3], const float (&quat)[4]);
+    static void quat2eul(float (&eul)[3], const float (&quat)[4]);
 
-static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+    static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
 
-void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
+    static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
 
-static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
+    static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
 
-static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
+    //static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
 
-static float sq(float valIn);
+    static inline float sq(float valIn) {return valIn * valIn;}
 
-static float maxf(float valIn1, float valIn2);
+    /**
+    * @brief
+    *   Tell the EKF if the vehicle has landed
+    **/
+    void setOnGround(const bool isLanded);
 
-static float min(float valIn1, float valIn2);
+    void CovarianceInit();
 
-void OnGroundCheck();
+    void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
 
-void CovarianceInit();
+    float ConstrainFloat(float val, float min, float maxf);
 
-void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
+    void ConstrainVariances();
 
-float ConstrainFloat(float val, float min, float maxf);
+    void ConstrainStates();
 
-void ConstrainVariances();
+    void ForceSymmetry();
 
-void ConstrainStates();
+    /**
+    * @brief
+    *   Check the filter inputs and bound its operational state
+    *  
+    *   This check will reset the filter states if required
+    *   due to a failure of consistency or timeout checks.
+    *   it should be run after the measurement data has been
+    *   updated, but before any of the fusion steps are
+    *   executed.
+    */
+    int CheckAndBound(struct ekf_status_report *last_error);
 
-void ForceSymmetry();
+    /**
+     * @brief
+     *   Reset the filter position states
+     *  
+     *   This resets the position to the last GPS measurement
+     *   or to zero in case of static position.
+     */
+    void ResetPosition();
 
-int CheckAndBound(struct ekf_status_report *last_error);
+    /**
+     * @brief
+     *   Reset the velocity state.
+     */
+    void ResetVelocity();
 
-void ResetPosition();
+    void ZeroVariables();
 
-void ResetVelocity();
+    void GetFilterState(struct ekf_status_report *state);
 
-void ZeroVariables();
+    void GetLastErrorState(struct ekf_status_report *last_error);
 
-void GetFilterState(struct ekf_status_report *state);
+    bool StatesNaN();
 
-void GetLastErrorState(struct ekf_status_report *last_error);
+    void InitializeDynamic(float (&initvelNED)[3], float declination);
 
-bool StatesNaN();
+protected:
 
-void InitializeDynamic(float (&initvelNED)[3], float declination);
+    /**
+    * @brief 
+    *   Initializes algorithm parameters to safe default values
+    **/
+    void InitialiseParameters();
 
-protected:
+    void updateDtVelPosFilt(float dt);
 
-void updateDtVelPosFilt(float dt);
+    bool FilterHealthy();
 
-bool FilterHealthy();
+    bool GyroOffsetsDiverged();
 
-bool GyroOffsetsDiverged();
+    bool VelNEDDiverged();
 
-bool VelNEDDiverged();
+    /**
+     * @brief
+     *   Reset the height state.
+     *  
+     *   This resets the height state with the last altitude measurements
+     */
+    void ResetHeight();
 
-void ResetHeight(void);
+    void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
 
-void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
+    void ResetStoredStates();
+    
+private:
+    bool _onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
 
 };
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
index 77cc1eeeb..e2f4c7e82 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp
@@ -1,3 +1,41 @@
+/****************************************************************************
+* Copyright (c) 2014, Paul Riseborough All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* Redistributions of source code must retain the above copyright notice, this
+* list of conditions and the following disclaimer.
+*
+* Redistributions in binary form must reproduce the above copyright notice,
+* this list of conditions and the following disclaimer in the documentation
+* and/or other materials provided with the distribution.
+*
+* Neither the name of the {organization} nor the names of its contributors
+* may be used to endorse or promote products derived from this software without
+* specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+****************************************************************************/
+
+/**
+ * @file estimator_utilities.cpp
+ *
+ * Estimator support utilities.
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
 
 #include "estimator_utilities.h"
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
index a6b670c4d..95b83ead4 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_utilities.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h
@@ -1,8 +1,47 @@
-#include <math.h>
-#include <stdint.h>
+/****************************************************************************
+* Copyright (c) 2014, Paul Riseborough All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* Redistributions of source code must retain the above copyright notice, this
+* list of conditions and the following disclaimer.
+*
+* Redistributions in binary form must reproduce the above copyright notice,
+* this list of conditions and the following disclaimer in the documentation
+* and/or other materials provided with the distribution.
+*
+* Neither the name of the {organization} nor the names of its contributors
+* may be used to endorse or promote products derived from this software without
+* specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+****************************************************************************/
+
+/**
+ * @file estimator_utilities.h
+ *
+ * Estimator support utilities.
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
 
 #pragma once
 
+#include <math.h>
+#include <stdint.h>
+
 #define GRAVITY_MSS 9.80665f
 #define deg2rad 0.017453292f
 #define rad2deg 57.295780f