diff --git a/make/source.mk b/make/source.mk
index e1a76eea0ff..534877f409b 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -163,6 +163,8 @@ COMMON_SRC = \
             navigation/navigation_geo.c \
             navigation/navigation_multicopter.c \
             navigation/navigation_pos_estimator.c \
+            navigation/navigation_pos_estimator_agl.c \
+            navigation/navigation_pos_estimator_flow.c \
             sensors/barometer.c \
             sensors/pitotmeter.c \
             sensors/rangefinder.c \
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 6843ffd2efe..7c76ba2ceba 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -54,6 +54,7 @@ typedef enum {
     DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
     DEBUG_AGL,
     DEBUG_FLOW_RAW,
+    DEBUG_FLOW,
     DEBUG_SBUS,
     DEBUG_FPORT,
     DEBUG_ALWAYS,
diff --git a/src/main/common/vector.h b/src/main/common/vector.h
index fdb2ce29d46..391fa042655 100644
--- a/src/main/common/vector.h
+++ b/src/main/common/vector.h
@@ -41,6 +41,13 @@ typedef struct {
 void rotationMatrixFromAngles(fpMat3_t * rmat, const fp_angles_t * angles);
 void rotationMatrixFromAxisAngle(fpMat3_t * rmat, const fpAxisAngle_t * a);
 
+static inline void vectorZero(fpVector3_t * v)
+{ 
+    v->x = 0.0f;
+    v->y = 0.0f;
+    v->z = 0.0f;
+}
+
 static inline fpVector3_t * rotationMatrixRotateVector(fpVector3_t * result, const fpVector3_t * a, const fpMat3_t * rmat)
 {
     fpVector3_t r;
diff --git a/src/main/drivers/opflow/opflow.h b/src/main/drivers/opflow/opflow.h
index f78326761bc..ef3033b9403 100755
--- a/src/main/drivers/opflow/opflow.h
+++ b/src/main/drivers/opflow/opflow.h
@@ -31,7 +31,7 @@ struct opflowDev_s;
 
 typedef struct opflowData_s {
     timeDelta_t deltaTime;      // Integration timeframe of motionX/Y
-    int32_t     flowRateRaw[2]; // Flow rotation in raw sensor uints (per deltaTime interval)
+    int32_t     flowRateRaw[3]; // Flow rotation in raw sensor uints (per deltaTime interval). Use dummy 3-rd axis (always zero) for compatibility with alignment functions
     int16_t     quality;
 } opflowData_t;
 
diff --git a/src/main/drivers/opflow/opflow_fake.c b/src/main/drivers/opflow/opflow_fake.c
index 0dc6ae085e8..517b14cfe15 100755
--- a/src/main/drivers/opflow/opflow_fake.c
+++ b/src/main/drivers/opflow/opflow_fake.c
@@ -43,6 +43,7 @@ void fakeOpflowSet(timeDelta_t deltaTime, int32_t flowRateX, int32_t flowRateY,
     fakeData.deltaTime = deltaTime;
     fakeData.flowRateRaw[0] = flowRateX;
     fakeData.flowRateRaw[1] = flowRateY;
+    fakeData.flowRateRaw[2] = 0;
     fakeData.quality = quality;
 }
 
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
index 5cfe9a35bf6..79704a7aa9d 100644
--- a/src/main/fc/fc_msp_box.c
+++ b/src/main/fc/fc_msp_box.c
@@ -182,12 +182,18 @@ void initActiveBoxIds(void)
         activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
     }
 
-    if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
+    // if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || 
+        // ((1) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || 
+        // (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
+    if (sensors(SENSOR_ACC)) {
         activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
         activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
         activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
         activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
-        activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
+
+        if (feature(FEATURE_GPS)) {
+            activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
+        }
     }
 #endif
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 02517b66f97..0d724f8d296 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -67,7 +67,7 @@ tables:
   - name: i2c_speed
     values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
   - name: debug_modes
-    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"]
+    values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR", "SAG_COMP_VOLTAGE"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -231,6 +231,9 @@ groups:
     members:
       - name: opflow_hardware
         table: opflow_hardware
+      - name: opflow_align
+        type: uint8_t
+        table: alignment
       - name: opflow_scale
         min: 0
         max: 10000
@@ -1082,6 +1085,9 @@ groups:
       - name: inav_use_gps_velned
         field: use_gps_velned
         type: bool
+      - name: inav_allow_dead_reckoning
+        field: allow_dead_reckoning
+        type: bool
       - name: inav_reset_altitude
         field: reset_altitude_type
         table: reset_type
@@ -1100,6 +1106,14 @@ groups:
         field: w_z_surface_v
         min: 0
         max: 10
+      - name: inav_w_xy_flow_p
+        field: w_xy_flow_p
+        min: 0
+        max: 10
+      - name: inav_w_xy_flow_v
+        field: w_xy_flow_v
+        min: 0
+        max: 10
       - name: inav_w_z_baro_p
         field: w_z_baro_p
         min: 0
diff --git a/src/main/io/opflow_cxof.c b/src/main/io/opflow_cxof.c
index fae1e65b511..a6d1bab3147 100755
--- a/src/main/io/opflow_cxof.c
+++ b/src/main/io/opflow_cxof.c
@@ -113,6 +113,7 @@ static bool cxofOpflowUpdate(opflowData_t * data)
                     tmpData.deltaTime += (currentTimeUs - previousTimeUs);
                     tmpData.flowRateRaw[0] += pkt->motionX;
                     tmpData.flowRateRaw[1] += pkt->motionY;
+                    tmpData.flowRateRaw[2] = 0;
                     tmpData.quality = (constrain(pkt->squal, 64, 78) - 64) * 100 / 14;
 
                     previousTimeUs = currentTimeUs;
diff --git a/src/main/io/opflow_msp.c b/src/main/io/opflow_msp.c
index 92af18b9934..029fa095737 100644
--- a/src/main/io/opflow_msp.c
+++ b/src/main/io/opflow_msp.c
@@ -81,6 +81,7 @@ void mspOpflowReceiveNewData(uint8_t * bufferPtr)
     sensorData.deltaTime = currentTimeUs - updatedTimeUs;
     sensorData.flowRateRaw[0] = pkt->motionX;
     sensorData.flowRateRaw[1] = pkt->motionY;
+    sensorData.flowRateRaw[2] = 0;
     sensorData.quality = (int)pkt->quality * 100 / 255;
     hasNewData = true;
 
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index c38fbec6b3b..ce387f85cc5 100755
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -173,7 +173,6 @@ static void setupAltitudeController(void);
 static void resetHeadingController(void);
 void resetGCSFlags(void);
 
-static bool posEstimationHasGlobalReference(void);
 static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
 static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
 void calculateInitialHoldPosition(fpVector3_t * pos);
@@ -750,7 +749,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
 {
     navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
 
-    if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME) || !posEstimationHasGlobalReference()) {
+    if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) {
         // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
         // If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
@@ -1440,7 +1439,7 @@ bool checkForPositionSensorTimeout(void)
 /*-----------------------------------------------------------
  * Processes an update to XY-position and velocity
  *-----------------------------------------------------------*/
-void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
+void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY)
 {
     posControl.actualState.abs.pos.x = newX;
     posControl.actualState.abs.pos.y = newY;
@@ -1454,13 +1453,24 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f
 
     posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
 
-    if (estimateValid) {
+    // CASE 1: POS & VEL valid
+    if (estPosValid && estVelValid) {
         posControl.flags.estPosStatus = EST_TRUSTED;
+        posControl.flags.estVelStatus = EST_TRUSTED;
+        posControl.flags.horizontalPositionDataNew = 1;
+        posControl.lastValidPositionTimeMs = millis();
+    }
+    // CASE 1: POS invalid, VEL valid
+    else if (!estPosValid && estVelValid) {
+        posControl.flags.estPosStatus = EST_USABLE;     // Pos usable, but not trusted
+        posControl.flags.estVelStatus = EST_TRUSTED;
         posControl.flags.horizontalPositionDataNew = 1;
         posControl.lastValidPositionTimeMs = millis();
     }
+    // CASE 3: can't use pos/vel data
     else {
         posControl.flags.estPosStatus = EST_NONE;
+        posControl.flags.estVelStatus = EST_NONE;
         posControl.flags.horizontalPositionDataNew = 0;
     }
 
@@ -2089,7 +2099,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
     // WP #255 - special waypoint - directly set desiredPosition
     // Only valid when armed and in poshold mode
     else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
-             ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
+             ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
              (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
         // Convert to local coordinates
         geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
@@ -2297,7 +2307,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
 #if defined(NAV_BLACKBOX)
     navFlags = 0;
     if (posControl.flags.estAltStatus == EST_TRUSTED)       navFlags |= (1 << 0);
-    if (posControl.flags.estAglStatus == EST_TRUSTED)   navFlags |= (1 << 1);
+    if (posControl.flags.estAglStatus == EST_TRUSTED)       navFlags |= (1 << 1);
     if (posControl.flags.estPosStatus == EST_TRUSTED)       navFlags |= (1 << 2);
 #if defined(NAV_GPS_GLITCH_DETECTION)
     if (isGPSGlitchDetected())                              navFlags |= (1 << 4);
@@ -2368,12 +2378,12 @@ static bool canActivateAltHoldMode(void)
 
 static bool canActivatePosHoldMode(void)
 {
-    return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
+    return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
 }
 
-static bool posEstimationHasGlobalReference(void)
+static bool canActivateNavigationModes(void)
 {
-    return true;    // For now assume that we always have global coordinates available
+    return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
 }
 
 static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
@@ -2389,8 +2399,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         }
 
         // Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
-        bool canActivateAltHold = canActivateAltHoldMode();
-        bool canActivatePosHold = canActivatePosHoldMode();
+        bool canActivateAltHold    = canActivateAltHoldMode();
+        bool canActivatePosHold    = canActivatePosHoldMode();
+        bool canActivateNavigation = canActivateNavigationModes();
 
         // LAUNCH mode has priority over any other NAV mode
         if (STATE(FIXED_WING)) {
@@ -2417,7 +2428,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         }
 
         // RTH/Failsafe_RTH can override MANUAL
-        if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME))) {
+        if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
             // If we request forced RTH - attempt to activate it no matter what
             // This might switch to emergency landing controller if GPS is unavailable
             canActivateWaypoint = false;    // Block WP mode if we switched to RTH for whatever reason
@@ -2431,7 +2442,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         }
 
         if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
