The SVSS Algorithm

Input {$x_0,P_0,\psi ,F,Q,R,H$} and the sequence measurement {$z_1,z_2,\dots ,z_N$}   Step 1 fiter   ${\widehat{x}}_{k+1|k}=F{x}_{k|k}$   $P_{k+1|k}=F{P}_{k|k}{F}^T+Q_k$   $e_{k+1|k}=z_k-H{\widehat{x}}_{k+1|k}$   $K_{k+1}=H^{+}diag\left(\left|e_{k+1|k}\right|+\gamma \left|e_{k|k}\right|\right)•sat\left({\overline{\psi }}^{-1}{e}_{k+1|k}\right)\times {\left[diag\left(e_{k+1|k}\right)\right]}^{-1}$   ${\widehat{x}}_{k+1|k+1}={\widehat{x}}_{k+1|k}+K_{k+1}{e}_{k+1|k}$   $P_{k+1|k+1}=\left(I-K_{k+1}H\right)P_{k|k+1}{\left(I-K_{k+1}H\right)}^T+K_{k+1}{R}_{k+1}{K}_{k+1}^T$   $e_{k+1|k+1}=z_{k+1}-H{\widehat{x}}_{k+1|k+1}$   Step 2 smoothing   $C_i=P_{i|i}{F}^T{P}_{i+1|i}^{-1}$   $B_N=P_{N|N-1}{H}^T{(H{P}_{N|N-1}{H}^T+R)}^{-1}$   ${\widehat{x}}_{k|N}={\widehat{x}}_{k|N-1}+({\displaystyle \prod _{i=k}^{N-1}{C}_i})B_N(z_N-H{\widehat{x}}_{N-1})$   Output {${\widehat{x}}_{k|N}$}