Update queued measurements in kalman filter

Discussions about quadcopters/multi-rotors
Post Reply
marc345
Beginner
Posts: 7
Joined: Mon Aug 24, 2020 8:02 pm

Update queued measurements in kalman filter

Post by marc345 »

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
Last edited by marc345 on Tue Jan 05, 2021 11:09 am, edited 1 time in total.
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: Update queued measurements in kalman filter

Post by kimberly »

Hi!

Yes this is interesting indeed! I will try to look into it this week if I have time or else next week! It is the holidays so it is a bit empty here now at the office :)
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Update queued measurements in kalman filter

Post by kristoffer »

Hi Marc!

That is not the intention, good catch!
It looks like it was me messing up when refactoring.

Fixed in https://github.com/bitcraze/crazyflie-f ... issues/669

Thanks!
marc345
Beginner
Posts: 7
Joined: Mon Aug 24, 2020 8:02 pm

Re: Update queued measurements in kalman filter

Post by marc345 »

Cool, thanks for letting me know.

All the best
Marc
Post Reply