addressing #68, #69, #71, #95, and #96 for the kalman module

[pavelkomarov]

The tests are definitely going to fail, because I identified at least one bug that changes core logic, so the results will be a tiny bit numerically different. We'll check against improved unit tests once that's merged. This also addresses #97 some, but I'm afraid that one will take significantly more work if we want to actually properly extend this module. Even the linear KF in this module isn't calculating a derivative (mistake?), which makes me extra suspicious that it hasn't had any love in a long time.

[pavelkomarov]

There's an easier way.

[pavelkomarov]

I found making this a separate function to be unnecessary; its logic now lives in the forward filter.

[pavelkomarov]

Some duplicated lines in here. I got rid of them.

[pavelkomarov]

What is this supposed to do? It was doing nothing, so I got rid of it.

[pavelkomarov]

I've renamed all these to be a little more explicit about what they are.

[pavelkomarov]

I many places you weren't using 0 as the batch dimension. But axis 0 should always be the batch dimension so you can pull out a sample without having to do complicated slicing.

[pavelkomarov]

The order of these calls is inverted from how I tend to think about the KF. You can start on either the prediction or the update side, but I start on prediction in the paper, and so do the standard Wikipedia and many other pseudocodes for the KF. Thus, your _fm variables here contain the next time step's prediction, but they're indexed at this time step, which causes confusion when you go do your RTS implementation.

[pavelkomarov]

So this line is actually correct, because P_fm[t] holds $P_{t+1|t}$.

[pavelkomarov]

But this line is incorrect, because you're supposed to use $P_{t+1|t}$ here as well, which is stored in P_fm[t]. https://arc.aiaa.org/doi/10.2514/3.3166

This line is also wrong because you're subtracting the L part instead of adding it. So this isn't doing RTS smoothing at all. Serious bug.

[pavelkomarov]

You don't need all these shadow functions behind the front-facing ones, because you should really only define the matrices once and then call smoothing twice. As written, the matrices were getting redefined for the forward and backward passes separately, extra work. So the two levels got combined across all these methods.

[pavelkomarov]

So so so so much code in this module was just copy and pasted, and the structure to call things either forward or forward and backward was repeated unnecessarily, so I've put that logic in a single place.

[pavelkomarov]

It's very odd to me that the Kalman with RTS smoothing already works forward and backward, but then you invert the dynamics and go again and then take a weighting of the answers. Why? I thought RTS smoothing was already supposed to be optimal. Did it not seem to work at one point, and these were further hacks? I wonder if having the bugs in the RTS smoother fixed will make the hacks unnecessary.

[pavelkomarov]

The answer is that a non-optimal guess to kick off the process biases the end you start on, and it takes a while for that bias to wear off. Classic RTS (even sans bugs) doesn't fully address this, but doing two of them and weighting together does.

Ideally, you could choose an initial x0,P0 that wouldn't horribly bias the process, but due to $C$ not being full rank, there is no way to choose the single optimal x0,P0. See my ponderings on #97.

[pavelkomarov]

This strikes me as a hack. I don't like it.

[pavelkomarov]

Unfortunately, these function signatures repeat a bunch of docstring and input-checking logic, but I can't simplify further without breaking backwards compatibility. I'd prefer a single constant_derivative function that takes order as a parameter.

[pavelkomarov]

The docstring indicates here and in its topline by use of the words "to estimate the derivative" that this function should be calculating a derivative, but it's not. This begs the question: Does anybody use this, since it went unnoticed? Can I break backwards compatibility for this function alone?

[pavelkomarov]

If this is true, then we should nuke this code per #95. If we ever wish to recover the experiment, we can go back to an ancient tagged commit and fish it out of the nether.

[pavelkomarov]

It's not necessary to reshape 1D vectors into vertical 2D column vectors, because A @ x will do the exact same thing in either case.

[pavelkomarov]

This initialization is slightly odd, because it says x0, P0 are the a priori estimate for the current time based on the previous time, rather than considering them the a posteriori estimate from last time. The difference is that last time's a posteriori estimate would be propagated forward before projection through $C$ to produce an expected measurement for combination with the first measurement, whereas an a priori estimate already associated with the current time would simply be projected through $C$.

What makes more sense when we ask the user for an initial condition? "Tell us where you think the system is, one time step before seeing any measurements" or "Tell us where you think the system is at the moment of the first measurement"? Six on one hand, half dozen on the other. I spent a good while in #97 trying to think of a good way to initialize the cycle, but any projection from measurement space to full state space requires the measurement space to have the same dimension as the state space, which just isn't our case at all for all the constant derivative stuff, where the dimension of x goes up to the $\nu^\text{th}$ derivative but the dimension of y is always 1.

In the end I decided I prefer to project our identity matrix P0 guess through $A$ and then add $Q$ before projecting through $C$ to get the expected covariance, if only because it agrees with what I've written in the paper and most explanations I've ever seen.

[pavelkomarov]

If we end up preferring the opposite, it's easy to change _kalman_forward_filter.

[pavelkomarov]

u can safely be 1D.

[pavelkomarov]

I'm using a more exact A here, per #96

[pavelkomarov]

Okay, reporting in about the "correction" happening in the Kalman smooth functions, which I thought shouldn't be there and was maybe the result of a hack to compensate for an RTS smoothing bug.

Here is constant_velocity on the first two test functions with the "correction" still in my code:

The blue line is the function, and the yellow line is its derivative.

And here is the performance if I remove the correction and just stick to weighting the forward and backward passes of RTS:

And just for completeness, here is the result again without the correction, but when I set forwardbackward=False:

I'll compare performance for constant_acceleration and constant_jerk as well to see if this pattern holds.

[pavelkomarov]

Here is the full plot for constant_acceleration with the "correction":

And here is without the correction:

Without a doubt, without is better. I'm going to remove the correction in this PR.

[pavelkomarov]

This one now makes use of the changes from #102, so order of operations says that one should be reviewed and merged first. Then we can git pull origin master to this branch to rebase and exclude those changes from this review.