Ukf

[keffklau]

This branch includes the error-state unscented Kalman Filter based on the simplex sigma-point selection strategy for improved flying qualities especially when relying on LPS-TDOA localization method. The filter was presented on Fusion 2022 and is described in the paper below:

Klaus Kefferpütz, Kimberly McGuire: Error-State Unscented Kalman-Filter for UAV Indoor Navigation, 2022 25th International Conference on Information Fusion (FUSION), https://ieeexplore.ieee.org/abstract/document/9841385

The algorithm supports

• LPS TDOA navigation
• LPS TDOA navigation with additional flow deck measurements
• Pure flow deck measurements.

The filter can be selected within the kbuild menuconfig.

[knmcguire]

Hi! Thanks for this! Unfortunately there seems to be some build errors so I'm not sure if that is something you can try to solve? It seems to go wrong when all configs in kbuild are set to yes or random.

[keffklau]

Hey!
we suppose it is an issue with variables and CCMRAM. We'll try to move variables out of the CCMRAM and hopefully, this will solve the problem.

[knmcguire]

Hi! Unfortunately there are currently conflicts too if we try to merge this... Probably relatively easy to fix but we shouldn't wait too long with this.

What is the ETA for updating this PR?

[TommiAux]

Hi! We examined the allocated stack size for the task running the UKF more closely and it was set much too high. It was possible to reduce the stack size so far that now no overflow of the CCM occurs. Further changes to the source code were not necessary.

[knmcguire]

Thanks! We are still a bit low here on staff so we need to find time to test this out in the next two weeks.

[knmcguire]

Hi! Sorry for the delay! It was difficult with the winterbreak in between to get started with this big PR 😅

I have already a first batch of comments. There are a couple of things to consider I think.

• The error_estimator_uki.c is very large. Have you considered splitting it up like was done with the kalman_core?
• There are several functions that exist, like matrix transpose or such that already exist in the crazyflie-firmware already as standard math functions. In generic the firmware is already a bit inconsistent with that (see this ticket Unifying math functions #1182) but still.
• Documentation: In the comments I noted that you could add comments to the logs and parameters so that they will be autogenerated in the web like here . Also you could also add a piece of text in the state estimation page on the web ( source ).

So there are quite some comments indeed... but on the other hand, the estimator is also turned off by default in kconfig and then everything in one file is actually a bit handier for the time being...

I will discuss with the rest once everybody is back at the office next Monday on their opinion.

[knmcguire]

Hi! Me and @krichardsson discussed a bit and came to the conclusion that in general it would be fine to merge this all as one file, especially since the filter is disabled in kbuild. So I've resolved some of the parts I've mentioned above. However we do think that by minimal there should be some documentation.

• A mention in the stabilization documentation, with a mention of that it needs to be enabled in kbuild since it is experimental. It can be added here in markdown format: https://github.com/bitcraze/crazyflie-firmware/blob/master/docs/functional-areas/sensor-to-control/state_estimators.md
• The stabilizer parameter should mention this as option here, but perhaps with a '(has to be enabled in kbuild)' behind it or '(experimental)':

  crazyflie-firmware/src/modules/src/stabilizer.c

  Lines 361 to 364 in 45d07a2

  	/**
  	* @brief Estimator type Any(0), complementary(1), kalman(2) (Default: 0)
  	*/
  	PARAM_ADD_CORE(PARAM_UINT8, estimator, &estimatorType)

• (OPTIONAL) You can fill in the parameter documentation as well as I mentioned above but since it is experimental this can be optional.
• There are some things I noticed in the previous review runthrough, so please have a look at all comments that I haven't marked unresolved

I also flight tested it:

• Loco and Flow+Loco works great!
• Lighthouse alone does not work at all... does it need to be combined with loco?
• Flowdeck alone kinda works... but it drifts away quite easily.
  In that sense, these limitations can be added in the general documentation.

[keffklau]

Hey,

great to hear that. Honestly, it is a time issue to optimize the code and restructure it (what could be done in general - since there are some functions are already implemented in the existing framework and are somehow redundant).
Having a separate file makes it easier to further extend/modify this.

Documentation: I will have a look on it (by the very end of the week). Do I need permissions to modify it or send you the text?

Flight Test

• Loco and Flow / only Loco: That is indeed the parameterization. My primary concern was improving LPS flying quality so the standard-parameterization is for this use-case.
• Only Flow /Drift: This might be due to the parameterization for the outlier rejection. If one would only use flow, one has to increase the threshold for the quality gate in the outlier rejection. I am pretty sure, using the current setting those bounds are to tight which is better when there is another positioning system around. In summary: It works with pure flow but needs another setting for qualityGateTof (very large values for "qualityGateTof" deactivate the outlier rejection and it should work)
• Lighthouse: Unfortunately yes. That's the next issue I have to solve (since I want to guide Crazyflies with others relying on LH it must work reliably.). The issue (please state if you observed another behavior) is the initial convergence. If the ukf is able to converge initially, I could fly nicely with the error UKF and LH. However, the initialization does not work out reliably. I hope to find out the reason soon - but it appears to be painful to debug since the firmware usually crashes when the problem occurs. It depends also on where you place your CF in the flying space (for me, it works approximately every second try if I place the CF very close to the middle of the flying space - which is of course far from being satisfactory).

For now, I think I should state in the documentation that the use case is LPS and LPS & Flow, Maybe spend a comment on setting different parameters for qualityGateTof for pure Flow-case.
s soon as the LH issue is solved, I could make an update. It might be that different initial covariances are required for LH and LPS - not ideal but that is what I expect.

[knmcguire]

I can help out with fixing all code-related things that I mentioned, so I'll push that to this branch soon since I have write access to that branch. If you indeed could suggest a piece of text that we can add at the documentation, then I'll take care of that as well.

[keffklau]

Hey, that would help indeed. I'll provide a text for the documentation in the next days - next weekend at latest.

[knmcguire]

Alright! I've pushed the changes and doc!

Another big change is that I've renamed the error_estimator_UKF.* to estimator_ukf. This is out of consistency with the other estimators and it would be easier for people to find the code. Moreover, the documentation already states the error-state' preamble several times already so I think that most get the picture.

Also, I've removed the part of the lighthouse, as I think it would be better to put that into the issue list. https://github.com/bitcraze/crazyflie-firmware/issues

@keffklau Are you okay with the latest changes I made?