kalman filter sensor fusion added to altitude estimation

mk/source.mk

@@ -159,6 +159,7 @@ COMMON_SRC = \
             fc/rc_modes.c \
             flight/alt_hold.c \
             flight/autopilot.c \
+            flight/kalman_filter_1d.c \
             flight/dyn_notch_filter.c \
             flight/failsafe.c \
             flight/gps_rescue.c \

src/main/flight/kalman_filter_1d.c

@@ -0,0 +1,56 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight 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.
+ *
+ * Betaflight 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 Betaflight. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+#include "math.h"
+
+
+#include "kalman_filter_1d.h"
+
+void kf_init(KalmanFilter *kf, float initialValue, float initialVariance, float processVariance) {
+    kf->estimatedValue = initialValue;
+    kf->estimatedVariance = initialVariance;
+    kf->processVariance = processVariance;
+}
+
+void kf_update_variance(KalmanFilter *kf) {
+    // Increase the estimated variance by the process variance
+    kf->estimatedVariance += kf->processVariance;
+}
+
+void kf_update(KalmanFilter *kf, SensorMeasurement sensorMeas) {
+    if (sensorMeas.value == -1) {
+        return; // Skip update if measurement is invalid
+    }
+
+    float kalmanGain = kf->estimatedVariance / (kf->estimatedVariance + sensorMeas.variance);
+
+    kf->estimatedValue += kalmanGain * (sensorMeas.value - kf->estimatedValue);
+
+    kf->estimatedVariance = (1 - kalmanGain) * kf->estimatedVariance;
+}
+
+#ifdef USE_BARO
+void updateBaroVariance(SensorMeasurement * sensorMeas) {
+    static uint16_t n = 0;
+    if (n == 0) {
+        sensorMeas->variance = 0.0f;
+    }
+    sensorMeas->variance = (sensorMeas->variance * n + powf(sensorMeas->value, 2)) / (n + 1);
+    n++;
+}
+#endif

src/main/flight/kalman_filter_1d.h

@@ -0,0 +1,37 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight are free software. You can redistribute
+ * this software and/or modify this software 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.
+ *
+ * Betaflight are distributed in the hope that they
+ * 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 software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+typedef struct {
+    float estimatedValue;
+    float estimatedVariance;
+    float processVariance;
+} KalmanFilter;
+
+typedef struct {
+    float value;
+    float variance;
+} SensorMeasurement;
+
+void kf_init(KalmanFilter *kf, float initialValue, float initialVariance, float processVariance);
+void kf_update_variance(KalmanFilter *kf);
+void kf_update(KalmanFilter *kf, SensorMeasurement sensorMeas);
+void updateBaroVariance(SensorMeasurement * sensorMeas);

src/main/flight/position.c

@@ -36,6 +36,7 @@
 #include "flight/position.h"
 #include "flight/imu.h"
 #include "flight/pid.h"
+#include "flight/kalman_filter_1d.h"
 
 #include "io/gps.h"
 
@@ -43,6 +44,7 @@
 
 #include "sensors/sensors.h"
 #include "sensors/barometer.h"
+#include "sensors/rangefinder.h"
 
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
@@ -50,27 +52,17 @@
 static float displayAltitudeCm = 0.0f;
 static bool altitudeAvailable = false;
 
-static float zeroedAltitudeCm = 0.0f;
-static float zeroedAltitudeDerivative = 0.0f;
+static float zeroedGpsAltitudeCm = 0.0f;
 
-static pt2Filter_t altitudeLpf;
-static pt2Filter_t altitudeDerivativeLpf;
+static float zeroedFusedAltitudeCm = 0.0f;
+static float zeroedFusedAltitudeCmDerivative = 0.0f;
 
 #ifdef USE_VARIO
 static int16_t estimatedVario = 0; // in cm/s
 #endif
 
