From 0fa99709d929361629559614fb5e4838d03acb61 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Wed, 30 Oct 2024 14:25:16 +0100 Subject: [PATCH] Add initial terrain jump handling in EKF ToF measurement model This commit introduces tracking of elevation and predicted terrain height in the EKF ToF measurement model. If a significant difference between the predicted elevation and measured elevation is detected (i.e., greater than the terrain threshold), the model attributes this change to a terrain height adjustment. This predicted terrain height is then subtracted on each iteration to correct the elevation estimate. The threshold is currently set at 0.04m, based on a maximum vertical velocity of 1 m/s and a ToF sensor update rate of 25Hz. This value represents the maximum achievable elevation difference without a terrain change. Known Issues: - Vertical velocity is currently not considered when detecting terrain jumps, leading to the assumption that any elevation change upon exceeding the threshold is entirely due to a terrain shift. Accounting for vertical velocity could improve accuracy. - This approach introduces dead reckoning into the Z-axis estimate, with errors accumulating each time the threshold is exceeded, causing drift in the Z estimate. --- src/modules/src/kalman_core/mm_tof.c | 35 ++++++++++++++++++++++++++-- 1 file changed, 33 insertions(+), 2 deletions(-) diff --git a/src/modules/src/kalman_core/mm_tof.c b/src/modules/src/kalman_core/mm_tof.c index b7499475e1..f69592fdaa 100644 --- a/src/modules/src/kalman_core/mm_tof.c +++ b/src/modules/src/kalman_core/mm_tof.c @@ -24,6 +24,13 @@ */ #include "mm_tof.h" +#include "log.h" +#include "FreeRTOS.h" +#include "task.h" + +static float predictedTerrainHeight = 0.0f; +static float predictedElevation = 0.0f; +static float measuredElevation = 0.0f; void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof) { @@ -38,7 +45,9 @@ void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof) angle = 0.0f; } float predictedDistance = this->S[KC_STATE_Z] / cosf(angle); + predictedElevation = this->S[KC_STATE_Z]; float measuredDistance = tof->distance; // [m] + measuredElevation = tof->distance * cosf(angle) + predictedTerrainHeight; // [m] /* The sensor model (Pg.95-96, https://lup.lub.lu.se/student-papers/search/publication/8905295) @@ -55,7 +64,29 @@ void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof) h[KC_STATE_Z] = 1 / cosf(angle); // This just acts like a gain for the sensor model. Further updates are done in the scalar update function below - // Scalar update - kalmanCoreScalarUpdate(this, &H, measuredDistance-predictedDistance, tof->stdDev); + // Compute residual for ToF measurement (difference between measured and predicted distance) + float residual = measuredDistance - predictedDistance; + + // Check if residual elevation indicates a new terrain feature below + static float terrainThreshold = INFINITY; + if (fabsf(predictedElevation - measuredElevation) > terrainThreshold) { + // Significant change detected, prepare terrain height measurement + predictedTerrainHeight += predictedElevation - measuredElevation; // Update terrain height + } + + // set terrain threshold after first iteration + // sensor update rate we receive here is about 25Hz, assuming a maximum vertical velocity of 1m/s, we can expect up to 4cm change in elevation without terrain change + terrainThreshold = 0.04f; + + residual += predictedTerrainHeight; // Add predicted terrain height to residual + + // Perform Kalman update with the computed residual + kalmanCoreScalarUpdate(this, &H, residual, tof->stdDev); } } + +LOG_GROUP_START(mmtof) + LOG_ADD(LOG_FLOAT, prdTerrainHeight, &predictedTerrainHeight) + LOG_ADD(LOG_FLOAT, prdElevation, &predictedElevation) + LOG_ADD(LOG_FLOAT, measElevation, &measuredElevation) +LOG_GROUP_STOP(mmtof)