RTS Smoothing bug

[pavelkomarov]

This one was handled in #98, but I'm moving the discussion about it out of #97.

I'm not 100% sure the forward-backward smoothing is implemented correctly. I've reached that point of the paper, and the equations hanging around in there don't match Wikipedia, so I'm going to extra interrogate the relevant piece of code.

The smoothing is slightly wrong.

L = P_fp[t]@A.T@np.linalg.pinv(P_fm[t])
xhat_smooth[:, [t]] = xhat_fp[:, [t]] + L@(xhat_smooth[:, [t+1]] - xhat_fm[:, [t+1]])
P_smooth[t] = P_fp[t] - L@(P_smooth[t+1] - P_fm[t+1])

That -L on the last line should be a +L, and the final xhat_fm[:,[t+1]] and P_fm[t+1] should have been from index t instead, because the _fm variables--m for "minus", implying a priori predictions--held what I call in the math the $\circ_{n|n-1}$ variables, except they held them off-by-one index from what I might expect, because the old code chose to do measurement combination first, then prediction of the next state. This means P_fm[n] actually held $P_{n+1|n}$, which means the old calculation of L was actually correct, but then the wrong indices were used on subsequent lines. Difficult to see as the code was written, so I rewrote it to use _pre and _post for a priori and a posteriori and index assuming the prediction happens before measurement in a step, and then added comments to make this clearer.