ToF terrain jump handling in EKF #1431
Conversation
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.
gemenerik
changed the title
|
The first 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:
|
gemenerik
changed the title
|
VL53L1x can be configured for higher ranging frequency at a cost in accuracy and max ranging distance. This could allow us to put lower a lower threshold on the terrain changes. |
|
Nice! Is this a feature we would like to be configurable? |
Absolutely, especially given the limitations I think making it a non-default configurable is a good choice |
This PR proposes enhancements to the EKF to improve its ability to handle jumps caused by terrain height changes when using ToF sensors. The aim is to ensure more accurate elevation estimates and minimize the impact of sudden terrain variations on EKF performance.