A quick and dirty test of passing difference of distance to the kalma…

…n estimator as opposed to absolute rx/tx times for messages.

This works in the Bitcraze lab! There are instabilities the makes the kalman estimator go loco from time to time, but when stable it delivers a reasonable position.
The advantage of this approach is that is simplifies the code both in the kalman estimator as well as in lpsTdoaTag.c.

This is a spike. It is ugly and unit tests are not updated.

src/deck/drivers/src/lpsTdoaTag.c

@@ -79,29 +79,25 @@ static uint64_t truncateToTimeStamp(uint64_t fullTimeStamp) {
   return fullTimeStamp & 0x00FFFFFFFFFFul;
 }
 
-static int64_t eliminateClockWrap(clockWrap_t* data, int64_t time) {
-  if ((time < data->latestTime)) {
-    data->offset += 0x10000000000;
-  }
-
-  data->latestTime = time;
-
-  return time + data->offset;
-}
+static int64_t previuosTime;
 
-static void enqueueTDOA(uint8_t anchor, int64_t rxAn_by_T_in_cl_T, int64_t txAn_in_cl_M) {
+static void enqueueTDOA(uint8_t anchor, double distDiff, int64_t rxTime) {
   tdoaMeasurement_t tdoa = {.stdDev = MEASUREMENT_NOISE_STD};
 
-  memcpy(&(tdoa.measurement[0]), &lastTOA, sizeof(toaMeasurement_t));
+  tdoa.distDiff = distDiff;
+  tdoa.measurement[1].rx = rxTime - previuosTime;
+  previuosTime = rxTime;
+
+  tdoa.measurement[0].senderId = 0;
+  tdoa.measurement[0].x = options->anchorPosition[0].x;
+  tdoa.measurement[0].y = options->anchorPosition[0].y;
+  tdoa.measurement[0].z = options->anchorPosition[0].z;
 
   tdoa.measurement[1].senderId = anchor;
-  tdoa.measurement[1].rx = eliminateClockWrap(&clockWrapTag, rxAn_by_T_in_cl_T);
-  tdoa.measurement[1].tx = eliminateClockWrap(&clockWrapMaster, txAn_in_cl_M);
   tdoa.measurement[1].x = options->anchorPosition[anchor].x;
   tdoa.measurement[1].y = options->anchorPosition[anchor].y;
   tdoa.measurement[1].z = options->anchorPosition[anchor].z;
 
-  memcpy(&lastTOA, &tdoa.measurement[1], sizeof(toaMeasurement_t));
 #ifdef ESTIMATOR_TYPE_kalman
   stateEstimatorEnqueueTDOA(&tdoa);
 #endif
@@ -138,7 +134,6 @@ static void rxcallback(dwDevice_t *dev) {
         clockCorrection_T_To_M = frameTime_in_cl_M / frameTime_in_T;
       }
 
-      enqueueTDOA(MASTER, rxAn_by_T_in_cl_T, txM_in_cl_M);
     } else {
       int64_t previous_txAn_in_cl_An = timestampToUint64(rxPacketBuffer[anchor].timestamps[anchor]);
       int64_t rxAn_by_M_in_cl_M = timestampToUint64(rxPacketBuffer[MASTER].timestamps[anchor]);
@@ -159,14 +154,13 @@ static void rxcallback(dwDevice_t *dev) {
       int64_t tof_M_to_An_in_cl_M = (((truncateToTimeStamp(rxM_by_An_in_cl_An - previous_txAn_in_cl_An) * clockCorrection_An_To_M) - truncateToTimeStamp(txM_in_cl_M - rxAn_by_M_in_cl_M))) / 2.0;
       int64_t delta_txM_to_txAn_in_cl_M = (tof_M_to_An_in_cl_M + truncateToTimeStamp(txAn_in_cl_An - rxM_by_An_in_cl_An) * clockCorrection_An_To_M);
       int64_t timeDiffOfArrival_in_cl_M =  truncateToTimeStamp(rxAn_by_T_in_cl_T - rxM_by_T_in_cl_T) * clockCorrection_T_To_M - delta_txM_to_txAn_in_cl_M;
-      int64_t txAn_in_cl_M = txM_in_cl_M + delta_txM_to_txAn_in_cl_M;
 
       float tdoaDistDiff = SPEED_OF_LIGHT * timeDiffOfArrival_in_cl_M / LOCODECK_TS_FREQ;
 
       // Sanity check distances in case of missed packages
       if (tdoaDistDiff > -MAX_DISTANCE_DIFF && tdoaDistDiff < MAX_DISTANCE_DIFF) {
         uwbTdoaDistDiff[anchor] = tdoaDistDiff;
-        enqueueTDOA(anchor, rxAn_by_T_in_cl_T, txAn_in_cl_M);
+        enqueueTDOA(anchor, tdoaDistDiff, rxAn_by_T_in_cl_T);
       }
     }
 
@@ -239,4 +233,5 @@ LOG_ADD(LOG_FLOAT, d04, &uwbTdoaDistDiff[4])
 LOG_ADD(LOG_FLOAT, d05, &uwbTdoaDistDiff[5])
 LOG_ADD(LOG_FLOAT, d06, &uwbTdoaDistDiff[6])
 LOG_ADD(LOG_FLOAT, d07, &uwbTdoaDistDiff[7])
+
 LOG_GROUP_STOP(tdoa)

src/modules/interface/stabilizer_types.h

@@ -84,6 +84,8 @@ typedef struct toaMeasurement_s {
 
 typedef struct tdoaMeasurement_s {
   toaMeasurement_t measurement[2];
+  double distDiff;
+  float timeDiff;
   float stdDev;
 } tdoaMeasurement_t;
 

src/modules/src/estimator_kalman.c

@@ -878,21 +878,13 @@ static void stateEstimatorUpdateWithTDOA(tdoaMeasurement_t *tdoa)
    * dR = dT + dT*skew + d1 - d0
    */
 
-  // We cannot accurately position, until we have an accurate knowledge of skew.
-  // We assume that skew is uncorrelated with other quad states, we can therefore track it
-  // using a separate, 1D Kalman filter. Here, we predict the skew forward
-  float dt = (float)(tdoa->measurement[1].rx - tdoa->measurement[0].rx)*SECONDS_PER_TDOATICK;
+  float dt = tdoa->timeDiff * SECONDS_PER_TDOATICK;
   if (dt > 0)
   {
-    // We assume skew stays constant, but increase the variance
-    varSkew += powf(procNoiseSkew * dt, 2); // compute variance from standard deviation
     stateEstimatorAddProcessNoise(dt); // add process noise occurring between packet receptions
   }
 
-  // calculate the TDOA measurement
-  int64_t dR = tdoa->measurement[1].rx - tdoa->measurement[0].rx;
-  int64_t dT = tdoa->measurement[1].tx - tdoa->measurement[0].tx;
-  float measurement = METERS_PER_TDOATICK*(dR - dT); // possible to lose precision here in floats
+  float measurement = tdoa->distDiff;
 
   // predict based on current state
   float x = S[STATE_X];
@@ -905,13 +897,9 @@ static void stateEstimatorUpdateWithTDOA(tdoaMeasurement_t *tdoa)
   float d1 = sqrtf(powf(x - x1, 2) + powf(y - y1, 2) + powf(z - z1, 2));
   float d0 = sqrtf(powf(x - x0, 2) + powf(y - y0, 2) + powf(z - z0, 2));
 
-  float predicted = METERS_PER_TDOATICK * dT * stateSkew + (d1 - d0);
+  float predicted = (d1 - d0);
   float error = measurement - predicted;
-  float varUwb = tdoa->stdDev*tdoa->stdDev*dt*dt;
-  float Hs = METERS_PER_TDOATICK * dT; // derivative of prediction equation with respect to stateSkew
-  float Ks = varSkew * Hs / (Hs * varSkew * Hs + varUwb);
-  stateSkew = (stateSkew + Ks * error);
-  varSkew = ((1 - Ks * Hs) * varSkew * (1 - Ks * Hs) + Ks * varUwb * Ks);
+  float varUwb = tdoa->stdDev * tdoa->stdDev * dt * dt;
 
   if (tdoaCount >= 100)
   {

test/deck/drivers/src/TestLpsTdoaTag.c

@@ -111,9 +111,8 @@ void testDifferenceOfDistanceWithNoClockDrift() {
 
 
 void testKalmanEstimatorWithNoClockDrift() {
-  #ifndef ESTIMATOR_TYPE_kalman
   TEST_IGNORE();
-  #endif
+
   // Fixture
   // Two anchors (A0 and A1), separated by 1.0m
   // Distance from A0 to tag is 2.0m
@@ -202,9 +201,8 @@ void testDifferenceOfDistanceWithNoClockDriftWithTagClockWrapping() {
 }
 
 void testKalmanEstimatorWithNoClockDriftWithTagClockWrapping() {
-  #ifndef ESTIMATOR_TYPE_kalman
   TEST_IGNORE();
-  #endif
+
   // Fixture
   // Two anchors (A0 and A1), separated by 1.0m
   // Distance from A0 to tag is 2.0m
@@ -250,9 +248,8 @@ void testKalmanEstimatorWithNoClockDriftWithTagClockWrapping() {
 }
 
 void testKalmanEstimatorWithNoClockDriftWithA0ClockWrapping() {
-  #ifndef ESTIMATOR_TYPE_kalman
   TEST_IGNORE();
-  #endif
+
   // Fixture
   // Two anchors (A0 and A1), separated by 1.0m
   // Distance from A0 to tag is 2.0m
@@ -298,9 +295,8 @@ void testKalmanEstimatorWithNoClockDriftWithA0ClockWrapping() {
 }
 
 void testKalmanEstimatorWithNoClockDriftWithA1ClockWrapping() {
-  #ifndef ESTIMATOR_TYPE_kalman
   TEST_IGNORE();
-  #endif
+
   // Fixture
   // Two anchors (A0 and A1), separated by 1.0m
   // Distance from A0 to tag is 2.0m