-            if ((FLIGHT_MODE(NAV_WP_MODE)) || (posEstimationHasGlobalReference() && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
+            if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateNavigation && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
                 return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
         }
         else {
@@ -2670,6 +2681,7 @@ void navigationInit(void)
 
     posControl.flags.estAltStatus = EST_NONE;
     posControl.flags.estPosStatus = EST_NONE;
+    posControl.flags.estVelStatus = EST_NONE;
     posControl.flags.estHeadingStatus = EST_NONE;
     posControl.flags.estAglStatus = EST_NONE;
 
diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h
index fb30fa33ed1..fe507ab2c3d 100755
--- a/src/main/navigation/navigation.h
+++ b/src/main/navigation/navigation.h
@@ -80,6 +80,7 @@ typedef struct positionEstimationConfig_s {
     uint8_t reset_home_type; // nav_reset_type_e
     uint8_t gravity_calibration_tolerance;    // Tolerance of gravity calibration (cm/s/s)
     uint8_t use_gps_velned;
+    uint8_t allow_dead_reckoning;
 
     uint16_t max_surface_altitude;
 
@@ -94,6 +95,9 @@ typedef struct positionEstimationConfig_s {
     float w_xy_gps_p;   // Weight (cutoff frequency) for GPS position measurements
     float w_xy_gps_v;   // Weight (cutoff frequency) for GPS velocity measurements
 
+    float w_xy_flow_p;
+    float w_xy_flow_v;
+
     float w_z_res_v;    // When velocity sources lost slowly decrease estimated velocity with this weight
     float w_xy_res_v;
 
@@ -279,9 +283,20 @@ bool navIsCalibrationComplete(void);
 bool navigationTerrainFollowingEnabled(void);
 
 /* Access to estimated position and velocity */
+typedef struct {
+    uint8_t altStatus;
+    uint8_t posStatus;
+    uint8_t velStatus;
+    uint8_t aglStatus;
+    fpVector3_t pos;
+    fpVector3_t vel;
+    float agl;
+} navPositionAndVelocity_t;
+
 float getEstimatedActualVelocity(int axis);
 float getEstimatedActualPosition(int axis);
 int32_t getTotalTravelDistance(void);
+void getEstimatedPositionAndVelocity(navPositionAndVelocity_t * pos);
 
 /* Waypoint list access functions */
 int getWaypointCount(void);
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 1a7ece4d596..f6ddce4c13c 100755
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -36,146 +36,22 @@
 #include "drivers/time.h"
 
 #include "fc/config.h"
-#include "fc/rc_controls.h"
-#include "fc/runtime_config.h"
 
 #include "flight/imu.h"
-#include "flight/pid.h"
 
 #include "io/gps.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
+#include "navigation/navigation_pos_estimator_private.h"
 
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
-#include "sensors/boardalignment.h"
 #include "sensors/compass.h"
 #include "sensors/pitotmeter.h"
-#include "sensors/rangefinder.h"
 #include "sensors/opflow.h"
-#include "sensors/sensors.h"
 
-
-/**
- * Model-identification based position estimator
- * Based on INAV position estimator for PX4 by Anton Babushkin <anton.babushkin@me.com>
- * @author Konstantin Sharlaimov <konstantin.sharlaimov@gmail.com>
- */
-#define INAV_GPS_DEFAULT_EPH                200.0f  // 2m GPS HDOP  (gives about 1.6s of dead-reckoning if GPS is temporary lost)
-#define INAV_GPS_DEFAULT_EPV                500.0f  // 5m GPS VDOP
-
-#define INAV_GPS_ACCEPTANCE_EPE             500.0f  // 5m acceptance radius
-
-#define INAV_ACC_BIAS_ACCEPTANCE_VALUE      (GRAVITY_CMSS * 0.25f)   // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway
-
-#define INAV_GPS_GLITCH_RADIUS              250.0f  // 2.5m GPS glitch radius
-#define INAV_GPS_GLITCH_ACCEL               1000.0f // 10m/s/s max possible acceleration for GPS glitch detection
-
-#define INAV_POSITION_PUBLISH_RATE_HZ       50      // Publish position updates at this rate
-#define INAV_PITOT_UPDATE_RATE              10
-
-#define INAV_GPS_TIMEOUT_MS                 1500    // GPS timeout
-#define INAV_BARO_TIMEOUT_MS                200     // Baro timeout
-#define INAV_SURFACE_TIMEOUT_MS             300     // Surface timeout    (missed 3 readings in a row)
-
-#define INAV_HISTORY_BUF_SIZE               (INAV_POSITION_PUBLISH_RATE_HZ / 2)     // Enough to hold 0.5 sec historical data
-
-typedef struct {
-    timeUs_t    lastTriggeredTime;
-    timeUs_t    deltaTime;
-} navigationTimer_t;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-#if defined(NAV_GPS_GLITCH_DETECTION)
-    bool        glitchDetected;
-    bool        glitchRecovery;
-#endif
-    fpVector3_t pos;            // GPS position in NEU coordinate system (cm)
-    fpVector3_t vel;            // GPS velocity (cms)
-    float       eph;
-    float       epv;
-} navPositionEstimatorGPS_t;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-    float       alt;            // Raw barometric altitude (cm)
-    float       epv;
-} navPositionEstimatorBARO_t;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-    float       airspeed;            // airspeed (cm/s)
-} navPositionEstimatorPITOT_t;
-
-typedef enum {
-    SURFACE_QUAL_LOW,   // Surface sensor signal lost long ago - most likely surface distance is incorrect
-    SURFACE_QUAL_MID,   // Surface sensor is not available but we can somewhat trust the estimate
-    SURFACE_QUAL_HIGH   // All good
-} navAGLEstimateQuality_e;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-    float       alt;            // Raw altitude measurement (cm)
-    float       reliability;
-} navPositionEstimatorSURFACE_t;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-    bool        isValid;
-    float       quality;
-    float       flowRate[2];
-    float       bodyRate[2];
-} navPositionEstimatorFLOW_t;
-
-typedef struct {
-    timeUs_t    lastUpdateTime; // Last update time (us)
-
-    // 3D position, velocity and confidence
-    fpVector3_t pos;
-    fpVector3_t vel;
-    float       eph;
-    float       epv;
-
-    // AGL
-    navAGLEstimateQuality_e aglQual;
-    float                   aglOffset;  // Offset between surface and pos.Z
-    float                   aglAlt;
-    float                   aglVel;
-} navPositionEstimatorESTIMATE_t;
-
-typedef struct {
-    timeUs_t    baroGroundTimeout;
-    float       baroGroundAlt;
-    bool        isBaroGroundValid;
-} navPositionEstimatorSTATE_t;
-
-typedef struct {
-    fpVector3_t     accelNEU;
-    fpVector3_t     accelBias;
-    bool            gravityCalibrationComplete;
-} navPosisitonEstimatorIMU_t;
-
-typedef struct {
-    // Data sources
-    navPositionEstimatorGPS_t   gps;
-    navPositionEstimatorBARO_t  baro;
-    navPositionEstimatorSURFACE_t surface;
-    navPositionEstimatorPITOT_t pitot;
-    navPositionEstimatorFLOW_t  flow;
-
-    // IMU data
-    navPosisitonEstimatorIMU_t  imu;
-
-    // Estimate
-    navPositionEstimatorESTIMATE_t  est;
-
-    // Extra state variables
-    navPositionEstimatorSTATE_t state;
-} navigationPosEstimator_t;
-
-static navigationPosEstimator_t posEstimator;
+navigationPosEstimator_t posEstimator;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 3);
 
