Kalman filter modification for constant altitude flight

src/deck/drivers/src/multiranger.c

@@ -43,6 +43,8 @@
 
 #include <stdlib.h>
 
+#include "cf_math.h"
+
 static bool isInit = false;
 static bool isTested = false;
 static bool isPassed = false;
@@ -53,12 +55,21 @@ static bool isPassed = false;
 #define MR_PIN_LEFT   PCA95X4_P6
 #define MR_PIN_RIGHT  PCA95X4_P2
 
+#define RANGE_UP_OUTLIER_LIMIT 5000 // the measured range is in [mm]
+
 NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devFront;
 NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devBack;
 NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devUp;
 NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devLeft;
 NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devRight;
 
+// Measurement noise model
+static const float expPointA = 2.5f;
+static const float expStdA = 0.0025f; // STD at elevation expPointA [m]
+static const float expPointB = 4.0f;
+static const float expStdB = 0.2f;    // STD at elevation expPointB [m]
+static float expCoeff;
+
 static bool mrInitSensor(VL53L1_Dev_t *pdev, uint32_t pca95pin, char *name)
 {
   bool status;
@@ -79,6 +90,10 @@ static bool mrInitSensor(VL53L1_Dev_t *pdev, uint32_t pca95pin, char *name)
       status = false;
   }
 
+  // pre-compute constant in the measurement noise model for kalman
+  expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA);
+
+
   return status;
 }
 
@@ -129,12 +144,19 @@ static void mrTask(void *param)
     while (1)
     {
         vTaskDelayUntil(&lastWakeTime, M2T(100));
-
+        float distanceUp = mrGetMeasurementAndRestart(&devUp)/1000.0f;
         rangeSet(rangeFront, mrGetMeasurementAndRestart(&devFront)/1000.0f);
         rangeSet(rangeBack, mrGetMeasurementAndRestart(&devBack)/1000.0f);
+        rangeSet(rangeUp, distanceUp);
         rangeSet(rangeUp, mrGetMeasurementAndRestart(&devUp)/1000.0f);
         rangeSet(rangeLeft, mrGetMeasurementAndRestart(&devLeft)/1000.0f);
         rangeSet(rangeRight, mrGetMeasurementAndRestart(&devRight)/1000.0f);
+
+        // Add up range to kalman filter measurements
+        if (distanceUp < RANGE_UP_OUTLIER_LIMIT) {
+            float stdDev = expStdA * (1.0f  + expf( expCoeff * (distanceUp - expPointA)));
+            rangeEnqueueUpRangeInEstimator(distanceUp, stdDev, xTaskGetTickCount());
+        }
     }
 }
 

src/modules/interface/estimator.h

@@ -43,6 +43,7 @@ typedef enum {
   MeasurementTypePose,
   MeasurementTypeDistance,
   MeasurementTypeTOF,
+  MeasurementTypeUpTOF,
   MeasurementTypeAbsoluteHeight,
   MeasurementTypeFlow,
   MeasurementTypeYawError,
@@ -62,6 +63,7 @@ typedef struct
     poseMeasurement_t pose;
     distanceMeasurement_t distance;
     tofMeasurement_t tof;
+    tofMeasurement_t uptof;
     heightMeasurement_t height;
     flowMeasurement_t flow;
     yawErrorMeasurement_t yawError;
@@ -123,6 +125,14 @@ static inline void estimatorEnqueueTOF(const tofMeasurement_t *tof)
   estimatorEnqueue(&m);
 }
 
