Refactor kalman estimator

src/modules/interface/axis3fSubSampler.h

@@ -0,0 +1,62 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2023 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
+ * the Free Software Foundation, in version 3.
+ *
+ * This program 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/>.
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include "imu_types.h"
+
+typedef struct {
+  Axis3f sum;
+  uint32_t count;
+  float conversionFactor;
+
+  Axis3f subSample;
+} Axis3fSubSampler_t;
+
+/**
+ * @brief Initialize sub sampler
+ *
+ * @param this  Pointer to sub sampler
+ * @param conversionFactor  Conversion factor used for unit conversion.
+ */
+void axis3fSubSamplerInit(Axis3fSubSampler_t* this, const float conversionFactor);
+
+/**
+ * @brief Accumulate a sample
+ *
+ * @param this  Pointer to sub sampler
+ * @param sample  The sample to accumulate
+ */
+void axis3fSubSamplerAccumulate(Axis3fSubSampler_t* this, const Axis3f* sample);
+
+/**
+ * @brief Compute the sub sample, uses simple averaging of samples. The sub sample is multiplied with the conversion
+ * factor and the result is stored in the subSample member of the Axis3fSubSampler_t.
+ *
+ * @param this  Pointer to sub sampler
+ * @return Axis3f*  Pointer to the resulting sub sample
+ */
+Axis3f* axis3fSubSamplerFinalize(Axis3fSubSampler_t* this);

src/modules/interface/kalman_core/kalman_core.h

@@ -93,6 +93,12 @@ typedef struct {
 
   // Quaternion used for initial orientation [w,x,y,z]
   float initialQuaternion[4];
+
+  // Tracks whether an update to the state has been made, and the state therefore requires finalization
+  bool isUpdated;
+
+  uint32_t lastPredictionMs;
+  uint32_t lastProcessNoiseUpdateMs;
 } kalmanCoreData_t;
 
 // The parameters used by the filter
@@ -129,7 +135,7 @@ typedef struct {
 void kalmanCoreDefaultParams(kalmanCoreParams_t *params);
 
 /*  - Initialize Kalman State */
-void kalmanCoreInit(kalmanCoreData_t *this, const kalmanCoreParams_t *params);
+void kalmanCoreInit(kalmanCoreData_t *this, const kalmanCoreParams_t *params, const uint32_t nowMs);
 
 /*  - Measurement updates based on sensors */
 
@@ -141,15 +147,21 @@ void kalmanCoreUpdateWithBaro(kalmanCoreData_t *this, const kalmanCoreParams_t *
  *
  * The filter progresses as:
  *  - Predicting the current state forward */
-void kalmanCorePredict(kalmanCoreData_t *this, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying);
+void kalmanCorePredict(kalmanCoreData_t *this, Axis3f *acc, Axis3f *gyro, const uint32_t nowMs, bool quadIsFlying);
 
-void kalmanCoreAddProcessNoise(kalmanCoreData_t *this, const kalmanCoreParams_t *params, float dt);
+void kalmanCoreAddProcessNoise(kalmanCoreData_t *this, const kalmanCoreParams_t *params, const uint32_t nowMs);
 
-/*  - Finalization to incorporate attitude error into body attitude */
-void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick);
+/**
+ * @brief Finalization to incorporate attitude error into body attitude
+ *
+ * @param this Core data
+ * @return true The state was finalized
+ * @return false The state was not changed and did not require finalization
+ */
+bool kalmanCoreFinalize(kalmanCoreData_t* this);
 
 /*  - Externalization to move the filter's internal state into the external state expected by other modules */
-void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, const Axis3f *acc, uint32_t tick);
+void kalmanCoreExternalizeState(const kalmanCoreData_t* this, state_t *state, const Axis3f *acc);
 
 void kalmanCoreDecoupleXY(kalmanCoreData_t* this);
 

src/modules/interface/kalman_core/mm_sweep_angles.h

@@ -29,4 +29,4 @@
 #include "outlierFilter.h"
 
 // Measurement of sweep angles from a Lighthouse base station
-void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t tick, OutlierFilterLhState_t* sweepOutlierFilterState);
+void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t nowMs, OutlierFilterLhState_t* sweepOutlierFilterState);

src/modules/interface/outlierFilter.h

@@ -31,11 +31,11 @@ bool outlierFilterValidateTdoaSimple(const tdoaMeasurement_t* tdoa);
 bool outlierFilterValidateTdoaSteps(const tdoaMeasurement_t* tdoa, const float error, const vector_t* jacobian, const point_t* estPos);
 
 typedef struct {
-    uint32_t openingTime;
-    int32_t openingWindow;
+    uint32_t openingTimeMs;
+    int32_t openingWindowMs;
 } 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);
+bool outlierFilterValidateLighthouseSweep(OutlierFilterLhState_t* this, const float distanceToBs, const float angleError, const uint32_t nowMs);
+void outlierFilterReset(OutlierFilterLhState_t* this, const uint32_t nowMs);
 
 
 #endif // __OUTLIER_FILTER_H__

src/modules/interface/stabilizer_types.h

@@ -65,8 +65,6 @@ typedef struct vec3_s acc_t;
 
 /* Orientation as a quaternion */
 typedef struct quaternion_s {
-  uint32_t timestamp;
-
   union {
     struct {
       float q0;

src/modules/src/Kbuild

@@ -27,6 +27,7 @@ obj-y += eventtrigger.o
 obj-y += extrx.o
 obj-y += health.o
 obj-$(CONFIG_ESTIMATOR_KALMAN_ENABLE) += kalman_supervisor.o
+obj-y += axis3fSubSampler.o
 obj-y += log.o
 obj-y += mem.o
 obj-y += msp.o

src/modules/src/axis3fSubSampler.c

@@ -0,0 +1,54 @@
+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2023 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
+ * the Free Software Foundation, in version 3.
+ *
+ * This program 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 <string.h>
+#include "axis3fSubSampler.h"
+
+void axis3fSubSamplerInit(Axis3fSubSampler_t* this, const float conversionFactor) {
+  memset(this, 0, sizeof(Axis3fSubSampler_t));
+  this->conversionFactor = conversionFactor;
+}
+
+void axis3fSubSamplerAccumulate(Axis3fSubSampler_t* this, const Axis3f* sample) {
+  this->sum.x += sample->x;
+  this->sum.y += sample->y;
+  this->sum.z += sample->z;
+
+  this->count++;
+}
+
+Axis3f* axis3fSubSamplerFinalize(Axis3fSubSampler_t* this) {
+  if (this->count > 0) {
+    this->subSample.x = this->sum.x * this->conversionFactor / this->count;
+    this->subSample.y = this->sum.y * this->conversionFactor / this->count;
+    this->subSample.z = this->sum.z * this->conversionFactor / this->count;
+
+    // Reset
+    this->count = 0;
+    this->sum = (Axis3f){.axis={0}};
+  }
+
+  return &this->subSample;
+}