@@ -186,6 +62,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         .reset_home_type = NAV_RESET_ON_EACH_ARM,
         .gravity_calibration_tolerance = 5,     // 5 cm/s/s calibration error accepted (0.5% of gravity)
         .use_gps_velned = 1,         // "Disabled" is mandatory with gps_dyn_model = Pedestrian
+        .allow_dead_reckoning = 0,
 
         .max_surface_altitude = 200,
 
@@ -195,11 +72,14 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         .w_z_surface_v = 6.100f,
 
         .w_z_gps_p = 0.2f,
-        .w_z_gps_v = 0.5f,
+        .w_z_gps_v = 0.1f,
 
         .w_xy_gps_p = 1.0f,
         .w_xy_gps_v = 2.0f,
 
+        .w_xy_flow_p = 1.0f,
+        .w_xy_flow_v = 2.0f,
+
         .w_z_res_v = 0.5f,
         .w_xy_res_v = 0.5f,
 
@@ -209,25 +89,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
         .baro_epv = 100.0f
 );
 
-/* Inertial filter, implementation taken from PX4 implementation by Anton Babushkin <rk3dov@gmail.com> */
-static void inavFilterPredict(int axis, float dt, float acc)
-{
-    posEstimator.est.pos.v[axis] += posEstimator.est.vel.v[axis] * dt + acc * dt * dt / 2.0f;
-    posEstimator.est.vel.v[axis] += acc * dt;
-}
-
-static void inavFilterCorrectPos(int axis, float dt, float e, float w)
-{
-    float ewdt = e * w * dt;
-    posEstimator.est.pos.v[axis] += ewdt;
-    posEstimator.est.vel.v[axis] += w * ewdt;
-}
-
-static void inavFilterCorrectVel(int axis, float dt, float e, float w)
-{
-    posEstimator.est.vel.v[axis] += e * w * dt;
-}
-
 #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
 #define getTimerDeltaMicros(tim) ((tim)->deltaTime)
 static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t currentTimeUs)
@@ -480,64 +341,6 @@ static void updatePitotTopic(timeUs_t currentTimeUs)
 }
 #endif
 
-#ifdef USE_RANGEFINDER
-/**
- * Read surface and update alt/vel topic
- *  Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
- */
-#define RANGEFINDER_RELIABILITY_RC_CONSTANT     (0.47802f)
-#define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
-#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD   (0.33f)
-#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD  (0.75f)
-
-void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
-{
-    const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime);
-    float newReliabilityMeasurement = 0;
-
-    posEstimator.surface.lastUpdateTime = currentTimeUs;
-
-    if (newSurfaceAlt >= 0) {
-        if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
-            newReliabilityMeasurement = 1.0f;
-            posEstimator.surface.alt = newSurfaceAlt;
-        }
-        else {
-            newReliabilityMeasurement = 0.0f;
-        }
-    }
-    else {
-        // Negative values - out of range or failed hardware
-        newReliabilityMeasurement = 0.0f;
-    }
-
-    /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */
-    if (dt > 0.5f) {
-        posEstimator.surface.reliability = 0.0f;
-    }
-    else {
-        const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT);
-        posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha;
-    }
-}
-#endif
-
-#ifdef USE_OPTICAL_FLOW
-/**
- * Read optical flow topic
- *  Function is called by OPFLOW task as soon as new update is available
- */
-void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs)
-{
-    posEstimator.flow.lastUpdateTime = currentTimeUs;
-    posEstimator.flow.isValid = opflow.isHwHealty && (opflow.flowQuality == OPFLOW_QUALITY_VALID);
-    posEstimator.flow.flowRate[X] = opflow.flowRate[X];
-    posEstimator.flow.flowRate[Y] = opflow.flowRate[Y];
-    posEstimator.flow.bodyRate[X] = opflow.bodyRate[X];
-    posEstimator.flow.bodyRate[Y] = opflow.bodyRate[Y];
-}
-#endif
-
 /**
  * Update IMU topic
  *  Function is called at main loop rate
@@ -611,62 +414,97 @@ static void updateIMUTopic(void)
     }
 }
 
-static float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w)
+float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w)
 {
     return oldEPE + (newEPE - oldEPE) * w * dt;
 }
 
-static bool navIsHeadingUsable(void) 
+static bool navIsAccelerationUsable(void)
 {
-    // If we have GPS - we need true IMU north (valid heading)
-    return isImuHeadingValid();
+    return true;
 }
 
-/**
- * Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
- *  Function is called at main loop rate
- */
-static void updateEstimatedTopic(timeUs_t currentTimeUs)
+static bool navIsHeadingUsable(void)
 {
-    /* Calculate dT */
-    float dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
-    posEstimator.est.lastUpdateTime = currentTimeUs;
+    if (sensors(SENSOR_GPS)) {
+        // If we have GPS - we need true IMU north (valid heading)
+        return isImuHeadingValid();
+    }
+    else {
+        // If we don't have GPS - we may use whatever we have, other sensors are operating in body frame
+        return isImuHeadingValid() || positionEstimationConfig()->allow_dead_reckoning;
+    }
+}
 
-    /* If IMU is not ready we can't estimate anything */
-    if (!isImuReady()) {
-        posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
-        posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
-        return;
+static uint32_t calculateCurrentValidityFlags(timeUs_t currentTimeUs)
+{
+    /* Figure out if we have valid position data from our data sources */
+    uint32_t newFlags = 0;
+
+    if (sensors(SENSOR_GPS) && posControl.gpsOrigin.valid &&
+        ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
+        (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv)) {
+        if (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv) {
+            newFlags |= EST_GPS_XY_VALID | EST_GPS_Z_VALID;
+        }
+        else {
+            newFlags |= EST_GPS_XY_VALID;
+        }
     }
 
-    /* Calculate new EPH and EPV for the case we didn't update postion */
-    float newEPH = posEstimator.est.eph;
-    float newEPV = posEstimator.est.epv;
+    if (sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS))) {
+        newFlags |= EST_BARO_VALID;
+    }
 
-    if (newEPH <= positionEstimationConfig()->max_eph_epv) {
-        newEPH *= 1.0f + dt;
+    if (sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS))) {
+        newFlags |= EST_SURFACE_VALID;
     }
 