+static inline void estimatorEnqueueUpTOF(const tofMeasurement_t *tof)
+{
+  measurement_t m;
+  m.type = MeasurementTypeUpTOF;
+  m.data.uptof = *tof;
+  estimatorEnqueue(&m);
+}
+
 static inline void estimatorEnqueueAbsoluteHeight(const heightMeasurement_t *height)
 {
   measurement_t m;

src/modules/interface/kalman_core/kalman_core.h

@@ -59,7 +59,7 @@
 // Indexes to access the quad's state, stored as a column vector
 typedef enum
 {
-  KC_STATE_X, KC_STATE_Y, KC_STATE_Z, KC_STATE_PX, KC_STATE_PY, KC_STATE_PZ, KC_STATE_D0, KC_STATE_D1, KC_STATE_D2, KC_STATE_DIM
+  KC_STATE_X, KC_STATE_Y, KC_STATE_Z, KC_STATE_PX, KC_STATE_PY, KC_STATE_PZ, KC_STATE_D0, KC_STATE_D1, KC_STATE_D2, KC_STATE_F, KC_STATE_R, KC_STATE_DIM
 } kalmanCoreStateIdx_t;
 
 
@@ -74,6 +74,10 @@ typedef struct {
    * - D0, D1, D2: attitude error
    *
    * For more information, refer to the paper
+   * 
+   * Add two states for better z estimation using range measurements:
+   * - F: Estimated distance from the starting z position to what is below the CF
+   * - R: Estimated distance from the starting z position to the current roof above the CF
    */
   float S[KC_STATE_DIM];
 
@@ -93,6 +97,9 @@ typedef struct {
   bool resetEstimation;
 
   float baroReferenceHeight;
+
+  // Flag to signal if R (roof height estimate) has been intialized
+  bool stateRInitialized;
 } kalmanCoreData_t;
 
 

src/modules/interface/kalman_core/mm_flow.h

@@ -29,3 +29,6 @@
 
 // Measurements of flow (dnx, dny)
 void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *flow, const Axis3f *gyro);
+
+// Kalman update with optical flow measurements using F (floor height estimate)
+void kalmanCoreUpdateWithFlowUsingF(kalmanCoreData_t* this, const flowMeasurement_t *flow, const Axis3f *gyro);

src/modules/interface/kalman_core/mm_tof.h

@@ -29,3 +29,9 @@
 
 // Measurements of TOF from laser sensor
 void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof);
+
+// Kalman update using the TOF from downward laser sensor and F (floor height estimate)
+void kalmanCoreUpdateWithTofUsingF(kalmanCoreData_t* this, tofMeasurement_t *tof);
+
+// Kalman update using the TOF from upward laser sensor and R (roof height estimate)
+void kalmanCoreUpdateWithUpTofUsingR(kalmanCoreData_t* this, tofMeasurement_t *tof);

src/modules/interface/range.h

@@ -61,3 +61,12 @@ float rangeGet(rangeDirection_t direction);
  * @param timeStamp The time when the range was sampled (in sys ticks)
  */
 void rangeEnqueueDownRangeInEstimator(float distance, float stdDev, uint32_t timeStamp);
+
+/**
+ * Enqueue a upward range measurement for distance to the roof in the current estimator.
+ *
+ * @param distance Distance to the roof (m)
+ * @param stdDev The standard deviation of the range sample
+ * @param timeStamp The time when the range was sampled (in sys ticks)
+ */
+void rangeEnqueueUpRangeInEstimator(float distance, float stdDev, uint32_t timeStamp); 

src/modules/src/estimator_kalman.c

@@ -120,6 +120,9 @@ static StaticSemaphore_t dataMutexBuffer;
 static bool robustTwr = false;
 static bool robustTdoa = false;
 
+// Nonzero to take advantage of the extended state (F and R) to get a better estimate of Z
+static bool useFAndR = true;
+
 /**
  * Quadrocopter State
  *
@@ -129,6 +132,10 @@ static bool robustTdoa = false;
  * - D0, D1, D2: attitude error
  *
  * For more information, refer to the paper
+ * 
+ * Add two states for better z estimation using range measurements:
+ * - F: Estimated distance from the starting z position to what is below the CF
+ * - R: Estimated distance from the starting z position to the current roof above the CF
  */
 
 NO_DMA_CCM_SAFE_ZERO_INIT static kalmanCoreData_t coreData;
