Kalman instable when pushing distance difference

[ataffanel]

Since #163 we push a distance difference in the Kalman filter. To debug TDoA position estimation I have been pushing the anchor distance difference calculate using two way ranging. The idea is that the position estimate is pretty good and robust with the two way ranging data, so pushing the same data but as distance difference should work well enough.

However we see the same instability when pushing TWR range difference as when trying the filter with real TDoA data which seems to indicate that there is a bug in the way the Kalman filter is accepting ranging difference.

For reference this is the change to the two way ranging tag code that allows to push distance difference in the kalman filter

diff --git a/src/deck/drivers/src/lpsTwrTag.c b/src/deck/drivers/src/lpsTwrTag.c
index 2ff039f..d99436a 100644
--- a/src/deck/drivers/src/lpsTwrTag.c
+++ b/src/deck/drivers/src/lpsTwrTag.c
@@ -169,13 +169,29 @@ static uint32_t rxcallback(dwDevice_t *dev) {
       rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = options->distance[current_anchor];
 
       if (options->anchorPositionOk && (diff < (OUTLIER_TH*stddev))) {
-        distanceMeasurement_t dist;
-        dist.distance = options->distance[current_anchor];
-        dist.x = options->anchorPosition[current_anchor].x;
-        dist.y = options->anchorPosition[current_anchor].y;
-        dist.z = options->anchorPosition[current_anchor].z;
-        dist.stdDev = 0.25;
-        stateEstimatorEnqueueDistance(&dist);
+        // distanceMeasurement_t dist;
+        // dist.distance = options->distance[current_anchor];
+        // dist.x = options->anchorPosition[current_anchor].x;
+        // dist.y = options->anchorPosition[current_anchor].y;
+        // dist.z = options->anchorPosition[current_anchor].z;
+        // dist.stdDev = 0.25;
+        // stateEstimatorEnqueueDistance(&dist);
+
+
+        static int previousAnchor = -1;
+
+        if (previousAnchor != -1) {
+          tdoaMeasurement_t measurement;
+
+          measurement.anchorPosition[0] = options->anchorPosition[previousAnchor];
+          measurement.anchorPosition[1] = options->anchorPosition[current_anchor];
+          measurement.distanceDiff = options->distance[current_anchor] - options->distance[previousAnchor];
+          measurement.timeBetweenMeasurements = 2e-3;
+          measurement.stdDev = 0.5;
+
+          stateEstimatorEnqueueTDOA(&measurement);
+        }
+        previousAnchor = current_anchor;
       }
 #endif

[ataffanel]

With the commit 4f0594f it now works really well flying with TWR measured distance difference. The Kalman state update of TDoA is now very similar to the distance state update.

@mikehamer It seems to make sense to let the TDoA receiver decide of the stdDev. If so, it is not needed to have timeBetweenMeasurements in the tdoa data structure anymore. Any thought?

[mikehamer]

It is okay to remove the time between measurements. This was initially to add numerical stability, which I now realise is required due to a subtle bug in the way that the KF handles multiple measurement updates during one update step. I need to fix this at some point, however, it only affects things slightly, and only if the Kalman Filter is processing multiple measurement updates per step. Since you're running TDoA at the same rate as the KF, it won't affect you.