-    if (newEPV <= positionEstimationConfig()->max_eph_epv) {
-        newEPV *= 1.0f + dt;
+    if (sensors(SENSOR_OPFLOW) && posEstimator.flow.isValid && ((currentTimeUs - posEstimator.flow.lastUpdateTime) <= MS2US(INAV_FLOW_TIMEOUT_MS))) {
+        newFlags |= EST_FLOW_VALID;
     }
 
-    /* Figure out if we have valid position data from our data sources */
-#if defined(NAV_GPS_GLITCH_DETECTION)
-    //isGPSValid = isGPSValid && !posEstimator.gps.glitchDetected;
-#endif
-    const bool isGPSValid = sensors(SENSOR_GPS) &&
-                            posControl.gpsOrigin.valid &&
-                            ((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
-                            (posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv);
-    const bool isGPSZValid = isGPSValid && (posEstimator.gps.epv < positionEstimationConfig()->max_eph_epv);
-    const bool isBaroValid = sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS));
-#if defined(USE_BARO) || defined(USE_RANGEFINDER)
-    const bool isSurfaceValid = sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.surface.lastUpdateTime) <= MS2US(INAV_SURFACE_TIMEOUT_MS));
-#endif
+    if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
+        newFlags |= EST_XY_VALID;
+    }
+
+    if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
+        newFlags |= EST_Z_VALID;
+    }
+
+    return newFlags;
+}
+
+static void estimationPredict(estimationContext_t * ctx)
+{
+    /* Prediction step: Z-axis */
+    if ((ctx->newFlags & EST_Z_VALID)) {
+        posEstimator.est.pos.z += posEstimator.est.vel.z * ctx->dt;
+        posEstimator.est.pos.z += posEstimator.imu.accelNEU.z * sq(ctx->dt) / 2.0f;
+        posEstimator.est.vel.z += posEstimator.imu.accelNEU.z * ctx->dt;
+    }
+
+    /* Prediction step: XY-axis */
+    if ((ctx->newFlags & EST_XY_VALID)) {
+        // Predict based on known velocity
+        posEstimator.est.pos.x += posEstimator.est.vel.x * ctx->dt;
+        posEstimator.est.pos.y += posEstimator.est.vel.y * ctx->dt;
+
+        // If heading is valid, accelNEU is valid as well. Account for acceleration
+        if (navIsHeadingUsable() && navIsAccelerationUsable()) {
+            posEstimator.est.pos.x += posEstimator.imu.accelNEU.x * sq(ctx->dt) / 2.0f;
+            posEstimator.est.pos.y += posEstimator.imu.accelNEU.y * sq(ctx->dt) / 2.0f;
+            posEstimator.est.vel.x += posEstimator.imu.accelNEU.z * ctx->dt;
+            posEstimator.est.vel.y += posEstimator.imu.accelNEU.z * ctx->dt;
+        }
+    }
+}
+
+static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
+{
+    if (ctx->newFlags & EST_BARO_VALID) {
+        timeUs_t currentTimeUs = micros();
 
-    /* Do some preparations to data */
-    if (isBaroValid) {
         if (!ARMING_FLAG(ARMED)) {
             posEstimator.state.baroGroundAlt = posEstimator.est.pos.z;
             posEstimator.state.isBaroGroundValid = true;
@@ -682,261 +520,189 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
                 posEstimator.state.baroGroundTimeout = currentTimeUs + 250000;   // 0.25 sec
             }
         }
-    }
-    else {
-        posEstimator.state.isBaroGroundValid = false;
-    }
-
-#if defined(USE_BARO)
-    /* We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it */
-    bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
-                                        ((isSurfaceValid && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
-                                         (isBaroValid && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
-#endif
-
-    /* Validate EPV for GPS and calculate altitude/climb rate correction flags */
-    const bool useGpsZPos = STATE(FIXED_WING) && !sensors(SENSOR_BARO) && isGPSValid && isGPSZValid;
-    const bool useGpsZVel = isGPSValid && isGPSZValid;
 
-    /* Estimate validity */
-    const bool isEstXYValid = (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv);
-    const bool isEstZValid = (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv);
+        // We might be experiencing air cushion effect - use sonar or baro groung altitude to detect it
+        bool isAirCushionEffectDetected = ARMING_FLAG(ARMED) &&
+                                            (((ctx->newFlags & EST_SURFACE_VALID) && posEstimator.surface.alt < 20.0f && posEstimator.state.isBaroGroundValid) ||
+                                             ((ctx->newFlags & EST_BARO_VALID) && posEstimator.state.isBaroGroundValid && posEstimator.baro.alt < posEstimator.state.baroGroundAlt));
 
-    /* Prediction step: Z-axis */
-    if (isEstZValid) {
-        inavFilterPredict(Z, dt, posEstimator.imu.accelNEU.z);
-    }
+        // Altitude
+        const float baroAltResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
+        ctx->estPosCorr.z += baroAltResidual * positionEstimationConfig()->w_z_baro_p * ctx->dt;
+        ctx->estVelCorr.z += baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p) * ctx->dt;
 
-    /* Prediction step: XY-axis */
-    if (isEstXYValid) {
-        if (navIsHeadingUsable()) {
-            inavFilterPredict(X, dt, posEstimator.imu.accelNEU.x);
-            inavFilterPredict(Y, dt, posEstimator.imu.accelNEU.y);
+        // If GPS is available - also use GPS climb rate
+        if (ctx->newFlags & EST_GPS_Z_VALID) {
+            // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
+            const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
+            const float gpsRocScaler = bellCurve(gpsRocResidual, 2.5f);
+            ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
         }
-        else {
-            inavFilterPredict(X, dt, 0.0f);
-            inavFilterPredict(Y, dt, 0.0f);
-        }
-    }
 
-    /* Accelerometer bias correction */
-    const bool updateAccBias = (positionEstimationConfig()->w_acc_bias > 0);
-    fpVector3_t accelBiasCorr = { { 0, 0, 0} };
+        ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
 
-    /* Correction step: Z-axis */
-    if (useGpsZPos || isBaroValid) {
-        float gpsWeightScaler = 1.0f;
+        // Accelerometer bias
+        if (!isAirCushionEffectDetected) {
+            ctx->accBiasCorr.z -= baroAltResidual * sq(positionEstimationConfig()->w_z_baro_p);
+        }
 
-        /* Handle Z-axis reset */
-        if (!isEstZValid && useGpsZPos) {
-            posEstimator.est.pos.z = posEstimator.gps.pos.z;
-            posEstimator.est.vel.z = posEstimator.gps.vel.z;
-            newEPV = posEstimator.gps.epv;
+        return true;
+    }
+    else if (STATE(FIXED_WING) && (ctx->newFlags & EST_GPS_Z_VALID)) {
+        // If baro is not available - use GPS Z for correction on a plane
+        // Reset current estimate to GPS altitude if estimate not valid
+        if (!(ctx->newFlags & EST_Z_VALID)) {
+            ctx->estPosCorr.z += posEstimator.gps.pos.z - posEstimator.est.pos.z;
+            ctx->estVelCorr.z += posEstimator.gps.vel.z - posEstimator.est.vel.z;
+            ctx->newEPV = posEstimator.gps.epv;
         }
         else {
-#if defined(USE_BARO)
-            /* Apply BARO correction to altitude */
-            if (isBaroValid) {
-                const float baroResidual = (isAirCushionEffectDetected ? posEstimator.state.baroGroundAlt : posEstimator.baro.alt) - posEstimator.est.pos.z;
-                inavFilterCorrectPos(Z, dt, baroResidual, positionEstimationConfig()->w_z_baro_p);
-                newEPV = updateEPE(posEstimator.est.epv, dt, posEstimator.baro.epv, positionEstimationConfig()->w_z_baro_p);
-
-                /* accelerometer bias correction for baro */
-                if (updateAccBias && !isAirCushionEffectDetected) {
-                    accelBiasCorr.z -= baroResidual * sq(positionEstimationConfig()->w_z_baro_p);
-                }
-            }
-#endif
-
-            /* Apply GPS correction to altitude */
-            if (useGpsZPos) {
-                const float gpsResidualZ = posEstimator.gps.pos.z - posEstimator.est.pos.z;
-                inavFilterCorrectPos(Z, dt, gpsResidualZ, positionEstimationConfig()->w_z_gps_p * gpsWeightScaler);
-                newEPV = updateEPE(posEstimator.est.epv, dt, MAX(posEstimator.gps.epv, gpsResidualZ), positionEstimationConfig()->w_z_gps_p);
+            // Altitude
+            const float gpsAltResudual = posEstimator.gps.pos.z - posEstimator.est.pos.z;
 
-                if (updateAccBias) {
-                    accelBiasCorr.z -= gpsResidualZ * sq(positionEstimationConfig()->w_z_gps_p);
-                }
-            }
+            ctx->estPosCorr.z += gpsAltResudual * positionEstimationConfig()->w_z_gps_p * ctx->dt;
+            ctx->estVelCorr.z += gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p) * ctx->dt;
+            ctx->estVelCorr.z += (posEstimator.gps.vel.z - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_gps_v * ctx->dt;
+            ctx->newEPV = updateEPE(posEstimator.est.epv, ctx->dt, MAX(posEstimator.gps.epv, gpsAltResudual), positionEstimationConfig()->w_z_gps_p);
 
-            /* Apply GPS correction to climb rate */
-            if (useGpsZVel) {
-                const float gpsResidualZVel = posEstimator.gps.vel.z - posEstimator.est.vel.z;
-                inavFilterCorrectVel(Z, dt, gpsResidualZVel, positionEstimationConfig()->w_z_gps_v * sq(gpsWeightScaler));
-            }
+            // Accelerometer bias
+            ctx->accBiasCorr.z -= gpsAltResudual * sq(positionEstimationConfig()->w_z_gps_p);
         }
+
+        return true;
     }
-    else {
-        inavFilterCorrectVel(Z, dt, 0.0f - posEstimator.est.vel.z, positionEstimationConfig()->w_z_res_v);
-    }
 
-    /* Correction step: XY-axis */
-    /* GPS */
-    if (isGPSValid) {
+    return false;
+}
+
+static bool estimationCalculateCorrection_XY_GPS(estimationContext_t * ctx)
+{
+    if (ctx->newFlags & EST_GPS_XY_VALID) {
         /* If GPS is valid and our estimate is NOT valid - reset it to GPS coordinates and velocity */
-        if (!isEstXYValid) {
-            posEstimator.est.pos.x = posEstimator.gps.pos.x;
-            posEstimator.est.pos.y = posEstimator.gps.pos.y;
-            posEstimator.est.vel.x = posEstimator.gps.vel.x;
-            posEstimator.est.vel.y = posEstimator.gps.vel.y;
-            newEPH = posEstimator.gps.eph;
+        if (!(ctx->newFlags & EST_XY_VALID)) {
+            ctx->estPosCorr.x += posEstimator.gps.pos.x - posEstimator.est.pos.x;
+            ctx->estPosCorr.y += posEstimator.gps.pos.y - posEstimator.est.pos.y;
+            ctx->estVelCorr.x += posEstimator.gps.vel.x - posEstimator.est.vel.x;
+            ctx->estVelCorr.y += posEstimator.gps.vel.y - posEstimator.est.vel.y;
+            ctx->newEPH = posEstimator.gps.epv;
         }
         else {
-            const float gpsResidualX = posEstimator.gps.pos.x - posEstimator.est.pos.x;
-            const float gpsResidualY = posEstimator.gps.pos.y - posEstimator.est.pos.y;
-            const float gpsResidualXVel = posEstimator.gps.vel.x - posEstimator.est.vel.x;
-            const float gpsResidualYVel = posEstimator.gps.vel.y - posEstimator.est.vel.y;
-            const float gpsResidualXYMagnitude = sqrtf(sq(gpsResidualX) + sq(gpsResidualY));
+            const float gpsPosXResidual = posEstimator.gps.pos.x - posEstimator.est.pos.x;
+            const float gpsPosYResidual = posEstimator.gps.pos.y - posEstimator.est.pos.y;
+            const float gpsVelXResidual = posEstimator.gps.vel.x - posEstimator.est.vel.x;
+            const float gpsVelYResidual = posEstimator.gps.vel.y - posEstimator.est.vel.y;
+            const float gpsPosResidualMag = sqrtf(sq(gpsPosXResidual) + sq(gpsPosYResidual));
 
-            //const float gpsWeightScaler = scaleRangef(bellCurve(gpsResidualXYMagnitude, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
+            //const float gpsWeightScaler = scaleRangef(bellCurve(gpsPosResidualMag, INAV_GPS_ACCEPTANCE_EPE), 0.0f, 1.0f, 0.1f, 1.0f);
             const float gpsWeightScaler = 1.0f;
 
             const float w_xy_gps_p = positionEstimationConfig()->w_xy_gps_p * gpsWeightScaler;
             const float w_xy_gps_v = positionEstimationConfig()->w_xy_gps_v * sq(gpsWeightScaler);
 
-            inavFilterCorrectPos(X, dt, gpsResidualX, w_xy_gps_p);
-            inavFilterCorrectPos(Y, dt, gpsResidualY, w_xy_gps_p);
+            // Coordinates
+            ctx->estPosCorr.x += gpsPosXResidual * w_xy_gps_p * ctx->dt;
+            ctx->estPosCorr.y += gpsPosYResidual * w_xy_gps_p * ctx->dt;
+
+            // Velocity from coordinates
+            ctx->estVelCorr.x += gpsPosXResidual * sq(w_xy_gps_p) * ctx->dt;
+            ctx->estVelCorr.y += gpsPosYResidual * sq(w_xy_gps_p) * ctx->dt;
+
+            // Velocity from direct measurement
+            ctx->estVelCorr.x += gpsVelXResidual * w_xy_gps_v * ctx->dt;
+            ctx->estVelCorr.y += gpsVelYResidual * w_xy_gps_v * ctx->dt;
 
-            inavFilterCorrectVel(X, dt, gpsResidualXVel, w_xy_gps_v);
-            inavFilterCorrectVel(Y, dt, gpsResidualYVel, w_xy_gps_v);
+            // Accelerometer bias
+            ctx->accBiasCorr.x -= gpsPosXResidual * sq(w_xy_gps_p);
+            ctx->accBiasCorr.y -= gpsPosYResidual * sq(w_xy_gps_p);
 
             /* Adjust EPH */
-            newEPH = updateEPE(posEstimator.est.eph, dt, MAX(posEstimator.gps.eph, gpsResidualXYMagnitude), positionEstimationConfig()->w_xy_gps_p);
+            ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, MAX(posEstimator.gps.eph, gpsPosResidualMag), w_xy_gps_p);
         }
-    }
-    else {
-        inavFilterCorrectVel(X, dt, 0.0f - posEstimator.est.vel.x, positionEstimationConfig()->w_xy_res_v);
-        inavFilterCorrectVel(Y, dt, 0.0f - posEstimator.est.vel.y, positionEstimationConfig()->w_xy_res_v);
+
+        return true;
     }
 
-    /* Correct accelerometer bias */
-    if (updateAccBias) {
-        const float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.x) + sq(accelBiasCorr.y) + sq(accelBiasCorr.z);
-        if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
-            /* transform error vector from NEU frame to body frame */
-            imuTransformVectorEarthToBody(&accelBiasCorr);
+    return false;
+}
 
-            /* Correct accel bias */
-            posEstimator.imu.accelBias.x += accelBiasCorr.x * positionEstimationConfig()->w_acc_bias * dt;
-            posEstimator.imu.accelBias.y += accelBiasCorr.y * positionEstimationConfig()->w_acc_bias * dt;
-            posEstimator.imu.accelBias.z += accelBiasCorr.z * positionEstimationConfig()->w_acc_bias * dt;
-        }
+/**
+ * Calculate next estimate using IMU and apply corrections from reference sensors (GPS, BARO etc)
+ *  Function is called at main loop rate
+ */
+static void updateEstimatedTopic(timeUs_t currentTimeUs)
+{
+    estimationContext_t ctx;
+
+    /* Calculate dT */
+    ctx.dt = US2S(currentTimeUs - posEstimator.est.lastUpdateTime);
+    posEstimator.est.lastUpdateTime = currentTimeUs;
+
+    /* If IMU is not ready we can't estimate anything */
+    if (!isImuReady()) {
+        posEstimator.est.eph = positionEstimationConfig()->max_eph_epv + 0.001f;
+        posEstimator.est.epv = positionEstimationConfig()->max_eph_epv + 0.001f;
+        posEstimator.flags = 0;
+        return;
     }
 
-    /* Update uncertainty */
-    posEstimator.est.eph = newEPH;
-    posEstimator.est.epv = newEPV;
-
-    /* AGL estimation */
-#if defined(USE_RANGEFINDER) && defined(USE_BARO)
-    if (isSurfaceValid && isBaroValid) {
-        navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
-        bool resetSurfaceEstimate = false;
-        switch (posEstimator.est.aglQual) {
-            case SURFACE_QUAL_LOW:
-                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_HIGH;
-                    resetSurfaceEstimate = true;
-                }
-                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_LOW;
-                }
-                else {
-                    newAglQuality = SURFACE_QUAL_LOW;
-                }
-                break;
+    /* Calculate new EPH and EPV for the case we didn't update postion */
+    ctx.newEPH = posEstimator.est.eph * ((posEstimator.est.eph <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
+    ctx.newEPV = posEstimator.est.epv * ((posEstimator.est.epv <= positionEstimationConfig()->max_eph_epv) ? 1.0f + ctx.dt : 1.0f);
+    ctx.newFlags = calculateCurrentValidityFlags(currentTimeUs);
+    vectorZero(&ctx.estPosCorr);
+    vectorZero(&ctx.estVelCorr);
+    vectorZero(&ctx.accBiasCorr);
 
-            case SURFACE_QUAL_MID:
-                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_HIGH;
-                }
-                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_MID;
-                }
-                else {
-                    newAglQuality = SURFACE_QUAL_LOW;
-                }
-                break;
+    /* AGL estimation - separate process, decouples from Z coordinate */
+    estimationCalculateAGL(&ctx);
 
-            case SURFACE_QUAL_HIGH:
-                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_HIGH;
-                }
-                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
-                    newAglQuality = SURFACE_QUAL_MID;
-                }
-                else {
-                    newAglQuality = SURFACE_QUAL_LOW;
-                }
-                break;
-        }
+    /* Prediction stage: X,Y,Z */
+    estimationPredict(&ctx);
 
-        posEstimator.est.aglQual = newAglQuality;
+    /* Correction stage: Z */
+    const bool estZCorrectOk =
+        estimationCalculateCorrection_Z(&ctx);
 
-        if (resetSurfaceEstimate) {
-            posEstimator.est.aglAlt = posEstimator.surface.alt;
-            if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
-                posEstimator.est.aglVel = posEstimator.est.vel.z;
-                posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
-            }
-            else {
-                posEstimator.est.aglVel = 0;
-                posEstimator.est.aglOffset = 0;
-            }
-        }
+    /* Correction stage: XY: GPS, FLOW */
+    // FIXME: Handle transition from FLOW to GPS and back - seamlessly fly indoor/outdoor
+    const bool estXYCorrectOk =
+        estimationCalculateCorrection_XY_GPS(&ctx) ||
+        estimationCalculateCorrection_XY_FLOW(&ctx);
 
-        // Update estimate
-        posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * dt + posEstimator.imu.accelNEU.z * dt * dt * 0.5f;
-        posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * dt;
+    // If we can't apply correction or accuracy is off the charts - decay velocity to zero
+    if (!estXYCorrectOk || ctx.newEPH > positionEstimationConfig()->max_eph_epv) {
+        ctx.estVelCorr.x = (0.0f - posEstimator.est.vel.x) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
+        ctx.estVelCorr.y = (0.0f - posEstimator.est.vel.y) * positionEstimationConfig()->w_xy_res_v * ctx.dt;
+    }
 
-        // Apply correction
-        if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
-            // Correct estimate from rangefinder
-            const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
-            const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f);
+    if (!estZCorrectOk || ctx.newEPV > positionEstimationConfig()->max_eph_epv) {
+        ctx.estVelCorr.z = (0.0f - posEstimator.est.vel.z) * positionEstimationConfig()->w_z_res_v * ctx.dt;
+    }
 