@@ -372,15 +379,34 @@ static bool updateQueuedMeasurements(const uint32_t tick) {
         doneUpdate = true;
         break;
       case MeasurementTypeTOF:
-        kalmanCoreUpdateWithTof(&coreData, &m.data.tof);
+       if(useFAndR){
+          // Tof update using the estimated height of the floor (f)
+          kalmanCoreUpdateWithTofUsingF(&coreData, &m.data.tof);
+        }else{
+          // Standard Tof update
+          kalmanCoreUpdateWithTof(&coreData, &m.data.tof);
+        }
         doneUpdate = true;
         break;
+      case MeasurementTypeUpTOF:
+        if(useFAndR){
+          // Tof update using the upward range measurement and the estimated height of the roof (r)
+          kalmanCoreUpdateWithUpTofUsingR(&coreData, &m.data.tof);
+          doneUpdate = true;
+        }
+        break;
       case MeasurementTypeAbsoluteHeight:
         kalmanCoreUpdateWithAbsoluteHeight(&coreData, &m.data.height);
         doneUpdate = true;
         break;
       case MeasurementTypeFlow:
-        kalmanCoreUpdateWithFlow(&coreData, &m.data.flow, &gyroLatest);
+        if(useFAndR){
+          // Flow update using the estimated height of the floor 
+          kalmanCoreUpdateWithFlowUsingF(&coreData, &m.data.flow, &gyroLatest);
+        }else{
+          // Standard flow update
+          kalmanCoreUpdateWithFlow(&coreData, &m.data.flow, &gyroLatest);
+        }
         doneUpdate = true;
         break;
       case MeasurementTypeYawError:
@@ -506,6 +532,14 @@ LOG_GROUP_START(kalman)
   */
   LOG_ADD(LOG_FLOAT, stateD2, &coreData.S[KC_STATE_D2])
   /**
+  * @brief Estimated floor height compared to the reference point where Z = 0
+  */  
+  LOG_ADD(LOG_FLOAT, stateF, &coreData.S[KC_STATE_F])
+  /**
+  * @brief Estimated roof height compared to the reference point where Z = 0
+  */  
+  LOG_ADD(LOG_FLOAT, stateR, &coreData.S[KC_STATE_R])
+  /**
   * @brief Covariance matrix position x
   */
   LOG_ADD(LOG_FLOAT, varX, &coreData.P[KC_STATE_X][KC_STATE_X])
@@ -542,6 +576,14 @@ LOG_GROUP_START(kalman)
   */
   LOG_ADD(LOG_FLOAT, varD2, &coreData.P[KC_STATE_D2][KC_STATE_D2])
   /**
+  * @brief Covariance matrix floor height estimate
+  */
+  LOG_ADD(LOG_FLOAT, varF, &coreData.P[KC_STATE_F][KC_STATE_F])
+  /**
+  * @brief Covariance matrix roof height estimate
+  */
+  LOG_ADD(LOG_FLOAT, varR, &coreData.P[KC_STATE_R][KC_STATE_R])
+  /**
   * @brief Estimated Attitude quarternion w
   */
   LOG_ADD(LOG_FLOAT, q0, &coreData.q[0])
@@ -593,4 +635,8 @@ PARAM_GROUP_START(kalman)
  * @brief Nonzero to use robust TWR method (default: 0)
  */
   PARAM_ADD_CORE(PARAM_UINT8, robustTwr, &robustTwr)
+/**
+ * @brief Nonzero to use F (floor height) and R (roof height) in the estimation for a better Z estimate
+ */
+  PARAM_ADD_CORE(PARAM_UINT8, useFAndR, &useFAndR)
 PARAM_GROUP_STOP(kalman)

src/modules/src/kalman_core/kalman_core.c

@@ -131,6 +131,11 @@ static const float stdDevInitialVelocity = 0.01;
 static const float stdDevInitialAttitude_rollpitch = 0.01;
 static const float stdDevInitialAttitude_yaw = 0.01;
 
+static const float stdDevInitialF = 0; // initialF is almost true by definition 
+static const float stdDevInitialR = 0;
+static float procNoiseF = 0; // This could be tuned to some other value, but it seems natural for it to be 0
+static float procNoiseR = 0; // This could be tuned to some other value, but it seems natural for it to be 0
+
 static float procNoiseAcc_xy = 0.5f;
 static float procNoiseAcc_z = 1.0f;
 static float procNoiseVel = 0;
@@ -144,6 +149,9 @@ static float initialX = 0.0;
 static float initialY = 0.0;
 static float initialZ = 0.0;
 
+static float initialF = 0.0;
+static float initialR = -1.0; // It does not matter since it is overwritten on the first range up measurement (as long as stateRInitialized is set to false).
+
 // Initial yaw of the Crazyflie in radians.
 // 0 --- facing positive X
 // PI / 2 --- facing positive Y
@@ -168,6 +176,11 @@ void kalmanCoreInit(kalmanCoreData_t* this) {
 //  this->S[KC_STATE_D0] = 0;
 //  this->S[KC_STATE_D1] = 0;
 //  this->S[KC_STATE_D2] = 0;
+
+  this->S[KC_STATE_F] = initialF;
+  this->S[KC_STATE_R] = initialR;
+  // This is set to false so that the first range up measurement will become the initial value 
+  this->stateRInitialized = false;
 
   // reset the attitude quaternion
   initialQuaternion[0] = arm_cos_f32(initialYaw / 2);
@@ -200,6 +213,9 @@ void kalmanCoreInit(kalmanCoreData_t* this) {
   this->P[KC_STATE_D1][KC_STATE_D1] = powf(stdDevInitialAttitude_rollpitch, 2);
   this->P[KC_STATE_D2][KC_STATE_D2] = powf(stdDevInitialAttitude_yaw, 2);
 
+  this->P[KC_STATE_F][KC_STATE_F] = powf(stdDevInitialF, 2);
+  this->P[KC_STATE_R][KC_STATE_R] = powf(stdDevInitialR, 2);
+
   this->Pm.numRows = KC_STATE_DIM;
   this->Pm.numCols = KC_STATE_DIM;
   this->Pm.pData = (float*)this->P;
@@ -380,6 +396,9 @@ void kalmanCorePredict(kalmanCoreData_t* this, Axis3f *acc, Axis3f *gyro, float
   A[KC_STATE_D1][KC_STATE_D1] = 1;
   A[KC_STATE_D2][KC_STATE_D2] = 1;
 
+  A[KC_STATE_F][KC_STATE_F] = 1;
+  A[KC_STATE_R][KC_STATE_R] = 1;
+
   // position from body-frame velocity
   A[KC_STATE_X][KC_STATE_PX] = this->R[0][0]*dt;
   A[KC_STATE_Y][KC_STATE_PX] = this->R[1][0]*dt;
@@ -581,6 +600,9 @@ void kalmanCoreAddProcessNoise(kalmanCoreData_t* this, float dt)
     this->P[KC_STATE_D0][KC_STATE_D0] += powf(measNoiseGyro_rollpitch * dt + procNoiseAtt, 2);
     this->P[KC_STATE_D1][KC_STATE_D1] += powf(measNoiseGyro_rollpitch * dt + procNoiseAtt, 2);
     this->P[KC_STATE_D2][KC_STATE_D2] += powf(measNoiseGyro_yaw * dt + procNoiseAtt, 2);
+
+    this->P[KC_STATE_F][KC_STATE_F] += powf(procNoiseF, 2);
+    this->P[KC_STATE_R][KC_STATE_R] += powf(procNoiseR, 2);
   }
 
   for (int i=0; i<KC_STATE_DIM; i++) {
@@ -663,6 +685,9 @@ void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick)
     A[KC_STATE_PY][KC_STATE_PY] = 1;
     A[KC_STATE_PZ][KC_STATE_PZ] = 1;
 
+    A[KC_STATE_F][KC_STATE_F] = 1;
+    A[KC_STATE_R][KC_STATE_R] = 1;
+
     A[KC_STATE_D0][KC_STATE_D0] =  1 - d1*d1/2 - d2*d2/2;
     A[KC_STATE_D0][KC_STATE_D1] =  d2 + d0*d1/2;
     A[KC_STATE_D0][KC_STATE_D2] = -d1 + d0*d2/2;
@@ -814,6 +839,14 @@ PARAM_GROUP_START(kalman)
  */
   PARAM_ADD_CORE(PARAM_FLOAT, pNAtt, &procNoiseAtt)
  /**
+ * @brief Process noise for F (floor height)
+ */
+  PARAM_ADD_CORE(PARAM_FLOAT, pNF, &procNoiseF)
+ /**
+ * @brief Process noise for R (roof height)
+ */
+  PARAM_ADD_CORE(PARAM_FLOAT, pNR, &procNoiseR)
+ /**
  * @brief Measurement noise for barometer
  */
   PARAM_ADD_CORE(PARAM_FLOAT, mNBaro, &measNoiseBaro)
@@ -841,4 +874,12 @@ PARAM_GROUP_START(kalman)
  * @brief Initial Yaw after reset [rad]
  */
   PARAM_ADD_CORE(PARAM_FLOAT, initialYaw, &initialYaw)
+ /**
+ * @brief Initial F (floor height) after reset [m]
+ */
+  PARAM_ADD_CORE(PARAM_FLOAT, initialF, &initialF)
+ /**
+ * @brief Initial R (roof height) after reset [m] (does not really matter since the first range up measurement after reset is used to initialize)
+ */
+  PARAM_ADD_CORE(PARAM_FLOAT, initialR, &initialR)
 PARAM_GROUP_STOP(kalman)