#461 Added outlier filter for the lighthouse

src/deck/drivers/src/lighthouse.c

@@ -7,7 +7,7 @@
  *
  * Crazyflie control firmware
  *
- * Copyright (C) 2018 Bitcraze AB
+ * Copyright (C) 2018-2019 Bitcraze AB
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal

src/modules/interface/kalman_core.h

@@ -129,7 +129,7 @@ void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof);
 void kalmanCoreUpdateWithYawError(kalmanCoreData_t *this, yawErrorMeasurement_t *error);
 
 // Measurement of sweep angles from a Lighthouse base station
-void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles);
+void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t tick);
 
 /**
  * Primary Kalman filter functions

src/modules/interface/outlierFilter.h

@@ -5,9 +5,7 @@
  * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
  *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
  *
- * Crazyflie control firmware
- *
- * Copyright (C) 2011-2018 Bitcraze AB
+ * Copyright (C) 2011-2019 Bitcraze AB
  *
  * This program is free software: you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -21,7 +19,7 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- * outlierFilter.h: Outlier rejection filter for the LPS system
+ * outlierFilter.h: Outlier rejection filter for the kalman filter
  */
 
 #ifndef __OUTLIER_FILTER_H__
@@ -32,6 +30,12 @@
 bool outlierFilterValidateTdoaSimple(const tdoaMeasurement_t* tdoa);
 bool outlierFilterValidateTdoaSteps(const tdoaMeasurement_t* tdoa, const float error, const vector_t* jacobian, const point_t* estPos);
 
-void outlierFilterReset();
+typedef struct {
+    uint32_t openingTime;
+    int32_t openingWindow;
+} OutlierFilterLhState_t;
+bool outlierFilterValidateLighthouseSweep(OutlierFilterLhState_t* this, const float distanceToBs, const float angleError, const uint32_t now);
+void outlierFilterReset(OutlierFilterLhState_t* this, const uint32_t now);
+
 
 #endif // __OUTLIER_FILTER_H__

src/modules/src/estimator_kalman.c

@@ -266,7 +266,7 @@ static inline float arm_sqrt(float32_t in)
 
 static void kalmanTask(void* parameters);
 static bool predictStateForward(uint32_t osTick, float dt);
-static bool updateQueuedMeasurments(const Axis3f *gyro);
+static bool updateQueuedMeasurments(const Axis3f *gyro, const uint32_t tick);
 
 
 // --------------------------------------------------
