Merge pull request #174 from mgreiff/EKF-ranging

Add measurement update with elevation laser ranging in the EKF

src/deck/drivers/src/vl53l0x.c

@@ -35,9 +35,26 @@
 #include "log.h"
 
 #include "i2cdev.h"
-
 #include "vl53l0x.h"
 
+#include "stabilizer_types.h"
+#ifdef ESTIMATOR_TYPE_kalman
+
+#include "estimator_kalman.h"
+#include "arm_math.h"
+
+//#define UPDATE_KALMAN_WITH_RANGING // uncomment to push into the kalman
+#ifdef UPDATE_KALMAN_WITH_RANGING
+#define RANGE_OUTLIER_LIMIT 1500 // the measured range is in [mm]
+// Measurement noise model
+static float expPointA = 1.0f;
+static float expStdA = 0.0025f; // STD at elevation expPointA [m]
+static float expPointB = 1.3f;
+static float expStdB = 0.2f;    // STD at elevation expPointB [m]
+static float expCoeff;
+#endif // UPDATE_KALMAN_WITH_RANGING
+#endif // ESTIMATOR_TYPE_kalman
+
 static uint8_t devAddr;
 static I2C_Dev *I2Cx;
 static bool isInit;
@@ -120,6 +137,7 @@ static bool vl53l0xWriteReg32Bit(uint8_t reg, uint32_t val);
 /** Default constructor, uses default I2C address.
  * @see VL53L0X_DEFAULT_ADDRESS
  */
+
 void vl53l0xInit(DeckInfo* info)
 {
   if (isInit)
@@ -128,9 +146,13 @@ void vl53l0xInit(DeckInfo* info)
   i2cdevInit(I2C1_DEV);
   I2Cx = I2C1_DEV;
   devAddr = VL53L0X_DEFAULT_ADDRESS;
-
   xTaskCreate(vl53l0xTask, "vl53l0x", 2*configMINIMAL_STACK_SIZE, NULL, 3, NULL);
-
+
+#if defined(ESTIMATOR_TYPE_kalman) && defined(UPDATE_KALMAN_WITH_RANGING)
+  // pre-compute constant in the measurement noise mdoel
+  expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA);
+#endif
+
   isInit = true;
 }
 
@@ -140,7 +162,7 @@ bool vl53l0xTest(void)
 
   if (!isInit)
     return false;
-
+       // Measurement noise model
   testStatus  = vl53l0xTestConnection();
   testStatus &= vl53l0xInitSensor(true);
 
@@ -154,10 +176,24 @@ void vl53l0xTask(void* arg)
 
   vl53l0xSetVcselPulsePeriod(VcselPeriodPreRange, 18);
   vl53l0xSetVcselPulsePeriod(VcselPeriodFinalRange, 14);
-  vl53l0xStartContinuous(0);
+  vl53l0xStartContinuous(100);
   while (1) {
     xLastWakeTime = xTaskGetTickCount();
     range_last = vl53l0xReadRangeContinuousMillimeters();
+#if defined(ESTIMATOR_TYPE_kalman) && defined(UPDATE_KALMAN_WITH_RANGING)
+    // check if range is feasible and push into the kalman filter
+    // the sensor should not be able to measure >3 [m], and outliers typically
+    // occur as >8 [m] measurements
+    if (range_last < RANGE_OUTLIER_LIMIT){
+
+      // Form measurement
+      tofMeasurement_t tofData;
+      tofData.timestamp = xTaskGetTickCount();
+      tofData.distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
+      tofData.stdDev = expStdA * (1.0f  + expf( expCoeff * ( tofData.distance - expPointA)));
+      stateEstimatorEnqueueTOF(&tofData);
+    }
+#endif
     vTaskDelayUntil(&xLastWakeTime, M2T(measurement_timing_budget_ms));
   }
 }