-            posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * dt;
-            posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * dt;
+    // Apply corrections
+    vectorAdd(&posEstimator.est.pos, &posEstimator.est.pos, &ctx.estPosCorr);
+    vectorAdd(&posEstimator.est.vel, &posEstimator.est.vel, &ctx.estVelCorr);
 
-            // Update estimate offset
-            if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
-                posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
-            }
-        }
-        else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
-            // Correct estimate from altitude fused from rangefinder and global altitude
-            const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
-            const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
-            const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
-            const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
-
-            posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * dt;
-            posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * dt;
-        }
-        else {  // SURFACE_QUAL_LOW
-            // In this case rangefinder can't be trusted - simply use global altitude
-            posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
-            posEstimator.est.aglVel = posEstimator.est.vel.z;
+    /* Correct accelerometer bias */
+    if (positionEstimationConfig()->w_acc_bias > 0.0f) {
+        const float accelBiasCorrMagnitudeSq = sq(ctx.accBiasCorr.x) + sq(ctx.accBiasCorr.y) + sq(ctx.accBiasCorr.z);
+        if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) {
+            /* transform error vector from NEU frame to body frame */
+            imuTransformVectorEarthToBody(&ctx.accBiasCorr);
+
+            /* Correct accel bias */
+            posEstimator.imu.accelBias.x += ctx.accBiasCorr.x * positionEstimationConfig()->w_acc_bias * ctx.dt;
+            posEstimator.imu.accelBias.y += ctx.accBiasCorr.y * positionEstimationConfig()->w_acc_bias * ctx.dt;
+            posEstimator.imu.accelBias.z += ctx.accBiasCorr.z * positionEstimationConfig()->w_acc_bias * ctx.dt;
         }
     }
