Rauch Tung Striebel filter

Model and observation:

$$\begin{array}{ccc}\hfill X_{k+1}& =& f(X_k,U_k)+ϵ,\mathrm{where}ϵ\in \mathcal{N}(0,Q)\hfill \\ \hfill z_{i,k+1}& =& g_i(X_k,\mathcal{P})+{\epsilon }_i\mathrm{where}{\epsilon }_i\in \mathcal{N}(0,R_i)\hfill \\ & & \mathrm{for}i=1\dots \mathrm{Amount}\mathrm{of}\mathrm{measurements}\hfill \\ \hfill z_{k+1}& =& \left[\begin{array}{cccc}{z}_{1,k+1}& z_{2,k+1}& \cdots & z_{N,k+1}\end{array}\right]\hfill \\ \hfill g& =& \left[\begin{array}{cccc}{g}_1& g_2& \cdots & g_N\end{array}\right]\hfill \\ \hfill A_k& =& \mathrm{Jacobian}\mathrm{of}f\mathrm{with}\mathrm{respect}\mathrm{to}{X}_k\hfill \\ \hfill B_k& =& \mathrm{Jacobian}\mathrm{of}f\mathrm{with}\mathrm{respect}\mathrm{to}{U}_k\hfill \\ \hfill H_k& =& \mathrm{Jacobian}\mathrm{of}g\mathrm{with}\mathrm{respect}\mathrm{to}{X}_k\hfill \end{array}$$

Loop 1 - Forward filter: for $k=1\to N_{data}$

after proper initialization

$$\begin{array}{ccc}\hfill X_{k|k-1}& =& f(X_{k-1|k-1},U)\hfill \\ \hfill P_{k|k-1}& =& A_k{P}_{k-1|k-1}{A}_k^T+Q_k\hfill \\ \hfill y_{err}& =& z_k-g(X_{k|k-1},\mathcal{P})\hfill \\ \hfill S_k& =& R_k+H_k{P}_{k|k-1}{H}_k^T\hfill \\ \hfill K_k& =& P_{k|k-1}{H}_k^T\mathrm{inv}\left(S_k\right)\hfill \\ \hfill x_{k|k}& =& x_{k|k-1}+K_k{y}_{err}\hfill \\ \hfill P_{k|k}& =& (I-K_k{H}_k)P_{k|k-1}\hfill \end{array}$$

Loop 2 - Reverse filter: for $k=N_{data}\to 1$

after proper initialization

$$\begin{array}{ccc}\hfill C_k& =& P_{k|k}{F}_k^T\mathrm{inv}\left(P_{k+1|k}\right)\hfill \\ \hfill X_k^S& =& X_{k|k}+C_k(X_{k+1}^S-X_{k+1|k})\hfill \\ \hfill P_k^S& =& P_{k|k}+C_k(P_{k+1|k+1}-P_{k+1|k}),\hfill \end{array}$$