Update queued measurements in kalman filter
Posted: Thu Dec 24, 2020 3:31 pm
Hi everbody
I recently work on a university project involving the extended Kalman filter algorithm of the CF. I noticed something earlier, in the task that implements the eKf in the estimator_kalman.c source file in the modules folder of the firmware.
We have a variable doneUpdate which is initialized to false at the beginning of each task round and set to true if we predicted the state forward or updated the state with some sensor measurement, because we finally have to finalize the state according the the algorithm.
However on the following line the doneUpdate variable is logically ORed (||) with the function call to updateQueuedMeasurments() which returns true if any connected sensor had measurement data ready and an update was done.
I experimented a bit and found out that in the case when doneUpdate is already true, due to a prediction in the same eKf round, updateQueuedMeasurments() won't get called because the overall expression evaluates to true nevertheless what the function returns. This would mean we never do an measurement update in the same round as a prediction, and only look at the measurement queues if no prediction took place in a round before. Is this the intended behavior?
All the best and Merry Christmas to everybody,
Marc
I recently work on a university project involving the extended Kalman filter algorithm of the CF. I noticed something earlier, in the task that implements the eKf in the estimator_kalman.c source file in the modules folder of the firmware.
We have a variable doneUpdate which is initialized to false at the beginning of each task round and set to true if we predicted the state forward or updated the state with some sensor measurement, because we finally have to finalize the state according the the algorithm.
However on the following line the doneUpdate variable is logically ORed (||) with the function call to updateQueuedMeasurments() which returns true if any connected sensor had measurement data ready and an update was done.
I experimented a bit and found out that in the case when doneUpdate is already true, due to a prediction in the same eKf round, updateQueuedMeasurments() won't get called because the overall expression evaluates to true nevertheless what the function returns. This would mean we never do an measurement update in the same round as a prediction, and only look at the measurement queues if no prediction took place in a round before. Is this the intended behavior?
All the best and Merry Christmas to everybody,
Marc
