Kalman Filter: Gain matrix regression tests and bugfix

This MR locks down the numerical result of the gain matrix components. Splits up the tests into one for the updator and one for the smoother. Both are set up in dummy scenarios, and the output is explicitly asserted. Note that this does NOT actually test that the calculation is correct. The tests only make very basic assertions on the correctness of the result (in the last track state, smoothed and filtered parameters are the same, for instance), but it allows changing the KF math without introducing hidden regression in the numerical result.

After introducing this, I actually found a bug 😅. The smoother incorrectly didn't skip the last track state, smoothing it again, and also tried to reassign to a reference to the previous track state, which is not what's required here. Both are fixed now.

Fixes #563 (closed)