EKF: Convert magnetic field observation methods to use SymPy generated code

[priseborough]

This PR replaces the Matlab generated equations with sympy generated equations. Continues on from #870.

Modifies the methods used for fusion of 3-axis magnetometer data, declination angle and yaw angle.
Includes comparison test scripts that compare the observation Jacobians and Kalman gain equations generated for the Matlab symbolic toolbox and SymPy.

Testing

3Dmag_fusion_generated_compare.cpp output:

Pass: Magnetomer X axis Hfusion max diff fraction = 0.000000e+00
Pass: Magnetomer X axis Kfusion max diff fraction = 7.846418e-08
Pass: Magnetomer Y axis Hfusion max diff fraction = 0.000000e+00
Pass: Magnetomer Y axis Kfusion max diff fraction = 1.035609e-07
Pass: Magnetomer Z axis Hfusion max diff fraction = 0.000000e+00
Pass: Magnetomer Z axis Kfusion max diff fraction = 0.000000e+00

mag_decl_fusion_generated_compare.cpp output:

Pass: Mag Declination Hfusion max diff fraction = 0.000000e+00
Pass: Mag Declination Kfusion max diff fraction = 1.228912e-07

yaw_fusion_generated_compare.cpp output:

Pass: 321 yaw option A Hfusion max diff fraction = 2.090426e-07
Pass: 321 yaw option B Hfusion max diff fraction = 1.139841e-07
Pass: 312 yaw option A Hfusion max diff fraction = 1.333159e-07
Pass: 312 yaw option B Hfusion max diff fraction = 1.998272e-07

Comparison of mag fusion innovations and field estimates before and after the change:

Before:

After:

Comparison of yaw innovations and innovation variances before and after the change:

Before:

After:

[kamilritz]

I think this PR misses the mag related output of the autocode generation.

[kamilritz]

There are two ways how we compute the mag update. The normal sequential one and then the alternative. I am not sure if the alternative way is conceptually fully correct.
We compute first the innovation variances for all axes with the help of some shared intermediate variables. Then we fuse sequentially all axes, but during this we reuse shared intermediate variables from the innovation variance computation for the computation of H and K. If we want to be fully correct we would not be allowed to reuse some of the shared intermediate variables as they are a function of P, which changes after each sequential fusion.
@priseborough Do you think this is critical?

[priseborough]

I don't think it's an issue given they are orthogonal observations and i couldn't detect a visual difference either way when doing the early comparisons, but technically it is an approximation because the covariances will have changed as each axis is fused.

I will create an alternative version using the other method and lets see how much flash and operations we are saving. If the difference is small, then the more independent method is lower risk.

[kamilritz]

@priseborough @jkflying I created a SparseVector class, that only stores the N nonzero elements in a Vector<Type, N>, but still allows to access them with the original index. E.g.

SparseVector<float, 16, 17> a;  // owns a Vector<float, 2>
a(16) = ...;
a(17) = ...;
a(1) = ...;  // will assert during compilation as index is invalid

When calling the operator() with a compile-time constant as above, finding the right entry in the Vector<Type, N> will happen at compile time if using at least -O1 optimization. See here for produced assembly code in a toy example: https://godbolt.org/z/f7f9f9.

I think this would be a nice solution for the measurement jacobian, as it helps to avoid modifying the auto generated code (except for removing the zero lines), but still avoiding a Vector with a lot of nonzero elements that are stored in memory.

I took the liberty to add the class to this PR and applying it to the mag related measurement jacobians. @jkflying could you please have a look at the SparseVector class and let me know what you think about this approach?

[jkflying]

I took the liberty to add the class to this PR and applying it to the mag related measurement jacobians. @jkflying could you please have a look at the SparseVector class and let me know what you think about this approach?

This looks really good, I'd actually like to get it into Matrix first. There's a few things I think should be improved (eg handling of non-dense indices should return 0, add a dot product with dense vector (or other sparse vector) functionality, etc), but the use of the variadic template values in this case makes a lot of sense. Also I don't think ECL/sympy PR is the right place to review this in depth 😉 Once we have it in Matrix with the functionality we want, we can then go back and reduce the memory consumption of the new code generation, otherwise I think as SparseMatrix undergoes some small API changes there might be a lot of churn in ECL.

