search

rlabbe / filterpy

Python Kalman filtering and optimal estimation library. Implements Kalman filter, particle filter, Extended Kalman filter, Unscented Kalman filter, g-h (alpha-beta), least squares, H Infinity, smoothers, and more. Has companion book 'Kalman and Bayesian Filters in Python'.
MIT License
3.3k stars 615 forks source link

RTS smoother implementation bug? #258

Open nnop opened 2 years ago

nnop commented 2 years ago

RTS implementation only use posterior outputs of Kalman filter. https://github.com/rlabbe/filterpy/blob/06bd64fbb50b70e8bfc2be180c8e0a2fedd087f0/filterpy/kalman/tests/test_rts.py#L43-L46

However, according to wikipedia, both posterior and prior of x and P should be used.

image

This seems a clear bug.

KoertS commented 10 months ago

I was also confused by this. However in the source code you can see that the prior is recomputed during the rts smoothing. The prior covariance is pP, which is calculated as followed: pP[k] = dot(dot(Fs[k], P[k]), Fs[k].T) + Qs[k], which is equivalent to $P_{k+1|k} = FkP{k|k}F^T_k + Qk$. The a-priori state estimate $\hat{x{k|n}}$ is calculated with the code snippet dot(Fs[k], x[k])) (which is equal to $F \cdot X{k|k} = X{k+1|k}$).

So it is not a bug, the prior is just recalculated inside the rts_smoother function