-void positionInit(void)
-{
-    const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
+void positionInit(void) {
 
-    const float altitudeCutoffHz = positionConfig()->altitude_lpf / 100.0f;
-    const float altitudeGain = pt2FilterGain(altitudeCutoffHz, sampleTimeS);
-    pt2FilterInit(&altitudeLpf, altitudeGain);
-
-    const float altitudeDerivativeCutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
-    const float altitudeDerivativeGain = pt2FilterGain(altitudeDerivativeCutoffHz, sampleTimeS);
-    pt2FilterInit(&altitudeDerivativeLpf, altitudeDerivativeGain);
 }
 
 typedef enum {
@@ -88,29 +80,36 @@ PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
     .altitude_d_lpf = 100,
 );
 
-#if defined(USE_BARO) || defined(USE_GPS)
-void calculateEstimatedAltitude(void)
-{
+#if defined(USE_BARO) || defined(USE_GPS) || defined(USE_RANGEFINDER)
+void calculateEstimatedAltitude(void) {
     static bool wasArmed = false;
     static bool useZeroedGpsAltitude = false; // whether a zero for the GPS altitude value exists
     static float gpsAltCm = 0.0f; // will hold last value on transient loss of 3D fix
     static float gpsAltOffsetCm = 0.0f;
     static float baroAltOffsetCm = 0.0f;
     static float newBaroAltOffsetCm = 0.0f;
-
+    static KalmanFilter kf;
+    static bool kfInitDone = false;
     float baroAltCm = 0.0f;
     float gpsTrust = 0.3f; // if no pDOP value, use 0.3, intended range 0-1;
     bool haveBaroAlt = false; // true if baro exists and has been calibrated on power up
     bool haveGpsAlt = false; // true if GPS is connected and while it has a 3D fix, set each run to false
+    bool haveRangefinderAlt = false; // true if rangefinder is connected and has a valid reading
 
     // *** Get sensor data
 #ifdef USE_BARO
+    static SensorMeasurement baroAltMeasurement;
     if (sensors(SENSOR_BARO)) {
         baroAltCm = getBaroAltitude();
         haveBaroAlt = true; // false only if there is no sensor on the board, or it has failed
+        baroAltMeasurement.value = baroAltCm;
+        updateBaroVariance(&baroAltMeasurement); 
     }
+
 #endif
 #ifdef USE_GPS
+    static SensorMeasurement gpsAltMeasurement;
+    gpsAltMeasurement.value = -1.0f;
     if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
         // GPS_FIX means a 3D fix, which requires min 4 sats.
         // On loss of 3D fix, gpsAltCm remains at the last value, haveGpsAlt becomes false, and gpsTrust goes to zero.
@@ -121,12 +120,27 @@ void calculateEstimatedAltitude(void)
             // When pDOP is a value less than 3.3, GPS trust will be stronger than default.
             gpsTrust = 100.0f / gpsSol.dop.pdop;
             // *** TO DO - investigate if we should use vDOP or vACC with UBlox units;
+
+            gpsAltMeasurement.variance = gpsSol.dop.pdop;
         }
         // always use at least 10% of other sources besides gps if available
         gpsTrust = MIN(gpsTrust, 0.9f);
     }
 #endif
 
+#ifdef USE_RANGEFINDER
+    haveRangefinderAlt = rangefinderIsHealthy();
+    static SensorMeasurement rangefinderMeasurement;
+    rangefinderMeasurement.value = (float)rangefinderGetLatestAltitude();
+    rangefinderMeasurement.variance = 0.05; // todo: rangefinder init variance
+    static float rangefinderOffsetCm = 0.0f;
+#endif
+
+    if(!kfInitDone) { //first kf init
+        kf_init(&kf, 0.0f, 0.5, 0.5); //to do: init variance
+        kfInitDone = true;
+    }
+    
     //  ***  DISARMED  ***
     if (!ARMING_FLAG(ARMED)) {
         if (wasArmed) { // things to run once, on disarming, after being armed
@@ -144,7 +158,8 @@ void calculateEstimatedAltitude(void)
                 displayAltitudeCm = gpsAltCm; // estimatedAltitude shows most recent ASL GPS altitude in OSD and sensors, while disarmed
             }
         }
-        zeroedAltitudeCm = 0.0f; // always hold relativeAltitude at zero while disarmed
+        zeroedGpsAltitudeCm = 0.0f; // always hold relativeAltitude at zero while disarmed
+
         DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAltCm / 100.0f); // Absolute altitude ASL in metres, max 32,767m
     //  ***  ARMED  ***
     } else {
@@ -161,45 +176,60 @@ void calculateEstimatedAltitude(void)
                 useZeroedGpsAltitude = true;
             }
             if (useZeroedGpsAltitude) { // normal situation
-                zeroedAltitudeCm = gpsAltCm - gpsAltOffsetCm; // now that we have a GPS offset value, we can use it to zero relativeAltitude
+                zeroedGpsAltitudeCm = gpsAltCm - gpsAltOffsetCm; // now that we have a GPS offset value, we can use it to zero relativeAltitude
             }
         } else {
             gpsTrust = 0.0f;
             // TO DO - smoothly reduce GPS trust, rather than immediately dropping to zero for what could be only a very brief loss of 3D fix 
         }