[kamilritz]

This looks really good, I'd actually like to get it into Matrix first. There's a few things I think should be improved (eg handling of non-dense indices should return 0, add a dot product with dense vector (or other sparse vector) functionality, etc), but the use of the variadic template values in this case makes a lot of sense. Also I don't think ECL/sympy PR is the right place to review this in depth wink Once we have it in Matrix with the functionality we want, we can then go back and reduce the memory consumption of the new code generation, otherwise I think as SparseMatrix undergoes some small API changes there might be a lot of churn in ECL.

Ok, let me remove it from here. And open a PR on Matrix. Feel free to modify it as you like.

[jkflying]

@kamilritz some inspiration, note that there are recursion inlining limits hit in run2 unless you make the indices template parameters. If you make them template parameters, you can have compile-time checking that your indices are correct.

https://godbolt.org/z/rc5nnr

[priseborough]

I've created a branch here that implements the fusion without the assumption of invariant covariance and states between fusion of orthogonal axes - https://github.com/PX4/ecl/tree/pr-ekfSymPyMagFusionAlt

The results from replay are visually identical:

Approximation method (this PR):

Exact method from https://github.com/PX4/ecl/tree/pr-ekfSymPyMagFusionAlt

The next step is to see if i can generate some profiling comparison data using SITL - @dagar is there a way to do profiling with replay?

If the cost isn't much more, then I would prefer the 'exact' method. The previous sequential fusion implementation using the Matlab equations also had some built in assumptions of invariance, so the method implemented in https://github.com/PX4/ecl/tree/pr-ekfSymPyMagFusionAlt is theoretically better than what we had before.

[priseborough]

Here are the callgrind comparisons generated using "HEADLESS=1 make px4_sitl jmavsim___callgrind" to generate the callgrind.out file and using kcachegrind to generate the figures. These have been zoomed to show the 3d mag fusion cycle count estimates for the mag fusion processes as a % of the ekf update() function.

Approximate method (assumes invariant states and covariance between fusion of each axis:

Exact method from https://github.com/PX4/ecl/tree/pr-ekfSymPyMagFusionAlt branch:

The increase in cycle count is negligible so i will pull in the patch for the exact method.

[kamilritz]

@priseborough I changed the observation jacobian to be of type SparseVector24f. It uses a different syntax to access an element, H.at<index>(), so I also adopted the auto code generation to allow to output the observation jacobian code with this syntax.

Additionally, I added a function that computes KHP efficiently for different observation. Please have a look, as this can be applied for all the other fusion types, where we want to update to sympy code generation.

[priseborough]

@kamilritz I got the following build error when I tried to test your changes;

In file included from ../../src/lib/ecl/EKF/drag_fusion.cpp:42:0:
../../src/lib/ecl/EKF/ekf.h:56:34: error: ‘SparseVectorf’ in namespace ‘matrix’ does not name a template type
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;

Edit: The Firmware build test is failing for the same reason.

[kamilritz]

@kamilritz I got the following build error when I tried to test your changes;

In file included from ../../src/lib/ecl/EKF/drag_fusion.cpp:42:0:
../../src/lib/ecl/EKF/ekf.h:56:34: error: ‘SparseVectorf’ in namespace ‘matrix’ does not name a template type
using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;

Edit: The Firmware build test is failing for the same reason.

I am on it. I need to bring the matrix update into Firmware first. It is a bit of a hurdle race, but I am soon there.

EDIT: only needs merging PX4/PX4-Autopilot#15520 and updating change indication

[priseborough]

I've been through the SparseVector24 code changes and verified that attempting to access a 'blank' index for either read or write gives an unambiguous build error.

Will generate updated change indication.

[kamilritz]

@priseborough It would be great if you could re-run the log to check if everything is still okay. After this we should be good to merge this.

[priseborough]

Replay results before and after change shown below. They are visually the same.

Before:

After:

[kamilritz]

@priseborough could update the change indication file again. It seems I made a mistake when merging it. I think if you just add it again in a new commit. it will be okay.