@@ -373,7 +373,7 @@ static void kalmanTask(void* parameters) {
       xSemaphoreTake(dataMutex, portMAX_DELAY);
       memcpy(&gyro, &gyroSnapshot, sizeof(gyro));
       xSemaphoreGive(dataMutex);
-      doneUpdate = doneUpdate || updateQueuedMeasurments(&gyro);
+      doneUpdate = doneUpdate || updateQueuedMeasurments(&gyro, osTick);
     }
 
     /**
@@ -501,7 +501,7 @@ static bool predictStateForward(uint32_t osTick, float dt) {
 }
 
 
-static bool updateQueuedMeasurments(const Axis3f *gyro) {
+static bool updateQueuedMeasurments(const Axis3f *gyro, const uint32_t tick) {
   bool doneUpdate = false;
   /**
    * Sensor measurements can come in sporadically and faster than the stabilizer loop frequency,
@@ -567,7 +567,7 @@ static bool updateQueuedMeasurments(const Axis3f *gyro) {
   sweepAngleMeasurement_t angles;
   while (stateEstimatorHasSweepAnglesPacket(&angles))
   {
-    kalmanCoreUpdateWithSweepAngles(&coreData, &angles);
+    kalmanCoreUpdateWithSweepAngles(&coreData, &angles, tick);
     doneUpdate = true;
   }
 

src/modules/src/kalman_core.c

@@ -165,6 +165,8 @@ static float initialQuaternion[4] = {0.0, 0.0, 0.0, 0.0};
 
 static uint32_t tdoaCount;
 
+static OutlierFilterLhState_t sweepOutlierFilterState;
+
 
 void kalmanCoreInit(kalmanCoreData_t* this) {
   tdoaCount = 0;
@@ -218,6 +220,8 @@ void kalmanCoreInit(kalmanCoreData_t* this) {
   this->Pm.pData = (float*)this->P;
 
   this->baroReferenceHeight = 0.0;
+
+  outlierFilterReset(&sweepOutlierFilterState, 0);
 }
 
 static void scalarUpdate(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, float error, float stdMeasNoise)
@@ -568,82 +572,87 @@ void kalmanCoreUpdateWithYawError(kalmanCoreData_t *this, yawErrorMeasurement_t
     scalarUpdate(this, &H, this->S[KC_STATE_D2] - error->yawError, error->stdDev);
 }
 
-static void scalarUpdateForSweep(kalmanCoreData_t *this, float measuredSweepAngle, float dp, float dx, kalmanCoreStateIdx_t state_p, float stdDev, arm_matrix_instance_f32* R) {
+static void scalarUpdateForSweep(kalmanCoreData_t *this, float measuredSweepAngle, float dp, float dx, kalmanCoreStateIdx_t state_p, float stdDev, arm_matrix_instance_f32* R, float distanceToBs, const uint32_t tick) {
   if(dx != 0) {
     float predictedSweepAngle = atan2(dp, dx);
 
-    float n = (dx * dx + dp * dp);
-    float hx = -dp / n;
-    float hp = dx / n;
+    float angleError = measuredSweepAngle - predictedSweepAngle;
+    if (outlierFilterValidateLighthouseSweep(&sweepOutlierFilterState, distanceToBs, angleError, tick)) {
+      float n = (dx * dx + dp * dp);
+      float hx = -dp / n;
+      float hp = dx / n;
 
-    float h[KC_STATE_DIM] = {0};
-    arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+      float h[KC_STATE_DIM] = {0};
+      arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
 
-    if (roth) {
-      // Rotate back to global coordinate system
-      vec3d h_b = {0, 0, 0};
-      arm_matrix_instance_f32 H_B = {3, 1, h_b};
-      h_b[KC_STATE_X] = -dp / n;
-      h_b[state_p] = dx / n;
+      if (roth) {
+        // Rotate back to global coordinate system
+        vec3d h_b = {0, 0, 0};
+        arm_matrix_instance_f32 H_B = {3, 1, h_b};
+        h_b[KC_STATE_X] = -dp / n;
+        h_b[state_p] = dx / n;
 
-      vec3d h_g = {0, 0, 0};
-      arm_matrix_instance_f32 H_G = {3, 1, h_g};
+        vec3d h_g = {0, 0, 0};
+        arm_matrix_instance_f32 H_G = {3, 1, h_g};
 
-      mat_mult(R, &H_B, &H_G);
+        mat_mult(R, &H_B, &H_G);
 
-      h[KC_STATE_X] = h_g[0];
-      h[KC_STATE_Y] = h_g[1];
-      h[KC_STATE_Z] = h_g[2];
-    } else {
-      h[KC_STATE_X] = hx;
-      h[state_p] = hp;
-    }
+        h[KC_STATE_X] = h_g[0];
+        h[KC_STATE_Y] = h_g[1];
+        h[KC_STATE_Z] = h_g[2];
+      } else {
+        h[KC_STATE_X] = hx;
+        h[state_p] = hp;
+      }
 
-    scalarUpdate(this, &H, measuredSweepAngle - predictedSweepAngle, stdDev);
+      scalarUpdate(this, &H, angleError, stdDev);
+    }
   }
 }
 
-void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles)
+void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t tick)
 {
-    // Get rotation matrix and invert it (to get the global to local rotation matrix)
+  // Get rotation matrix and invert it (to get the global to local rotation matrix)
     arm_matrix_instance_f32 basestation_rotation_matrix = {3, 3, (float32_t *)(*angles->baseStationRot)};
     arm_matrix_instance_f32 basestation_rotation_matrix_inv = {3, 3, (float32_t *)(*angles->baseStationRotInv)};
 
-    // Rotate the sensor position using the CF roatation matrix, to rotate it to global coordinates
-    arm_matrix_instance_f32 CF_ROT_MATRIX = {3, 3, (float32_t *)this->R};
-    arm_matrix_instance_f32 SENSOR_RELATVIVE_POS = {3, 1, *angles->sensorPos};
-    vec3d sensor_relative_pos_glob = {0};
-    arm_matrix_instance_f32 SENSOR_RELATVIVE_POS_GLOB = {3, 1, sensor_relative_pos_glob};
-    mat_mult(&CF_ROT_MATRIX, &SENSOR_RELATVIVE_POS, &SENSOR_RELATVIVE_POS_GLOB);
-
-    // Get the current state values of the position of the crazyflie and add the relative sensor pos
-    float pos_x = this->S[KC_STATE_X] + sensor_relative_pos_glob[0];
-    float pos_y = this->S[KC_STATE_Y] + sensor_relative_pos_glob[1];
-    float pos_z = this->S[KC_STATE_Z] + sensor_relative_pos_glob[2];
-
-    // Calculate the difference between the base stations and the sensor on the CF.
-    const float* baseStationPos = *angles->baseStationPos;
-    float dx = pos_x - baseStationPos[0];
-    float dy = pos_y - baseStationPos[1];
-    float dz = pos_z - baseStationPos[2];
-
-    // Rotate the difference in position to be relative to the basestation
-    vec3d position_diff = {dx, dy, dz};
-    vec3d position_diff_rotated = {0, 0, 0};
-    arm_matrix_instance_f32 vec_pos_diff = {3, 1, position_diff};
-    arm_matrix_instance_f32 vec_pos_diff_rot = {3, 1, position_diff_rotated};
-    mat_mult(&basestation_rotation_matrix_inv, &vec_pos_diff, &vec_pos_diff_rot);
-
-    float dx_rot = position_diff_rotated[0];
-    float dy_rot = position_diff_rotated[1];
-    float dz_rot = position_diff_rotated[2];
-
-    // Retrieve the measured sweepangles
-    float measuredSweepAngleHorizontal = angles->angleX;
-    float measuredSweepAngleVertical = angles->angleY;
-
-    scalarUpdateForSweep(this, measuredSweepAngleHorizontal, dy_rot, dx_rot, KC_STATE_Y, angles->stdDevX, &basestation_rotation_matrix);
-    scalarUpdateForSweep(this, measuredSweepAngleVertical, dz_rot, dx_rot, KC_STATE_Z, angles->stdDevY, &basestation_rotation_matrix);
+  // Rotate the sensor position using the CF roatation matrix, to rotate it to global coordinates
+  arm_matrix_instance_f32 CF_ROT_MATRIX = {3, 3, (float32_t *)this->R};
+  arm_matrix_instance_f32 SENSOR_RELATVIVE_POS = {3, 1, *angles->sensorPos};
+  vec3d sensor_relative_pos_glob = {0};
+  arm_matrix_instance_f32 SENSOR_RELATVIVE_POS_GLOB = {3, 1, sensor_relative_pos_glob};
+  mat_mult(&CF_ROT_MATRIX, &SENSOR_RELATVIVE_POS, &SENSOR_RELATVIVE_POS_GLOB);
+
+  // Get the current state values of the position of the crazyflie and add the relative sensor pos
+  float pos_x = this->S[KC_STATE_X] + sensor_relative_pos_glob[0];
+  float pos_y = this->S[KC_STATE_Y] + sensor_relative_pos_glob[1];
+  float pos_z = this->S[KC_STATE_Z] + sensor_relative_pos_glob[2];
+
+  // Calculate the difference between the base stations and the sensor on the CF.
+  const float* baseStationPos = *angles->baseStationPos;
+  float dx = pos_x - baseStationPos[0];
+  float dy = pos_y - baseStationPos[1];
+  float dz = pos_z - baseStationPos[2];
+
+  // Rotate the difference in position to be relative to the basestation
+  vec3d position_diff = {dx, dy, dz};
+  vec3d position_diff_rotated = {0, 0, 0};
+  arm_matrix_instance_f32 vec_pos_diff = {3, 1, position_diff};
+  arm_matrix_instance_f32 vec_pos_diff_rot = {3, 1, position_diff_rotated};
+  mat_mult(&basestation_rotation_matrix_inv, &vec_pos_diff, &vec_pos_diff_rot);
+
+  float dx_rot = position_diff_rotated[0];
+  float dy_rot = position_diff_rotated[1];
+  float dz_rot = position_diff_rotated[2];
+
+  // Retrieve the measured sweepangles
+  float measuredSweepAngleHorizontal = angles->angleX;
+  float measuredSweepAngleVertical = angles->angleY;
+
+  float distanceToBs = arm_sqrt(dx * dx + dy * dy + dz * dz);
+
+  scalarUpdateForSweep(this, measuredSweepAngleHorizontal, dy_rot, dx_rot, KC_STATE_Y, angles->stdDevX, &basestation_rotation_matrix, distanceToBs, tick);
+  scalarUpdateForSweep(this, measuredSweepAngleVertical, dz_rot, dx_rot, KC_STATE_Z, angles->stdDevY, &basestation_rotation_matrix, distanceToBs, tick);
 }
 
 void kalmanCorePredict(kalmanCoreData_t* this, float cmdThrust, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying)
@@ -1119,6 +1128,10 @@ LOG_GROUP_START(kalman_pred)
   LOG_ADD(LOG_FLOAT, measNY, &measuredNY)
 LOG_GROUP_STOP(kalman_pred)
 
+LOG_GROUP_START(outlierf)
+  LOG_ADD(LOG_INT32, lhWin, &sweepOutlierFilterState.openingWindow)
+LOG_GROUP_STOP(outlierf)
+
 PARAM_GROUP_START(kalman)
   PARAM_ADD(PARAM_FLOAT, pNAcc_xy, &procNoiseAcc_xy)
   PARAM_ADD(PARAM_FLOAT, pNAcc_z, &procNoiseAcc_z)

src/modules/src/outlierFilter.c

@@ -28,6 +28,7 @@
 #include "outlierFilter.h"
 #include "stabilizer_types.h"
 #include "log.h"
+#include "debug.h"
 
 #define BUCKET_ACCEPTANCE_LEVEL 2
 #define MAX_BUCKET_FILL 10
@@ -101,10 +102,50 @@ bool outlierFilterValidateTdoaSteps(const tdoaMeasurement_t* tdoa, const float e
   return sampleIsGood;
 }
 
-void outlierFilterReset() {
-  // Nothing here
+
+static const uint32_t lhTicksPerFrame = 1000 / 120;
+static const int32_t lhMinWindowTime = -2 * lhTicksPerFrame;
+static const int32_t lhMaxWindowTime = 5 * lhTicksPerFrame;
+static const int32_t lhBadSampleWindowChange = -lhTicksPerFrame;
+static const int32_t lhGoodSampleWindowChange = lhTicksPerFrame / 2;
+static const float lhMaxError = 0.05f;
+
+void outlierFilterReset(OutlierFilterLhState_t* this, const uint32_t now) {
+  this->openingTime = now;
+  this->openingWindow = lhMinWindowTime;
 }
 
+
+bool outlierFilterValidateLighthouseSweep(OutlierFilterLhState_t* this, const float distanceToBs, const float angleError, const uint32_t now) {
+  // float error = distanceToBs * tan(angleError);
+  // We use an approximattion
+  float error = distanceToBs * angleError;
+
+  bool isGoodSample = (fabsf(error) < lhMaxError);
+  if (isGoodSample) {
+    this->openingWindow += lhGoodSampleWindowChange;
+    if (this->openingWindow > lhMaxWindowTime) {
+      this->openingWindow = lhMaxWindowTime;
+    }
+  } else {
+    this->openingWindow += lhBadSampleWindowChange;
+    if (this->openingWindow < lhMinWindowTime) {
+      this->openingWindow = lhMinWindowTime;
+    }
+  }
+
+  bool result = true;
+  bool isFilterClosed = (now < this->openingTime);
+  if (isFilterClosed) {
+    result = isGoodSample;
+  }
+
+  this->openingTime = now + this->openingWindow;
+
+  return result;
+}
+
+
 static bool isDistanceDiffSmallerThanDistanceBetweenAnchors(const tdoaMeasurement_t* tdoa) {
   float anchorDistanceSq = distanceSq(&tdoa->anchorPosition[0], &tdoa->anchorPosition[1]);
   float distanceDiffSq = sq(tdoa->distanceDiff);
@@ -156,5 +197,4 @@ LOG_GROUP_START(outlierf)
   LOG_ADD(LOG_INT32, bucket4, &filterLevels[4].bucket)
   LOG_ADD(LOG_FLOAT, accLev, &acceptanceLevel)
   LOG_ADD(LOG_FLOAT, errD, &errorDistance)
-
 LOG_GROUP_STOP(outlierf)