EKF: replacement of covariance prediction autocode with sympy generated output

[priseborough]

This is the covariance prediction part of #868 which is being implemented in a series of smaller PR's to make review and testing easier.

See #868 for earlier discussion.

Difference testing agains the matlab output for 100 randomised input P matrices showed difference ratios in two tests over the nominal test Pass/Fail ratio of 5E-5 that had been set, eg:

Fail: Covariance Prediction max diff fraction = 6.775925e-05 , old = 4.104861e+23 , new = 4.105139e+23 , location index = 4,6
Fail: Covariance Prediction max diff fraction = 1.048742e-04 , old = 1.940318e+11 , new = 1.940521e+11 , location index = 3,16

However the results are acceptable given the unrealistically large values for those entries induced by those tests. For the other 98 randomised tests, not a single matrix entry exceeded the 5E-5 difference ratio.

SITL Test log for gazebo_plane: https://logs.px4.io/plot_app?log=49cd1e69-bdd8-492f-99a8-0f2f94c35105

[kamilritz]

This looks good in general. The difference in the single entries seem small.

Some ideas for the code generation in general:

1. adding a subfolder EKF/python/ekf_derivation/generated for all the generated files. This makes it easier to distinguish between generating code and generated code. Otherwise the folder is a bit messy.
2. Adding some printout into main.py to state the progress of the code generation. e.g. Initialize symoblic variables, compute state jacoabian , and so on. Makes it easier for the user to get a feel for the progress, while running the file. Those prinout could even replace some plain comments.
3. The code generation is based on python2, which got discontinued this year. Could we make it run with python3? I quickly tried to run it with python3 - at least something in the CodeGenerator needs to change.
4. I am no python expert, do we really need the _ init _.py file?
5. The covariance_generated_compare.cpp is nice to show that there is not a big difference. But do we want to add it to the repo itself? Once the change is in, there is not any value in the comparison anymore.
6. It could increase the readability of the code generation if we break the main.py into separate files. Keeping only code from line 355 in the main.py. And moving the rest into files with name like observation_models.py and utils.py.

@priseborough Please let me know, what you think about this. I happy to help with the implementation of these ideas.

[jkflying]

Some ideas for the code generation in general:

1. adding a subfolder EKF/python/ekf_derivation/generated for all the generated files. This makes it easier to distinguish between generating code and generated code. Otherwise the folder is a bit messy.

👍

2. Adding some printout into main.py to state the progress of the code generation. e.g. `Initialize symoblic variables`, `compute state jacoabian` , and so on. Makes it easier for the user to get a feel for the progress, while running the file. Those prinout could even replace some plain comments.

👍

3. The code generation is based on python2, which got discontinued this year. Could we make it run with python3? I quickly tried to run it with python3 - at least something in the `CodeGenerator` needs to change.

My guess is it is something to do with the real / float32. If it isn't that, it is probably an upgraded sympy with a slightly different API. In general we should move to Python3 though, yes.

4. I am no python expert, do we really need the _ _init_ _.py file?

If we want to import the .py file from another directory, then yes. That's just how Python works...

5. The `covariance_generated_compare.cpp` is nice to show that there is not a big difference. But do we want to add it to the repo itself? Once the change is in, there is not any value in the comparison anymore.

IMO: add it, and we can remove it when we remove the old generation.

6. It could increase the readability of the code generation if we break the `main.py` into separate files. Keeping only code from line 355 in the main.py. And moving the rest into files with name like `observation_models.py`  and `utils.py`.

There's arguments both ways. IMO around 500 lines is still (barely) OK, and there's some value to having all the code next to each other so its obvious what calls what. If we end up adding eg. post-processing for powf replacements etc, these could start going to new files. The most important changes I think would be putting the main / top level code into a function, so that it can be used as a library instead of only being called as the main.

@priseborough Please let me know, what you think about this. I happy to help with the implementation of these ideas.

I'd be really happy if you did a bunch of this cleanup 😝

[kamilritz]

@priseborough I took the liberty of implementing points 1 to 3 & some additional cleanup and pushing the changes to this branch. I hope this is okay. The covariance_generated.cpp stays the same.

[priseborough]

@kamilritz thanks for doing that. I'll go through the changes today.

[kamilritz]

We could move ecl::powf to a better location, but besides that OK

[kamilritz]

It seems to me that this would be better placed inside EKF/utils.hpp

[priseborough]

I tried that previously and ran into a redefinition issue.

[kamilritz]

Yes, it was missing a #pragma once or a classic header guard, to avoid including it multiple times into a compilation unit.
I will move it to the utils.hpp

[priseborough]

Change indicator passes on OSX but fails on CI. Will try generating it using the Ubuntu build environment.

[priseborough]

PX4 Firmware build requires https://github.com/PX4/Firmware/pull/15207/files to pass. The problem is that https://github.com/PX4/Firmware/pull/15207/files is exceeding the flash size limit on V2 boards. However with this PR included, we should be below again.

[jkflying]

Let's get this in so we can also update firmware and get that working again.

[priseborough]

Thank you @RomanBapst and @kamilritz for your work on this.