#570 Added calibration correction for LH1 and LH2

* Calibration model for LH1 based on NASA AstroBee model
* Calibration model for LH2 not complete but good enough
* Extracted some math utils from kalman_core into cf_math.h

src/modules/interface/lighthouse/lighthouse_position_est.h

@@ -40,4 +40,4 @@ extern baseStationGeometry_t lighthouseBaseStationsGeometry[PULSE_PROCESSOR_N_BA
 void lightHousePositionGeometryDataUpdated();
 
 void lighthousePositionEstimatePoseCrossingBeams(pulseProcessorResult_t* angles, int baseStation);
-void lighthousePositionEstimatePoseSweeps(pulseProcessorResult_t* angles, int baseStation);
+void lighthousePositionEstimatePoseSweeps(pulseProcessorResult_t* angles, int baseStation, const lighthouseCalibration_t* bsCalib);

src/modules/interface/stabilizer_types.h

@@ -29,6 +29,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "imu_types.h"
+#include "pulse_processor.h"
 
 /* Data structure used by the stabilizer subsystem.
  * All have a timestamp to be set when the data is calculated.
@@ -251,9 +252,11 @@ typedef struct {
   vec3d* rotorPos;           // Pos of rotor origin in global reference frame
   mat3d* rotorRot;           // Rotor rotation matrix
   mat3d* rotorRotInv;        // Inverted rotor rotation matrix
-  float tan_t;               // t is the tilt angle of the light plane on the rotor
+  float t;                   // t is the tilt angle of the light plane on the rotor
   float measuredSweepAngle;
   float stdDev;
+  lighthouseBaseStationType_t baseStationType;
+  const lighthouseCalibrationAxis_t* calib;
 } sweepAngleMeasurement_t;
 
 // Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number.

src/modules/src/estimator_kalman.c

@@ -257,19 +257,6 @@ static const bool useBaroUpdate = true;
 static const bool useBaroUpdate = false;
 #endif
 
-/**
- * Supporting and utility functions
- */
-
-static inline void mat_trans(const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst)
-  { configASSERT(ARM_MATH_SUCCESS == arm_mat_trans_f32(pSrc, pDst)); }
-static inline void mat_inv(const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst)
-  { configASSERT(ARM_MATH_SUCCESS == arm_mat_inverse_f32(pSrc, pDst)); }
-static inline void mat_mult(const arm_matrix_instance_f32 * pSrcA, const arm_matrix_instance_f32 * pSrcB, arm_matrix_instance_f32 * pDst)
-  { configASSERT(ARM_MATH_SUCCESS == arm_mat_mult_f32(pSrcA, pSrcB, pDst)); }
-static inline float arm_sqrt(float32_t in)
-  { float pOut = 0; arm_status result = arm_sqrt_f32(in, &pOut); configASSERT(ARM_MATH_SUCCESS == result); return pOut; }
-
 static void kalmanTask(void* parameters);
 static bool predictStateForward(uint32_t osTick, float dt);
 static bool updateQueuedMeasurments(const Axis3f *gyro, const uint32_t tick);

src/modules/src/kalman_core.c

@@ -67,6 +67,8 @@
 #include "debug.h"
 #include "static_mem.h"
 
+#include "lighthouse_calibration.h"
+
 // #define DEBUG_STATE_CHECK
 
 // the reversion of pitch and roll to zero
@@ -81,16 +83,6 @@
  * Supporting and utility functions
  */
 
-static inline void mat_trans(const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst)
-{ ASSERT(ARM_MATH_SUCCESS == arm_mat_trans_f32(pSrc, pDst)); }
-static inline void mat_inv(const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst)
-{ ASSERT(ARM_MATH_SUCCESS == arm_mat_inverse_f32(pSrc, pDst)); }
-static inline void mat_mult(const arm_matrix_instance_f32 * pSrcA, const arm_matrix_instance_f32 * pSrcB, arm_matrix_instance_f32 * pDst)
-{ ASSERT(ARM_MATH_SUCCESS == arm_mat_mult_f32(pSrcA, pSrcB, pDst)); }
-static inline float arm_sqrt(float32_t in)
-{ float pOut = 0; arm_status result = arm_sqrt_f32(in, &pOut); ASSERT(ARM_MATH_SUCCESS == result); return pOut; }
-
-
 #ifdef DEBUG_STATE_CHECK
 static void assertStateNotNaN(const kalmanCoreData_t* this) {
   if ((isnan(this->S[KC_STATE_X])) ||
@@ -571,54 +563,60 @@ void kalmanCoreUpdateWithYawError(kalmanCoreData_t *this, yawErrorMeasurement_t
     scalarUpdate(this, &H, this->S[KC_STATE_D2] - error->yawError, error->stdDev);
 }
 
-void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t tick) {
+void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *sweepInfo, const uint32_t tick) {
   // Rotate the sensor position from CF reference frame to global reference frame,
   // using the CF roatation matrix
   vec3d s;
   arm_matrix_instance_f32 Rcf_ = {3, 3, (float32_t *)this->R};
-  arm_matrix_instance_f32 scf_ = {3, 1, *angles->sensorPos};
+  arm_matrix_instance_f32 scf_ = {3, 1, *sweepInfo->sensorPos};
   arm_matrix_instance_f32 s_ = {3, 1, s};
   mat_mult(&Rcf_, &scf_, &s_);
 
   // Get the current state values of the position of the crazyflie (global reference frame) and add the relative sensor pos
   vec3d pcf = {this->S[KC_STATE_X] + s[0], this->S[KC_STATE_Y] + s[1], this->S[KC_STATE_Z] + s[2]};
 
   // Calculate the difference between the rotor and the sensor on the CF (global reference frame)
-  const vec3d* pr = angles->rotorPos;
+  const vec3d* pr = sweepInfo->rotorPos;
   vec3d stmp = {pcf[0] - (*pr)[0], pcf[1] - (*pr)[1], pcf[2] - (*pr)[2]};
   arm_matrix_instance_f32 stmp_ = {3, 1, stmp};
 
   // Rotate the difference in position to the rotor reference frame,
   // using the rotor inverse rotation matrix
   vec3d sr;
-  arm_matrix_instance_f32 Rr_inv_ = {3, 3, (float32_t *)(*angles->rotorRotInv)};
+  arm_matrix_instance_f32 Rr_inv_ = {3, 3, (float32_t *)(*sweepInfo->rotorRotInv)};
   arm_matrix_instance_f32 sr_ = {3, 1, sr};
   mat_mult(&Rr_inv_, &stmp_, &sr_);
 
   // The following computations are in the rotor refernece frame
   const float x = sr[0];
   const float y = sr[1];
   const float z = sr[2];
-  const float tan_t = angles->tan_t;
+  const float t = sweepInfo->t;
+  const float tan_t = tanf(t);
 
   const float r2 = x * x + y * y;
   const float r = arm_sqrt(r2);
   const float z_tan_t = z * tan_t;
 
-  const float predictedSweepAngle = atan2(y, x) + asin(z_tan_t / r);
-  const float measuredSweepAngle = angles->measuredSweepAngle;
+  float predictedSweepAngle = 0.0f;
+  if (sweepInfo->baseStationType == lighthouseBsTypeV1) {
+    predictedSweepAngle = lighthouseCalibrationMeasurementModelLh1(x, y, z, sweepInfo->calib);
+  } else {
+    predictedSweepAngle = lighthouseCalibrationMeasurementModelLh2(x, y, z, t, sweepInfo->calib);
+  }
+  const float measuredSweepAngle = sweepInfo->measuredSweepAngle;
   const float error = measuredSweepAngle - predictedSweepAngle;
 
   if (outlierFilterValidateLighthouseSweep(&sweepOutlierFilterState, r, error, tick)) {
     // Calculate H vector (in the rotor reference frame)
-    const float q = tan_t / arm_sqrt(r2 - z_tan_t * z_tan_t);
+    const float q = tan_t / arm_sqrt(limPos(r2 - z_tan_t * z_tan_t));
     vec3d gr = {(-y - x * z * q) / r2, (x - y * z * q) / r2 , q};
 
     // gr is in the rotor reference frame, rotate back to the global
     // reference frame using the rotor rotation matrix
     vec3d g;
     arm_matrix_instance_f32 gr_ = {3, 1, gr};
-    arm_matrix_instance_f32 Rr_ = {3, 3, (float32_t *)(*angles->rotorRot)};
+    arm_matrix_instance_f32 Rr_ = {3, 3, (float32_t *)(*sweepInfo->rotorRot)};
     arm_matrix_instance_f32 g_ = {3, 1, g};
     mat_mult(&Rr_, &gr_, &g_);
 
@@ -628,7 +626,7 @@ void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasureme
     h[KC_STATE_Z] = g[2];
 
     arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
-    scalarUpdate(this, &H, error, angles->stdDev);
+    scalarUpdate(this, &H, error, sweepInfo->stdDev);
   }
 }
 

src/modules/src/lighthouse/lighthouse_core.c

@@ -165,7 +165,7 @@ static void usePulseResultSweeps(pulseProcessor_t *appState, pulseProcessorResul
 
   pulseProcessorClearOutdated(appState, angles, basestation);
 
-  lighthousePositionEstimatePoseSweeps(angles, basestation);
+  lighthousePositionEstimatePoseSweeps(angles, basestation, &appState->bsCalibration[basestation]);
 
   pulseProcessorProcessed(angles, basestation);
 }
@@ -177,7 +177,7 @@ static void convertV2AnglesToV1Angles(pulseProcessorResult_t* angles) {
       pulseProcessorBaseStationMeasuremnt_t* to = &angles->sensorMeasurementsLh1[sensor].baseStatonMeasurements[bs];
 
       if (2 == from->validCount) {
-        pulseProcessorV2ConvertToV1Angles(from->angles[0], from->angles[1], to->angles);
+        pulseProcessorV2ConvertToV1Angles(from->correctedAngles[0], from->correctedAngles[1], to->correctedAngles);
         to->validCount = from->validCount;
       } else {
         to->validCount = 0;
@@ -188,11 +188,11 @@ static void convertV2AnglesToV1Angles(pulseProcessorResult_t* angles) {
 
 static void usePulseResult(pulseProcessor_t *appState, pulseProcessorResult_t* angles, int basestation, int axis) {
   if (axis == sweepDirection_y) {
+    pulseProcessorApplyCalibration(appState, angles, basestation);
     if (lighthouseBsTypeV2 == angles->measurementType) {
       // Emulate V1 base stations for now, convert to V1 angles
       convertV2AnglesToV1Angles(angles);
     }
-    pulseProcessorApplyCalibration(appState, angles, basestation);
 
     switch(estimationMethod) {
       case 0:
@@ -370,6 +370,11 @@ LOG_ADD(LOG_FLOAT, rawAngle0ylh2, &angles.sensorMeasurementsLh2[0].baseStatonMea
 LOG_ADD(LOG_FLOAT, rawAngle1xlh2, &angles.sensorMeasurementsLh2[0].baseStatonMeasurements[1].angles[0])
 LOG_ADD(LOG_FLOAT, rawAngle1ylh2, &angles.sensorMeasurementsLh2[0].baseStatonMeasurements[1].angles[1])
 
+LOG_ADD(LOG_FLOAT, angle0x_0lh2, &angles.sensorMeasurementsLh2[0].baseStatonMeasurements[0].correctedAngles[0])
+LOG_ADD(LOG_FLOAT, angle0y_0lh2, &angles.sensorMeasurementsLh2[0].baseStatonMeasurements[0].correctedAngles[1])
+LOG_ADD(LOG_FLOAT, angle1x_0lh2, &angles.sensorMeasurementsLh2[0].baseStatonMeasurements[1].correctedAngles[0])
+LOG_ADD(LOG_FLOAT, angle1y_0lh2, &angles.sensorMeasurementsLh2[0].baseStatonMeasurements[1].correctedAngles[1])
+
 STATS_CNT_RATE_LOG_ADD(serRt, &serialFrameRate)
 STATS_CNT_RATE_LOG_ADD(frmRt, &frameRate)
 STATS_CNT_RATE_LOG_ADD(cycleRt, &cycleRate)

src/modules/src/lighthouse/lighthouse_position_est.c

@@ -37,8 +37,13 @@
 #include "lighthouse_position_est.h"
 
 baseStationGeometry_t lighthouseBaseStationsGeometry[PULSE_PROCESSOR_N_BASE_STATIONS]  = {
+// Arena LH1
 {.origin = {-1.958483,  0.542299,  3.152727, }, .mat = {{0.79721498, -0.004274, 0.60368103, }, {0.0, 0.99997503, 0.00708, }, {-0.60369599, -0.005645, 0.79719502, }, }},
 {.origin = {1.062398, -2.563488,  3.112367, }, .mat = {{0.018067, -0.999336, 0.031647, }, {0.76125097, 0.034269, 0.64755201, }, {-0.648206, 0.012392, 0.76136398, }, }},
+
+// Arena LH2
+// {.origin = {-2.057947, 0.398319, 3.109704, }, .mat = {{0.807210, 0.002766, 0.590258, }, {0.067095, 0.993078, -0.096409, }, {-0.586439, 0.117426, 0.801437, }, }},
+// {.origin = {0.866244, -2.566829, 3.132632, }, .mat = {{-0.043296, -0.997675, -0.052627, }, {0.766284, -0.066962, 0.639003, }, {-0.641042, -0.012661, 0.767401, }, }},
 };
 
 
@@ -51,11 +56,13 @@ static statsCntRateLogger_t* bsEstRates[PULSE_PROCESSOR_N_BASE_STATIONS] = {&est
 
 baseStationEulerAngles_t lighthouseBaseStationAngles[PULSE_PROCESSOR_N_BASE_STATIONS];
 
+// The light planes in LH2 are tilted +- 30 degrees
+static const float t30 = M_PI / 6;
+
 // Pre calculated data used in state updates
 static mat3d baseStationInvertedRotationMatrixes[PULSE_PROCESSOR_N_BASE_STATIONS];
 static mat3d lh1Rotor2RotationMatrixes[PULSE_PROCESSOR_N_BASE_STATIONS];
 static mat3d lh1Rotor2InvertedRotationMatrixes[PULSE_PROCESSOR_N_BASE_STATIONS];
-static float tan_t_30 = 0.0f;
 
 static void preProcessGeometryData(mat3d bsRot, mat3d bsRotInverted, mat3d lh1Rotor2Rot, mat3d lh1Rotor2RotInverted);
 
@@ -84,10 +91,6 @@ static void preProcessGeometryData(mat3d bsRot, mat3d bsRotInverted, mat3d lh1Ro
 
   arm_matrix_instance_f32 lh1Rotor2RotInverted_ = {3, 3, (float32_t *)lh1Rotor2RotInverted};
   arm_mat_trans_f32(&lh1Rotor2Rot_, &lh1Rotor2RotInverted_);
-
-  // The light planes in LH2 are tilted +- 30 degrees
-  const float t = M_PI / 6;
-  tan_t_30 = tanf(t);
 }
 
 
@@ -144,30 +147,33 @@ static void estimatePositionCrossingBeams(pulseProcessorResult_t* angles, int ba
   }
 }
 
-static void estimatePositionSweepsLh1(pulseProcessorResult_t* angles, int baseStation) {
+static void estimatePositionSweepsLh1(pulseProcessorResult_t* angles, int baseStation, const lighthouseCalibration_t* bsCalib) {
   sweepAngleMeasurement_t sweepInfo;
   sweepInfo.stdDev = sweepStd;
   sweepInfo.rotorPos = &lighthouseBaseStationsGeometry[baseStation].origin;
-  sweepInfo.tan_t = 0;
+  sweepInfo.t = 0;
+  sweepInfo.baseStationType = lighthouseBsTypeV1;
 
   for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
     pulseProcessorBaseStationMeasuremnt_t* bsMeasurement = &angles->sensorMeasurementsLh1[sensor].baseStatonMeasurements[baseStation];
     if (bsMeasurement->validCount == PULSE_PROCESSOR_N_SWEEPS) {
       sweepInfo.sensorPos = &sensorDeckPositions[sensor];
 
-      sweepInfo.measuredSweepAngle = bsMeasurement->correctedAngles[0];
+      sweepInfo.measuredSweepAngle = bsMeasurement->angles[0];
       if (sweepInfo.measuredSweepAngle != 0) {
         sweepInfo.rotorRot = &lighthouseBaseStationsGeometry[baseStation].mat;
         sweepInfo.rotorRotInv = &baseStationInvertedRotationMatrixes[baseStation];
+        sweepInfo.calib = &bsCalib->axis[0];
 
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
       }
 
-      sweepInfo.measuredSweepAngle = bsMeasurement->correctedAngles[1];
+      sweepInfo.measuredSweepAngle = bsMeasurement->angles[1];
       if (sweepInfo.measuredSweepAngle != 0) {
         sweepInfo.rotorRot = &lh1Rotor2RotationMatrixes[baseStation];
         sweepInfo.rotorRotInv = &lh1Rotor2InvertedRotationMatrixes[baseStation];
+        sweepInfo.calib = &bsCalib->axis[1];
 
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
@@ -176,12 +182,13 @@ static void estimatePositionSweepsLh1(pulseProcessorResult_t* angles, int baseSt
   }
 }
 
-static void estimatePositionSweepsLh2(pulseProcessorResult_t* angles, int baseStation) {
+static void estimatePositionSweepsLh2(pulseProcessorResult_t* angles, int baseStation, const lighthouseCalibration_t* bsCalib) {
   sweepAngleMeasurement_t sweepInfo;
   sweepInfo.stdDev = sweepStdLh2;
   sweepInfo.rotorPos = &lighthouseBaseStationsGeometry[baseStation].origin;
   sweepInfo.rotorRot = &lighthouseBaseStationsGeometry[baseStation].mat;
   sweepInfo.rotorRotInv = &baseStationInvertedRotationMatrixes[baseStation];
+  sweepInfo.baseStationType = lighthouseBsTypeV2;
 
   for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) {
     pulseProcessorBaseStationMeasuremnt_t* bsMeasurement = &angles->sensorMeasurementsLh2[sensor].baseStatonMeasurements[baseStation];
@@ -190,28 +197,30 @@ static void estimatePositionSweepsLh2(pulseProcessorResult_t* angles, int baseSt
 
       sweepInfo.measuredSweepAngle = bsMeasurement->angles[0];
       if (sweepInfo.measuredSweepAngle != 0) {
-        sweepInfo.tan_t = -tan_t_30;
+        sweepInfo.t = -t30;
+        sweepInfo.calib = &bsCalib->axis[0];
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
       }
 
       sweepInfo.measuredSweepAngle = bsMeasurement->angles[1];
       if (sweepInfo.measuredSweepAngle != 0) {
-        sweepInfo.tan_t = tan_t_30;
+        sweepInfo.t = t30;
+        sweepInfo.calib = &bsCalib->axis[1];
         estimatorEnqueueSweepAngles(&sweepInfo);
         STATS_CNT_RATE_EVENT(bsEstRates[baseStation]);
       }
     }
   }
 }
 
-static void estimatePositionSweeps(pulseProcessorResult_t* angles, int baseStation) {
+static void estimatePositionSweeps(pulseProcessorResult_t* angles, int baseStation, const lighthouseCalibration_t* bsCalib) {
   switch(angles->measurementType) {
     case lighthouseBsTypeV1:
-      estimatePositionSweepsLh1(angles, baseStation);
+      estimatePositionSweepsLh1(angles, baseStation, bsCalib);
       break;
     case lighthouseBsTypeV2:
-      estimatePositionSweepsLh2(angles, baseStation);
+      estimatePositionSweepsLh2(angles, baseStation, bsCalib);
       break;
     default:
       // Do nothing
@@ -294,11 +303,12 @@ void lighthousePositionEstimatePoseCrossingBeams(pulseProcessorResult_t* angles,
   estimateYaw(angles, baseStation);
 }
 
-void lighthousePositionEstimatePoseSweeps(pulseProcessorResult_t* angles, int baseStation) {
-  estimatePositionSweeps(angles, baseStation);
+void lighthousePositionEstimatePoseSweeps(pulseProcessorResult_t* angles, int baseStation, const lighthouseCalibration_t* bsCalib) {
+  estimatePositionSweeps(angles, baseStation, bsCalib);
   estimateYaw(angles, baseStation);
 }
 
+
 LOG_GROUP_START(lighthouse)
 STATS_CNT_RATE_LOG_ADD(posRt, &positionRate)
 STATS_CNT_RATE_LOG_ADD(estBs0Rt, &estBs0Rate)

src/utils/interface/cf_math.h

@@ -37,8 +37,51 @@
 #include "arm_math.h"
 #pragma GCC diagnostic pop
 
+#include "arm_math.h"
+#include "cfassert.h"
+
+
 #define DEG_TO_RAD (PI/180.0f)
 #define RAD_TO_DEG (180.0f/PI)
 
 #define MIN(a, b) ((b) < (a) ? (b) : (a))
 #define MAX(a, b) ((b) > (a) ? (b) : (a))
+
+static inline void mat_trans(const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst) {
+    ASSERT(ARM_MATH_SUCCESS == arm_mat_trans_f32(pSrc, pDst));
+}
+
+static inline void mat_inv(const arm_matrix_instance_f32 * pSrc, arm_matrix_instance_f32 * pDst) {
+    ASSERT(ARM_MATH_SUCCESS == arm_mat_inverse_f32(pSrc, pDst));
+}
+
+static inline void mat_mult(const arm_matrix_instance_f32 * pSrcA, const arm_matrix_instance_f32 * pSrcB, arm_matrix_instance_f32 * pDst) {
+    ASSERT(ARM_MATH_SUCCESS == arm_mat_mult_f32(pSrcA, pSrcB, pDst));
+}
+
+static inline float arm_sqrt(float32_t in) {
+    float pOut = 0;
+    arm_status result = arm_sqrt_f32(in, &pOut);
+    ASSERT(ARM_MATH_SUCCESS == result);
+    return pOut;
+}
+
+static inline float limPos(float in) {
+  if (in < 0.0f) {
+    return 0.0f;
+  }
+
+  return in;
+}
+
+static inline float clip1(float a) {
+  if (a < -1.0f) {
+    return -1.0f;
+  }
+
+  if (a > 1.0f) {
+    return 1.0f;
+  }
+
+  return a;
+}