-    else {
-        posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
-        posEstimator.est.aglVel = posEstimator.est.vel.z;
-        posEstimator.est.aglQual = SURFACE_QUAL_LOW;
-    }
 
-#if defined(NAV_BLACKBOX)
-        DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
-        DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
-        DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
-        DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
-#endif
+    /* Update uncertainty */
+    posEstimator.est.eph = ctx.newEPH;
+    posEstimator.est.epv = ctx.newEPV;
 
-#else
-    posEstimator.est.aglAlt = posEstimator.est.pos.z;
-    posEstimator.est.aglVel = posEstimator.est.vel.z;
-    posEstimator.est.aglQual = SURFACE_QUAL_LOW;
-#endif
+    // Keep flags for further usage
+    posEstimator.flags = ctx.newFlags;
 }
 
 /**
@@ -954,10 +720,11 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
     if (updateTimer(&posPublishTimer, HZ2US(INAV_POSITION_PUBLISH_RATE_HZ), currentTimeUs)) {
         /* Publish position update */
         if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
-            updateActualHorizontalPositionAndVelocity(true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
+            // FIXME!!!!!
+            updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, posEstimator.est.vel.x, posEstimator.est.vel.y);
         }
         else {
-            updateActualHorizontalPositionAndVelocity(false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0);
+            updateActualHorizontalPositionAndVelocity(false, false, posEstimator.est.pos.x, posEstimator.est.pos.y, 0, 0);
         }
 
         /* Publish altitude update and set altitude validity */
@@ -1001,6 +768,9 @@ void initializePositionEstimator(void)
     posEstimator.est.aglAlt = 0;
     posEstimator.est.aglVel = 0;
 