-        DEBUG_SET(DEBUG_ALTITUDE, 2, lrintf(zeroedAltitudeCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
-        
-        // Empirical mixing of GPS and Baro altitudes
-        if (useZeroedGpsAltitude && (positionConfig()->altitude_source == DEFAULT || positionConfig()->altitude_source == GPS_ONLY)) {
-            if (haveBaroAlt && positionConfig()->altitude_source == DEFAULT) {
-                // mix zeroed GPS with Baro altitude data, if Baro data exists if are in default altitude control mode 
-                const float absDifferenceM = fabsf(zeroedAltitudeCm - baroAltCm) / 100.0f * positionConfig()->altitude_prefer_baro / 100.0f;
-                if (absDifferenceM > 1.0f) { // when there is a large difference, favour Baro
-                    gpsTrust /=  absDifferenceM;
-                }
-                zeroedAltitudeCm = zeroedAltitudeCm * gpsTrust + baroAltCm * (1.0f - gpsTrust);
-            }
-        } else if (haveBaroAlt && (positionConfig()->altitude_source == DEFAULT || positionConfig()->altitude_source == BARO_ONLY)) {
-            zeroedAltitudeCm = baroAltCm; // use Baro if no GPS data, or we want Baro only
-        }
-    }
+        DEBUG_SET(DEBUG_ALTITUDE, 2, lrintf(zeroedGpsAltitudeCm / 10.0f)); // Relative altitude above takeoff, to 0.1m, rolls over at 3,276.7m
+    
+        // apply Kalman filter on available sensors
+        kf_update_variance(&kf);
 
-    zeroedAltitudeCm = pt2FilterApply(&altitudeLpf, zeroedAltitudeCm);
-    // NOTE: this filter must receive 0 as its input, for the whole disarmed time, to ensure correct zeroed values on arming
+#ifdef USE_RANGEFINDER
+        kf_update(&kf, rangefinderMeasurement);
+        if(haveRangefinderAlt) {
+            // update the offset to shift the zeroedGpsAltitudeCm and the baroAltCm to match the rangefinder zero point
+            rangefinderOffsetCm = 0.2 * (zeroedGpsAltitudeCm - rangefinderMeasurement.value)
+                                + 0.2 * (baroAltCm           - rangefinderMeasurement.value)
+                                + 0.6 * rangefinderOffsetCm;
+        }
+        zeroedGpsAltitudeCm -= rangefinderOffsetCm;
+        baroAltCm           -= rangefinderOffsetCm;
+#endif
+
+#ifdef USE_BARO
+        if (haveBaroAlt) {
+            baroAltMeasurement.value = baroAltCm;
+            kf_update(&kf, baroAltMeasurement);
+        }
+#endif
+
+#ifdef USE_GPS
+        if (haveGpsAlt) {
+            gpsAltMeasurement.value = zeroedGpsAltitudeCm;
+            kf_update(&kf, gpsAltMeasurement);
+        }
+#endif
+
+        zeroedFusedAltitudeCm = kf.estimatedValue;
+
+}
 
     if (wasArmed) {
-        displayAltitudeCm = zeroedAltitudeCm; // while armed, show filtered relative altitude in OSD / sensors tab
+        displayAltitudeCm = zeroedFusedAltitudeCm; // while armed, show filtered relative altitude in OSD / sensors tab
     }
 
     // *** calculate Vario signal
-    static float previousZeroedAltitudeCm = 0.0f;
-    zeroedAltitudeDerivative = (zeroedAltitudeCm - previousZeroedAltitudeCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
-    previousZeroedAltitudeCm = zeroedAltitudeCm;
+    static float previousZeroedFusedAltitudeCm = 0.0f;
+    zeroedFusedAltitudeCmDerivative = (zeroedFusedAltitudeCm - previousZeroedFusedAltitudeCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
+    previousZeroedFusedAltitudeCm = zeroedFusedAltitudeCm;
 
-    zeroedAltitudeDerivative = pt2FilterApply(&altitudeDerivativeLpf, zeroedAltitudeDerivative);
+    // zeroedFusedAltitudeCmDerivative = pt2FilterApply(&altitudeDerivativeLpf, zeroedFusedAltitudeCmDerivative); // do we still need this ?
 
 #ifdef USE_VARIO
-    estimatedVario = lrintf(zeroedAltitudeDerivative);
+    estimatedVario = lrintf(zeroedFusedAltitudeCmDerivative);
     estimatedVario = applyDeadband(estimatedVario, 10); // ignore climb rates less than 0.1 m/s
 #endif
  
@@ -210,21 +240,21 @@ void calculateEstimatedAltitude(void)
     DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedVario);
 #endif
     DEBUG_SET(DEBUG_RTH, 1, lrintf(displayAltitudeCm / 10.0f));
-    DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 2, lrintf(zeroedAltitudeCm));
+    DEBUG_SET(DEBUG_AUTOPILOT_ALTITUDE, 2, lrintf(zeroedFusedAltitudeCm));
 
-    altitudeAvailable = haveGpsAlt || haveBaroAlt;
+    altitudeAvailable = haveGpsAlt || haveBaroAlt || haveRangefinderAlt;
 }
 
-#endif //defined(USE_BARO) || defined(USE_GPS)
+#endif //defined(USE_BARO) || defined(USE_GPS) || defined(USE_RANGEFINDER)
 
 float getAltitudeCm(void)
 {
-    return zeroedAltitudeCm;
+    return zeroedFusedAltitudeCm;
 }
 
 float getAltitudeDerivative(void)
 {
-    return zeroedAltitudeDerivative; // cm/s
+    return zeroedFusedAltitudeCmDerivative; // cm/s
 }
 
 bool isAltitudeAvailable(void) {

src/main/flight/position.h

@@ -41,4 +41,3 @@ int32_t getEstimatedAltitudeCm(void);
 float getAltitudeAsl(void);
 int16_t getEstimatedVario(void);
 bool isAltitudeAvailable(void);
-