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.
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.
That$\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_{n+1|n}$ , which means the old calculation of
-Lon the last line should be a+L, and the finalxhat_fm[:,[t+1]]andP_fm[t+1]should have been from indextinstead, because the_fmvariables--mfor "minus", implying a priori predictions--held what I call in the math theP_fm[n]actually heldLwas 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_preand_postfor a priori and a posteriori and index assuming the prediction happens before measurement in a step, and then added comments to make this clearer.