+    posEstimator.est.flowCoordinates[X] = 0;
+    posEstimator.est.flowCoordinates[Y] = 0;
+
     posEstimator.imu.gravityCalibrationComplete = false;
 
     for (axis = 0; axis < 3; axis++) {
diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/src/main/navigation/navigation_pos_estimator_agl.c
new file mode 100644
index 00000000000..49a09225497
--- /dev/null
+++ b/src/main/navigation/navigation_pos_estimator_agl.c
@@ -0,0 +1,195 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#if defined(USE_NAV)
+
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+#include "navigation/navigation_pos_estimator_private.h"
+
+#include "sensors/rangefinder.h"
+#include "sensors/barometer.h"
+
+extern navigationPosEstimator_t posEstimator;
+
+#ifdef USE_RANGEFINDER
+/**
+ * Read surface and update alt/vel topic
+ *  Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
+ */
+void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
+{
+    const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime);
+    float newReliabilityMeasurement = 0;
+
+    posEstimator.surface.lastUpdateTime = currentTimeUs;
+
+    if (newSurfaceAlt >= 0) {
+        if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
+            newReliabilityMeasurement = 1.0f;
+            posEstimator.surface.alt = newSurfaceAlt;
+        }
+        else {
+            newReliabilityMeasurement = 0.0f;
+        }
+    }
+    else {
+        // Negative values - out of range or failed hardware
+        newReliabilityMeasurement = 0.0f;
+    }
+
+    /* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */
+    if (dt > 0.5f) {
+        posEstimator.surface.reliability = 0.0f;
+    }
+    else {
+        const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT);
+        posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha;
+    }
+}
+#endif
+
+void estimationCalculateAGL(estimationContext_t * ctx)
+{
+#if defined(USE_RANGEFINDER) && defined(USE_BARO)
+    if ((ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_BARO_VALID)) {
+        navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
+        bool resetSurfaceEstimate = false;
+        switch (posEstimator.est.aglQual) {
+            case SURFACE_QUAL_LOW:
+                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_HIGH;
+                    resetSurfaceEstimate = true;
+                }
+                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_LOW;
+                }
+                else {
+                    newAglQuality = SURFACE_QUAL_LOW;
+                }
+                break;
+
+            case SURFACE_QUAL_MID:
+                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_HIGH;
+                }
+                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_MID;
+                }
+                else {
+                    newAglQuality = SURFACE_QUAL_LOW;
+                }
+                break;
+
+            case SURFACE_QUAL_HIGH:
+                if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_HIGH;
+                }
+                else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
+                    newAglQuality = SURFACE_QUAL_MID;
+                }
+                else {
+                    newAglQuality = SURFACE_QUAL_LOW;
+                }
+                break;
+        }
+
+        posEstimator.est.aglQual = newAglQuality;
+
+        if (resetSurfaceEstimate) {
+            posEstimator.est.aglAlt = posEstimator.surface.alt;
+            if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
+                posEstimator.est.aglVel = posEstimator.est.vel.z;
+                posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
+            }
+            else {
+                posEstimator.est.aglVel = 0;
+                posEstimator.est.aglOffset = 0;
+            }
+        }
+
+        // Update estimate
+        posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * ctx->dt;
+        posEstimator.est.aglAlt = posEstimator.imu.accelNEU.z * sq(ctx->dt) * 0.5f;
+        posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.z * ctx->dt;
+
+        // Apply correction
+        if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
+            // Correct estimate from rangefinder
+            const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
+            const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 75.0f), 0.0f, 1.0f, 0.1f, 1.0f);
+
+            posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * ctx->dt;
+            posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * ctx->dt;
+
+            // Update estimate offset
+            if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
+                posEstimator.est.aglOffset = posEstimator.est.pos.z - posEstimator.surface.alt;
+            }
+        }
+        else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
+            // Correct estimate from altitude fused from rangefinder and global altitude
+            const float estAltResidual = (posEstimator.est.pos.z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
+            const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
+            const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
+            const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
+
+            posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * ctx->dt;
+            posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * ctx->dt;
+        }
+        else {  // SURFACE_QUAL_LOW
+            // In this case rangefinder can't be trusted - simply use global altitude
+            posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
+            posEstimator.est.aglVel = posEstimator.est.vel.z;
+        }
+    }
+    else {
+        posEstimator.est.aglAlt = posEstimator.est.pos.z - posEstimator.est.aglOffset;
+        posEstimator.est.aglVel = posEstimator.est.vel.z;
+        posEstimator.est.aglQual = SURFACE_QUAL_LOW;
+    }
+
+    DEBUG_SET(DEBUG_AGL, 0, posEstimator.surface.reliability * 1000);
+    DEBUG_SET(DEBUG_AGL, 1, posEstimator.est.aglQual);
+    DEBUG_SET(DEBUG_AGL, 2, posEstimator.est.aglAlt);
+    DEBUG_SET(DEBUG_AGL, 3, posEstimator.est.aglVel);
+
+#else
+    posEstimator.est.aglAlt = posEstimator.est.pos.z;
+    posEstimator.est.aglVel = posEstimator.est.vel.z;
+    posEstimator.est.aglQual = SURFACE_QUAL_LOW;
+#endif
+}
+
+#endif  // NAV
diff --git a/src/main/navigation/navigation_pos_estimator_flow.c b/src/main/navigation/navigation_pos_estimator_flow.c
new file mode 100644
index 00000000000..e9d098d8599
--- /dev/null
+++ b/src/main/navigation/navigation_pos_estimator_flow.c
@@ -0,0 +1,125 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+#include <string.h>
+
+#include "platform.h"
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#if defined(USE_NAV)
+
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+#include "navigation/navigation_pos_estimator_private.h"
+
+#include "sensors/rangefinder.h"
+#include "sensors/opflow.h"
+
+#include "flight/imu.h"
+
+extern navigationPosEstimator_t posEstimator;
+
+#ifdef USE_OPTICAL_FLOW
+/**
+ * Read optical flow topic
+ *  Function is called by OPFLOW task as soon as new update is available
+ */
+void updatePositionEstimator_OpticalFlowTopic(timeUs_t currentTimeUs)
+{
+    posEstimator.flow.lastUpdateTime = currentTimeUs;
+    posEstimator.flow.isValid = opflow.isHwHealty && (opflow.flowQuality == OPFLOW_QUALITY_VALID);
+    posEstimator.flow.flowRate[X] = opflow.flowRate[X];
+    posEstimator.flow.flowRate[Y] = opflow.flowRate[Y];
+    posEstimator.flow.bodyRate[X] = opflow.bodyRate[X];
+    posEstimator.flow.bodyRate[Y] = opflow.bodyRate[Y];
+}
+#endif
+
+bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx)
+{
+#if defined(USE_RANGEFINDER) && defined(USE_OPTICAL_FLOW)
+    if (!((ctx->newFlags & EST_FLOW_VALID) && (ctx->newFlags & EST_SURFACE_VALID) && (ctx->newFlags & EST_Z_VALID))) {
+        return false;
+    }
+
+    // FIXME: flow may use AGL estimate if available
+    const bool canUseFlow = (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD);
+
+    if (!canUseFlow) {
+        return false;
+    }
+
+    // Calculate linear velocity based on angular velocity and altitude
+    // Technically we should calculate arc length here, but for fast sampling this is accurate enough
+    fpVector3_t flowVel = {
+        .x = - (posEstimator.flow.flowRate[Y] - posEstimator.flow.bodyRate[Y]) * posEstimator.surface.alt,
+        .y =   (posEstimator.flow.flowRate[X] - posEstimator.flow.bodyRate[X]) * posEstimator.surface.alt,
+        .z =    posEstimator.est.vel.z
+    };
+
+    // At this point flowVel will hold linear velocities in earth frame
+    imuTransformVectorBodyToEarth(&flowVel);
+
+    // Calculate velocity correction
+    const float flowVelXInnov = flowVel.x - posEstimator.est.vel.x;
+    const float flowVelYInnov = flowVel.y - posEstimator.est.vel.y;
+
+    ctx->estVelCorr.x = flowVelXInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt;
+    ctx->estVelCorr.y = flowVelYInnov * positionEstimationConfig()->w_xy_flow_v * ctx->dt;
+
+    // Calculate position correction if possible/allowed
+    if ((ctx->newFlags & EST_GPS_XY_VALID)) {
+        // If GPS is valid - reset flow estimated coordinates to GPS
+        posEstimator.est.flowCoordinates[X] = posEstimator.gps.pos.x;
+        posEstimator.est.flowCoordinates[Y] = posEstimator.gps.pos.y;
+    }
+    else if (positionEstimationConfig()->allow_dead_reckoning) {
+        posEstimator.est.flowCoordinates[X] += flowVel.x * ctx->dt;
+        posEstimator.est.flowCoordinates[Y] += flowVel.y * ctx->dt;
+
+        const float flowResidualX = posEstimator.est.flowCoordinates[X] - posEstimator.est.pos.x;
+        const float flowResidualY = posEstimator.est.flowCoordinates[Y] - posEstimator.est.pos.y;
+
+        ctx->estPosCorr.x = flowResidualX * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
+        ctx->estPosCorr.y = flowResidualY * positionEstimationConfig()->w_xy_flow_p * ctx->dt;
+
+        ctx->newEPH = updateEPE(posEstimator.est.eph, ctx->dt, sqrtf(sq(flowResidualX) + sq(flowResidualY)), positionEstimationConfig()->w_xy_flow_p);
+    }
+
+    return true;
+
+    DEBUG_SET(DEBUG_FLOW, 0, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[Y]));
+    DEBUG_SET(DEBUG_FLOW, 1, RADIANS_TO_DEGREES(posEstimator.flow.flowRate[X]));
+    DEBUG_SET(DEBUG_FLOW, 2, posEstimator.est.flowCoordinates[X]);
+    DEBUG_SET(DEBUG_FLOW, 3, posEstimator.est.flowCoordinates[Y]);
+#endif
+}
+
+
+#endif  // NAV
diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h
new file mode 100644
index 00000000000..6b2b6c5d687
--- /dev/null
+++ b/src/main/navigation/navigation_pos_estimator_private.h
@@ -0,0 +1,175 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+#include <string.h>
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "sensors/sensors.h"
+
+#define INAV_GPS_DEFAULT_EPH                200.0f  // 2m GPS HDOP  (gives about 1.6s of dead-reckoning if GPS is temporary lost)
+#define INAV_GPS_DEFAULT_EPV                500.0f  // 5m GPS VDOP
+
+#define INAV_GPS_ACCEPTANCE_EPE             500.0f  // 5m acceptance radius
+
+#define INAV_ACC_BIAS_ACCEPTANCE_VALUE      (GRAVITY_CMSS * 0.25f)   // Max accepted bias correction of 0.25G - unlikely we are going to be that much off anyway
+
+#define INAV_GPS_GLITCH_RADIUS              250.0f  // 2.5m GPS glitch radius
+#define INAV_GPS_GLITCH_ACCEL               1000.0f // 10m/s/s max possible acceleration for GPS glitch detection
+
+#define INAV_POSITION_PUBLISH_RATE_HZ       50      // Publish position updates at this rate
+#define INAV_PITOT_UPDATE_RATE              10
+
+#define INAV_GPS_TIMEOUT_MS                 1500    // GPS timeout
+#define INAV_BARO_TIMEOUT_MS                200     // Baro timeout
+#define INAV_SURFACE_TIMEOUT_MS             300     // Surface timeout    (missed 3 readings in a row)
+#define INAV_FLOW_TIMEOUT_MS                200
+
+#define RANGEFINDER_RELIABILITY_RC_CONSTANT     (0.47802f)
+#define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
+#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD   (0.33f)
+#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD  (0.75f)
+
+typedef struct {
+    timeUs_t    lastTriggeredTime;
+    timeUs_t    deltaTime;
+} navigationTimer_t;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+#if defined(NAV_GPS_GLITCH_DETECTION)
+    bool        glitchDetected;
+    bool        glitchRecovery;
+#endif
+    fpVector3_t pos;            // GPS position in NEU coordinate system (cm)
+    fpVector3_t vel;            // GPS velocity (cms)
+    float       eph;
+    float       epv;
+} navPositionEstimatorGPS_t;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+    float       alt;            // Raw barometric altitude (cm)
+    float       epv;
+} navPositionEstimatorBARO_t;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+    float       airspeed;            // airspeed (cm/s)
+} navPositionEstimatorPITOT_t;
+
+typedef enum {
+    SURFACE_QUAL_LOW,   // Surface sensor signal lost long ago - most likely surface distance is incorrect
+    SURFACE_QUAL_MID,   // Surface sensor is not available but we can somewhat trust the estimate
+    SURFACE_QUAL_HIGH   // All good
+} navAGLEstimateQuality_e;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+    float       alt;            // Raw altitude measurement (cm)
+    float       reliability;
+} navPositionEstimatorSURFACE_t;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+    bool        isValid;
+    float       quality;
+    float       flowRate[2];
+    float       bodyRate[2];
+} navPositionEstimatorFLOW_t;
+
+typedef struct {
+    timeUs_t    lastUpdateTime; // Last update time (us)
+
+    // 3D position, velocity and confidence
+    fpVector3_t pos;
+    fpVector3_t vel;
+    float       eph;
+    float       epv;
+
+    // AGL
+    navAGLEstimateQuality_e aglQual;
+    float                   aglOffset;  // Offset between surface and pos.Z
+    float                   aglAlt;
+    float                   aglVel;
+
+    // FLOW
+    float                   flowCoordinates[2];
+} navPositionEstimatorESTIMATE_t;
+
+typedef struct {
+    fpVector3_t     accelNEU;
+    fpVector3_t     accelBias;
+    bool            gravityCalibrationComplete;
+} navPosisitonEstimatorIMU_t;
+
+typedef enum {
+    EST_GPS_XY_VALID            = (1 << 0),
+    EST_GPS_Z_VALID             = (1 << 1),
+    EST_BARO_VALID              = (1 << 2),
+    EST_SURFACE_VALID           = (1 << 3),
+    EST_FLOW_VALID              = (1 << 4),
+    EST_XY_VALID                = (1 << 5),
+    EST_Z_VALID                 = (1 << 6),
+} navPositionEstimationFlags_e;
+
+typedef struct {
+    timeUs_t    baroGroundTimeout;
+    float       baroGroundAlt;
+    bool        isBaroGroundValid;
+} navPositionEstimatorSTATE_t;
+
+
+typedef struct {
+    uint32_t                    flags;
+
+    // Data sources
+    navPositionEstimatorGPS_t   gps;
+    navPositionEstimatorBARO_t  baro;
+    navPositionEstimatorSURFACE_t surface;
+    navPositionEstimatorPITOT_t pitot;
+    navPositionEstimatorFLOW_t  flow;
+
+    // IMU data
+    navPosisitonEstimatorIMU_t  imu;
+
+    // Estimate
+    navPositionEstimatorESTIMATE_t  est;
+
+    // Extra state variables
+    navPositionEstimatorSTATE_t state;
+} navigationPosEstimator_t;
+
+typedef struct {
+    float dt;
+    uint32_t newFlags;
+    float newEPV;
+    float newEPH;
+    fpVector3_t estPosCorr;
+    fpVector3_t estVelCorr;
+    fpVector3_t accBiasCorr;
+} estimationContext_t;
+
+extern float updateEPE(const float oldEPE, const float dt, const float newEPE, const float w);
+extern void estimationCalculateAGL(estimationContext_t * ctx);
+extern bool estimationCalculateCorrection_XY_FLOW(estimationContext_t * ctx);
+
+
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 1130bfd3f2a..c31f50073d8 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -21,6 +21,8 @@
 
 #if defined(USE_NAV)
 