@@ -1105,8 +1141,8 @@ bool vl53l0xWriteReg32Bit(uint8_t reg, uint32_t val)
 
 // TODO: Decide on vid:pid and set the used pins
 static const DeckDriver vl53l0x_deck = {
-  .vid = 0,
-  .pid = 0,
+  .vid = 0, // Changed this from 0
+  .pid = 0, // Changed this from 0
   .name = "vl53l0x",
   .usedGpio = 0,
 

src/modules/interface/estimator_kalman.h

@@ -67,3 +67,4 @@ bool stateEstimatorTest(void);
 bool stateEstimatorEnqueueTDOA(tdoaMeasurement_t *uwb);
 bool stateEstimatorEnqueuePosition(positionMeasurement_t *pos);
 bool stateEstimatorEnqueueDistance(distanceMeasurement_t *dist);
+bool stateEstimatorEnqueueTOF(tofMeasurement_t *tof);

src/modules/interface/stabilizer_types.h

@@ -173,6 +173,13 @@ typedef struct setpointZ_s {
   bool isUpdate; // True = small update of setpoint, false = completely new
 } setpointZ_t;
 
+/** TOF measurement**/
+typedef struct tofMeasurement_s {
+  uint32_t timestamp;
+  float distance;
+  float stdDev;
+} tofMeasurement_t;
+
 // Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number.
 #define RATE_1000_HZ 1000
 #define RATE_500_HZ 500

src/modules/src/estimator_kalman.c

@@ -141,6 +141,15 @@ static inline bool stateEstimatorHasTDOAPacket(tdoaMeasurement_t *uwb) {
   return (pdTRUE == xQueueReceive(tdoaDataQueue, uwb, 0));
 }
 
+// Measurements of TOF from laser sensor
+static xQueueHandle tofDataQueue;
+#define TOF_QUEUE_LENGTH (10)
+
+static void stateEstimatorUpdateWithTof(tofMeasurement_t *tof);
+
+static inline bool stateEstimatorHasTOFPacket(tofMeasurement_t *tof) {
+  return (pdTRUE == xQueueReceive(tofDataQueue, tof, 0));
+}
 
 /**
  * Constants used in the estimator
@@ -261,8 +270,6 @@ static uint32_t lastFlightCmd;
 static uint32_t takeoffTime;
 static uint32_t tdoaCount;
 
-
-
 /**
  * Supporting and utility functions
  */
@@ -421,6 +428,13 @@ void stateEstimatorUpdate(state_t *state, sensorData_t *sensors, control_t *cont
    * we therefore consume all measurements since the last loop, rather than accumulating
    */
 
+  tofMeasurement_t tof;
+  while (stateEstimatorHasTOFPacket(&tof))
+  {
+    stateEstimatorUpdateWithTof(&tof);
+    doneUpdate = true;
+  }
+
   distanceMeasurement_t dist;
   while (stateEstimatorHasDistanceMeasurement(&dist))
   {
@@ -442,7 +456,6 @@ void stateEstimatorUpdate(state_t *state, sensorData_t *sensors, control_t *cont
     doneUpdate = true;
   }
 
-
   /**
    * If an update has been made, the state is finalized:
    * - the attitude error is moved into the body attitude quaternion,
@@ -923,6 +936,32 @@ static void stateEstimatorUpdateWithTDOA(tdoaMeasurement_t *tdoa)
   tdoaCount++;
 }
 
+static void stateEstimatorUpdateWithTof(tofMeasurement_t *tof)
+{
+  // Updates the filter with a measured distance in the zb direction using the
+  float h[STATE_DIM] = {0};
+  arm_matrix_instance_f32 H = {1, STATE_DIM, h};
+
+  // Only update the filter if the measurement is reliable (\hat{h} -> infty when R[2][2] -> 0)
+  if (fabs(R[2][2]) > 0.1 && R[2][2] > 0){
+    float angleOfApterure = 10 * DEG_TO_RAD; // Half aperture angle radians
+    float alpha = acosf(R[2][2]) - angleOfApterure;
+    if (alpha < 0.0f){
+      alpha = 0.0f;
+    }
+    float predictedDistance = S[STATE_Z] / cosf(alpha);
+    float measuredDistance = tof->distance; // [m]
+
+    //Measurement equation
+    //
+    // h = z/((R*z_b)\dot z_b) = z/cos(alpha)
+    h[STATE_Z] = 1 / cosf(alpha); 
+
+    // Scalar update
+    stateEstimatorScalarUpdate(&H, measuredDistance-predictedDistance, tof->stdDev);
+  }
+}
+
 static void stateEstimatorFinalize(sensorData_t *sensors, uint32_t tick)
 {
   // Matrix to rotate the attitude covariances once updated
@@ -1107,12 +1146,14 @@ void stateEstimatorInit(void) {
     distDataQueue = xQueueCreate(DIST_QUEUE_LENGTH, sizeof(distanceMeasurement_t));
     posDataQueue = xQueueCreate(POS_QUEUE_LENGTH, sizeof(positionMeasurement_t));
     tdoaDataQueue = xQueueCreate(UWB_QUEUE_LENGTH, sizeof(tdoaMeasurement_t));
+    tofDataQueue = xQueueCreate(TOF_QUEUE_LENGTH, sizeof(tofMeasurement_t));
   }
   else
   {
     xQueueReset(distDataQueue);
     xQueueReset(posDataQueue);
     xQueueReset(tdoaDataQueue);
+    xQueueReset(tofDataQueue);
   }
 
   lastPrediction = xTaskGetTickCount();
@@ -1207,6 +1248,12 @@ bool stateEstimatorEnqueueDistance(distanceMeasurement_t *dist)
   return stateEstimatorEnqueueExternalMeasurement(distDataQueue, (void *)dist);
 }
 
+bool stateEstimatorEnqueueTOF(tofMeasurement_t *tof)
+{
+  // A distance (distance) [m] to the ground along the z_B axis.
+  return stateEstimatorEnqueueExternalMeasurement(tofDataQueue, (void *)tof);
+}
+
 bool stateEstimatorTest(void)
 {
   // TODO: Figure out what we could test?