+#include "common/axis.h"
+#include "common/maths.h"
 #include "common/filter.h"
 #include "fc/runtime_config.h"
 
@@ -80,6 +82,7 @@ typedef struct navigationFlags_s {
 
     navigationEstimateStatus_e estAltStatus;        // Indicates that we have a working altitude sensor (got at least one valid reading from it)
     navigationEstimateStatus_e estPosStatus;        // Indicates that GPS is working (or not)
+    navigationEstimateStatus_e estVelStatus;        // Indicates that GPS is working (or not)
     navigationEstimateStatus_e estAglStatus;
     navigationEstimateStatus_e estHeadingStatus;    // Indicate valid heading - wither mag or GPS at certain speed on airplane
 
@@ -382,7 +385,7 @@ bool isApproachingLastWaypoint(void);
 float getActiveWaypointSpeed(void);
 
 void updateActualHeading(bool headingValid, int32_t newHeading);
-void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY);
+void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY);
 void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus);
 
 bool checkForPositionSensorTimeout(void);
diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c
index f39a023691e..f76937caa19 100755
--- a/src/main/sensors/opflow.c
+++ b/src/main/sensors/opflow.c
@@ -73,8 +73,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLO
 
 PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig,
     .opflow_hardware = OPFLOW_NONE,
-    .opflow_scale = 1.0f,
     .opflow_align = CW0_DEG_FLIP,
+    .opflow_scale = 1.0f,
 );
 
 static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse)
@@ -178,19 +178,19 @@ void opflowUpdate(timeUs_t currentTimeUs)
 
         if ((opflow.flowQuality == OPFLOW_QUALITY_VALID) && (opflow.gyroBodyRateTimeUs > 0)) {
             const float integralToRateScaler = (1.0e6 / opflow.dev.rawData.deltaTime) / (float)opticalFlowConfig()->opflow_scale;
-            int32_t flowRateRaw[3] = { opflow.dev.rawData.flowRateRaw[X], opflow.dev.rawData.flowRateRaw[Y], 0 };
 
-            applySensorAlignment(flowRateRaw, flowRateRaw, opticalFlowConfig()->opflow_align);
-            //applyBoardAlignment(flowRateRaw);
+            // Apply sensor alignment
+            applySensorAlignment(opflow.dev.rawData.flowRateRaw, opflow.dev.rawData.flowRateRaw, opticalFlowConfig()->opflow_align);
+            //applyBoardAlignment(opflow.dev.rawData.flowRateRaw);
 
-            opflow.flowRate[X] = DEGREES_TO_RADIANS(flowRateRaw[X] * integralToRateScaler);
-            opflow.flowRate[Y] = DEGREES_TO_RADIANS(flowRateRaw[Y] * integralToRateScaler);
+            opflow.flowRate[X] = DEGREES_TO_RADIANS(opflow.dev.rawData.flowRateRaw[X] * integralToRateScaler);
+            opflow.flowRate[Y] = DEGREES_TO_RADIANS(opflow.dev.rawData.flowRateRaw[Y] * integralToRateScaler);
 
             opflow.bodyRate[X] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[X] / opflow.gyroBodyRateTimeUs);
             opflow.bodyRate[Y] = DEGREES_TO_RADIANS(opflow.gyroBodyRateAcc[Y] / opflow.gyroBodyRateTimeUs);
 
-            DEBUG_SET(DEBUG_FLOW_RAW, 0, RADIANS_TO_DEGREES(opflow.flowRate[X]));
-            DEBUG_SET(DEBUG_FLOW_RAW, 1, RADIANS_TO_DEGREES(opflow.flowRate[Y]));
+            DEBUG_SET(DEBUG_FLOW_RAW, 0, RADIANS_TO_DEGREES(opflow.dev.rawData.flowRateRaw[X]));
+            DEBUG_SET(DEBUG_FLOW_RAW, 1, RADIANS_TO_DEGREES(opflow.dev.rawData.flowRateRaw[Y]));
             DEBUG_SET(DEBUG_FLOW_RAW, 2, RADIANS_TO_DEGREES(opflow.bodyRate[X]));
             DEBUG_SET(DEBUG_FLOW_RAW, 3, RADIANS_TO_DEGREES(opflow.bodyRate[Y]));
         }
diff --git a/src/main/sensors/opflow.h b/src/main/sensors/opflow.h
index 4bd1ec8fcb1..bbfced6470c 100755
--- a/src/main/sensors/opflow.h
+++ b/src/main/sensors/opflow.h
@@ -43,8 +43,8 @@ typedef enum {
 
 typedef struct opticalFlowConfig_s  {
     uint8_t opflow_hardware;
-    float   opflow_scale;       // Scaler value to convert between raw sensor units to [deg/s]
     uint8_t opflow_align;
+    float   opflow_scale;       // Scaler value to convert between raw sensor units to [deg/s]
 } opticalFlowConfig_t;
 
 PG_DECLARE(opticalFlowConfig_t, opticalFlowConfig);
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 6060856e29c..97131a577e9 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -105,6 +105,11 @@
   #define USE_BARO_BMP280
   #define BMP280_SPI_BUS        BUS_SPI3
   #define BMP280_CS_PIN         PB3 // v1
+
+  // Support external barometers
+  #define BARO_I2C_BUS          BUS_I2C2
+  #define USE_BARO_BMP085
+  #define USE_BARO_MS5611
 #else
   #define BARO_I2C_BUS          BUS_I2C2
   #define USE_BARO_BMP085
@@ -118,6 +123,10 @@
 #define USE_RANGEFINDER
 #define RANGEFINDER_I2C_BUS     BUS_I2C2
 #define USE_RANGEFINDER_HCSR04_I2C
+#define USE_RANGEFINDER_VL53L0X
+
+#define USE_OPTICAL_FLOW
+#define USE_OPFLOW_CXOF
 
 #define USE_VCP
 #define VBUS_SENSING_PIN        PC5