A New Heavy-Tailed Robust Kalman Filter with Time-Varying Process Bias

Abstract

A new heavy-tailed robust Kalman filter is presented to address the issue that the linear stochastic state-space model has heavy-tailed noise with time-varying process bias. The one-step predicted probability density function (PDF) is modeled as the Student’s-t-inverse-Wishart distribution, and the likelihood PDF is modeled as the Student’s-t distribution. To acquire the approximate joint posterior PDF, the conjugate prior distributions of the state vector and auxiliary variables are set as the Gaussian, the inverse-Wishart, the Gaussian-Gamma, and the Gamma distributions, respectively. A new Gaussian hierarchical state-space model is presented by introducing auxiliary variables. Based on the proposed Gaussian hierarchical state-space model, the parameters of the proposed heavy-tailed robust filter are jointly inferred using the approach of the variational Bayesian. The simulation illustrates that the time-varying process bias is adaptively real-time estimated in this paper. In comparison with the existing cutting-edge filters, the presented heavy-tailed robust filter obtains higher accuracy.

Appendices

Appendix A: Detailed Derivation of the Log Joint Posterior PDF

Using (16)–(18), (20)–(21) into (25), the joint posterior PDF $p(\varTheta ,{\mathbf {y}}_{1:k})$ can be reformulated as follows

$$\begin{aligned} \begin{aligned} p(\varTheta ,{\mathbf {y}}_{1:k})&=N({\mathbf {y}}_k;{\mathbf {H}}_k{\mathbf {x}}_k,{\mathbf {R}}_k/\lambda _k)G(\lambda _k;\nu /2,\nu /2) N({\mathbf {x}}_k;{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}+{\varvec{\psi }}_k,P_{k|k-1}/\xi _k)\\&\times IW({\mathbf {P}}_{k|k-1};{\hat{t}}_{k|k-1},{\mathbf {T}}_{k|k-1}) N({\varvec{\psi }}_k;\hat{{\mathbf {q}}}_{k|k-1},{\varvec{{\Psi }}}_{k|k-1}/\xi _k)G(\xi _k;\omega /2,\omega /2) \end{aligned}\nonumber \\ \end{aligned}$$

(57)

where the Gaussian PDF of a random vector of d dimension is given by [2]

$$\begin{aligned} N(x;\mu ,\varSigma )=\frac{1}{(2\pi )^{d/2}|\varSigma |^{1/2} }exp(-\frac{1}{2}(x-\mu )^T\varSigma ^{-1}(x-\mu )) \end{aligned}$$

(58)

and the inverse Wishart PDF of a symmetric positive definite random matrix B of $\text {d} \times \text {d}$ dimension is given by [2]

$$\begin{aligned} IW({\mathbf {B}};\varsigma , \varDelta )=\frac{|\varDelta |^{\varsigma /2}{\mathbf {B}}^{-(\varsigma +d+1)/2}exp(-0.5{\text {trace}}(\varDelta {\mathbf {B}}^{-1}))}{2^{d\varsigma /2}\varGamma _d(\varsigma /2)} \end{aligned}$$

(59)

where $\varGamma (\cdot )$ denotes the Gamma function, and the Gamma PDF is given by [2]

$$\begin{aligned} G(\sigma ;\zeta ,\eta )=\frac{\eta ^{\zeta }}{\varGamma (\zeta )}\sigma ^{\zeta -1}exp(-\sigma \eta ) \end{aligned}$$

(60)

Exploiting (58)–(60), and taking log operation to Eq.  (57), the log joint posterior PDF is formulated as follows

$$\begin{aligned} \begin{aligned} {\text {log}} p(\varTheta ,{\mathbf {y}}_{1:k})=&-0.5\lambda _k({\mathbf {y}}_k-{\mathbf {H}}_k{\mathbf {x}}_k)^T{\mathbf {R}}_k^{-1}({\mathbf {y}}_k-{\mathbf {H}}_k{\mathbf {x}}_k)\\&-0.5\xi _k({\mathbf {x}}_k-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)^T{\mathbf {P}}_{k|k-1}^{-1} ({\mathbf {x}}_k-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)\\&-0.5\xi _k({\varvec{\psi }}_k-\hat{{\mathbf {q}}}_{k|k-1})^T{\varvec{{\Psi }}}_{k|k-1}^{-1}({\varvec{\psi }}_k-\hat{{\mathbf {q}}}_{k|k-1})\\&-0.5{\text {trace}}({\mathbf {T}}_{k|k-1}{\mathbf {P}}_{k|k-1}^{-1}) -0.5(n+{\hat{t}}_{k|k-1}+2){\text {log}}|{\mathbf {P}}_{k|k-1}|\\&+(0.5m+0.5\nu -1){\text {log}}\lambda _k-0.5\nu \lambda _k\\&+(0.5n+0.5\omega -1){\text {log}}\xi _k-0.5\omega \xi _k+c_\theta \end{aligned} \end{aligned}$$

(61)

Appendix B: Proof of the Proposition 2

Define the modified one-step predicted PDF is $p^{(i+1)}({\mathbf {x}}_k|{\mathbf {y}}_{1:k-1})$ as follows

$$\begin{aligned} p^{(i+1)}({\mathbf {x}}_k|{\mathbf {y}}_{1:k-1}) =N({\mathbf {x}}_k;\hat{{\mathbf {x}}}_{k|k-1},\overline{{\mathbf {P}}}_{k|k-1}^{(i+1)}) \end{aligned}$$

(62)

similar to the modified one-step predicted PDF, define the modified likelihood PDF $p^{(i+1)}({\mathbf {y}}_k|{\mathbf {x}}_k)$ as follows

$$\begin{aligned} p^{(i+1)}({\mathbf {y}}_k|{\mathbf {x}}_k) =N({\mathbf {y}}_k;{\mathbf {H}}_k{\mathbf {x}}_k,\overline{{\mathbf {R}}}_k^{(i+1)}) \end{aligned}$$

(63)

where $\overline{{\mathbf {P}}}_{k|k-1}^{(i+1)}$ and $\overline{{\mathbf {R}}}_k^{(i+1)}$ are the modified one-step predicted error covariance matrix and the modified measurement noise covariance matrix, which is as follows

$$\begin{aligned} \overline{{\mathbf {P}}}_{k|k-1}^{(i+1)}=&\frac{\{E^{(i)}[{\mathbf {P}}_{k|k-1}]\}^{-1}}{E^{(i)}[\xi _k]} \end{aligned}$$

(64)

$$\begin{aligned} \overline{{\mathbf {R}}}_k^{(i+1)}=&\frac{{\mathbf {R}}_k}{E^{(i)}[\lambda _k]} \end{aligned}$$

(65)

Using (62)–(65) in (31), we obtain

$$\begin{aligned} q^{(i+1)}({\mathbf {x}}_k)=\int \frac{1}{c_k^{(i+1)}}p^{(i+1)}({\mathbf {y}}_k|{\mathbf {x}}_k)p^{(i+1)}({\mathbf {x}}_k|{\mathbf {y}}_{1:k-1}) \end{aligned}$$

(66)

where $c_k^{(i+1)}$ is the normalizing constant, which is as follows

$$\begin{aligned} c_k^{(i+1)}=\int p^{(i+1)}({\mathbf {y}}_k|{\mathbf {x}}_k)p^{(i+1)}({\mathbf {x}}_k|{\mathbf {y}}_{1:k-1})d{\mathbf {x}}_k \end{aligned}$$

(67)

Appendix C: Proof of the Proposition 3

Using (37), the ${\text {log}}q^{(i+1)}({\varvec{\psi }}_k,\xi _k)$ can be rewritten as follows

$$\begin{aligned} {\text {log}}q^{(i+1)}({\varvec{\psi }}_k,\xi _k)&={\text {log}}N({\varvec{\psi }}_k;\hat{{\mathbf {q}}}_{k|k-1},{\varvec{{\Psi }}}_{k|k-1}/\xi _k)\nonumber \\&\quad +\,(0.5n+0.5\omega -1){\text {log}}\xi _k-0.5\omega \xi _k\nonumber \\&\quad -0.5\xi _k {\text {trace}}({\mathbf {P}}_{k|k}^{(i)}E^{(i)}[{\mathbf {P}}_{k|k-1}^{-1}])\nonumber \\&\quad -0.5\xi _k(\hat{{\mathbf {x}}}_{k|k}^{(i)}-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)^TE^{(i)}[{\mathbf {P}}_{k|k-1}^{-1}]\nonumber \\&\quad \times \,(\hat{{\mathbf {x}}}_{k|k}^{(i)}-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)+c_{\{{\varvec{\psi }}_k,\xi _k\}} \end{aligned}$$

(68)

To derive the approximate posterior PDF of the time-varying process bias, define the modified time-varying process bias and the corresponding error covariance as $\widetilde{{\varvec{\psi }}}_k^{(i)}$ and $\widetilde{{\varvec{{\Psi }}}}_k^{(i)}$, which is as follows

$$\begin{aligned} \widetilde{{\varvec{\psi }}}_k^{(i)}&=\hat{{\mathbf {x}}}_{k|k}^{(i)}-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1} \end{aligned}$$

(69)

$$\begin{aligned} \widetilde{{\varvec{{\Psi }}}}_k^{(i)}&=\{E^{(i)}[{\mathbf {P}}_{k|k-1}^{-1}]\}^{-1} \end{aligned}$$

(70)

by inserting (69) and (70) into (68), Eq.  (68) is reformulated as follows

$$\begin{aligned} \begin{aligned} {\text {log}}q^{(i+1)}({\varvec{\psi }}_k,\xi _k)&= {\text {log}}N({\varvec{\psi }}_k;\hat{{\mathbf {q}}}_{k|k-1},{\varvec{{\Psi }}}_{k|k-1}/\xi _k)\\&+(0.5n+0.5\omega -1){\text {log}}\xi _k-0.5\omega \xi _k\\&-0.5\xi _k {\text {trace}}({\mathbf {P}}_{k|k}^{(i)}(\widetilde{{\varvec{{\Psi }}}}_k^{(i)})^{-1})\\&-0.5\xi _k(\widetilde{{\varvec{\psi }}}_k^{(i)}-{\varvec{\psi }}_k)^T(\widetilde{{\varvec{{\Psi }}}}_k^{(i)})^{-1} (\widetilde{{\varvec{\psi }}}_k^{(i)}-{\varvec{\psi }}_k)+c_{\{{\varvec{\psi }}_k,\xi _k\}} \end{aligned} \end{aligned}$$

(71)

Define prior and likelihood PDFs of the modified time-varying process bias as

$$\begin{aligned}&p({\varvec{\psi }}_k|\xi _k,{\mathbf {y}}_{1:k-1})=N({\varvec{\psi }}_k;\hat{{\mathbf {q}}}_{k|k-1},{\varvec{{\Psi }}}_{k|k-1}/\xi _k) \end{aligned}$$

(72)

$$\begin{aligned}&p(\widetilde{{\varvec{\psi }}}_k^{(i)}|{\varvec{\psi }}_k,\xi _k,{\mathbf {y}}_{1:k-1})=N(\widetilde{{\varvec{\psi }}}_k^{(i)}; {\varvec{\psi }}_k,\frac{\widetilde{{\varvec{{\Psi }}}}_k^{(i)}}{\xi _k}) \end{aligned}$$

(73)

using the Bayesian’s rule and the standard KF measurement-update stage, we have

$$\begin{aligned} \begin{aligned} p({\varvec{\psi }}_k|\xi _k,{\mathbf {y}}_{1:k-1})p(\widetilde{{\varvec{\psi }}}_k^{(i)}|{\varvec{\psi }}_k,\xi _k,{\mathbf {y}}_{1:k-1})= p({\varvec{\psi }}_k|\xi _k,{\mathbf {y}}_{1:k-1},\widetilde{{\varvec{\psi }}}_k^{(i)})p(\widetilde{{\varvec{\psi }}}_k^{(i)}|\xi _k,{\mathbf {y}}_{1:k-1}) \end{aligned}\nonumber \\ \end{aligned}$$

(74)

where the likelihood PDF and posterior PDF of the modified time-varying process bias are calculated by

$$\begin{aligned}&p(\widetilde{{\varvec{\psi }}}_k^{(i)}|\xi _k,{\mathbf {y}}_{1:k-1})= N(\widetilde{{\varvec{\psi }}}_k^{(i)};\hat{{\mathbf {q}}}_{k|k-1},\frac{{\varvec{{\Psi }}}_{k|k-1}+\widetilde{{\varvec{{\Psi }}}}_k^{(i)}}{\xi _k}) \end{aligned}$$

(75)

$$\begin{aligned}&p({\varvec{\psi }}_k|\xi _k,{\mathbf {y}}_{1:k-1},\widetilde{{\varvec{\psi }}}_k^{(i)})= N({\varvec{\psi }}_k;\hat{{\mathbf {q}}}_{k|k}^{(i)},\frac{{\varvec{{\Psi }}}_{k|k-1}}{\xi _k}) \end{aligned}$$

(76)

using (72)–(76) , (71) can be reformulated as follows

$$\begin{aligned} \begin{aligned} {\text {log}}q^{(i+1)}({\varvec{\psi }}_k,\xi _k)=&{\text {log}}N(\widetilde{{\varvec{\psi }}}_k^{(i)};\hat{{\mathbf {q}}}_{k|k-1},\frac{{\varvec{{\Psi }}}_{k|k-1}+ \widetilde{{\varvec{{\Psi }}}}_k^{(i)}}{\xi _k})\\&+{\text {log}}N({\varvec{\psi }}_k;\hat{{\mathbf {q}}}_{k|k}^{(i)},\frac{{\varvec{{\Psi }}}_{k|k-1}}{\xi _k})\\&+(0.5n+0.5\omega -1){\text {log}}\xi _k-0.5\omega \xi _k\\&-0.5\xi _k {\text {trace}}[{\mathbf {P}}_{k|k}^{(i)}( \widetilde{{\varvec{{\Psi }}}}_k^{(i)})^{-1}]+c_{\{{\varvec{\psi }}_k,\xi _k\}} \end{aligned} \end{aligned}$$

(77)

Appendix D: Calculation of the Complex Necessary Expectation

The expectation ${\mathbf {A}}_k^{(i)}$ in (27) can be calculated as follows

$$\begin{aligned} \begin{aligned} {\mathbf {A}}_k^{(i)}=&E_{{\mathbf {P}}_{k|k-1}}^{(i)}[({\mathbf {x}}_k-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)({\mathbf {x}}_k-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)^T]\\ =&E^{(i)}[({\mathbf {x}}_k-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-\hat{{\mathbf {q}}}_{k|k}^{(i)}+ \hat{{\mathbf {q}}}_{k|k}^{(i)}-{\varvec{\psi }}_k)\\&\times ({\mathbf {x}}_k-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-\hat{{\mathbf {q}}}_{k|k}^{(i)}+\hat{{\mathbf {q}}}_{k|k}^{(i)}-{\varvec{\psi }}_k)^T]\\ =&E^{(i)}[({\mathbf {x}}_k-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-\hat{{\mathbf {q}}}_{k|k}^{(i)}) ({\mathbf {x}}_k-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-\hat{{\mathbf {q}}}_{k|k}^{(i)})^T]+{\mathbf {P}}_{{\varvec{\psi }}_k}^{(i)}\\ =&E^{(i)}[({\mathbf {x}}_k-\hat{{\mathbf {x}}}_{k|k}^{(i)}+\hat{{\mathbf {x}}}_{k|k}^{(i)}- {\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-\hat{{\mathbf {q}}}_{k|k}^{(i)})\\&\times ({\mathbf {x}}_k-\hat{{\mathbf {x}}}_{k|k}^{(i)}+\hat{{\mathbf {x}}}_{k|k}^{(i)}-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}- \hat{{\mathbf {q}}}_{k|k}^{(i)})^T]+{\mathbf {P}}_{{\varvec{\psi }}_k}^{(i)}\\ =&{\mathbf {P}}_{k|k}^{(i)}+(\hat{{\mathbf {x}}}_{k|k}^{(i)}-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-\hat{{\mathbf {q}}}_{k|k}^{(i)})\\&\times (\hat{{\mathbf {x}}}_{k|k}^{(i)}-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}- \hat{{\mathbf {q}}}_{k|k}^{(i)})^T+{\mathbf {P}}_{{\varvec{\psi }}_k}^{(i)} \end{aligned} \end{aligned}$$

(78)

where ${\mathbf {P}}_{{\varvec{\psi }}_k}^{(i+1)}$ is obtained by

$$\begin{aligned} {\mathbf {P}}_{{\varvec{\psi }}_k}^{(i+1)}=\frac{{\varvec{{\Psi }}}_{k|k}^{(i+1)}}{E^{(i+1)}[\xi _k]} \end{aligned}$$

(79)

The expectation ${\mathbf {C}}_k^{(i)}$ in (37) can be calculated as follows

$$\begin{aligned} \begin{aligned} {\mathbf {C}}_k^{(i)}=&E_{({\varvec{\psi }}_k,\xi _k)}^{(i)}[({\mathbf {x}}_k-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)({\mathbf {x}}_k-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)^T]\\ =&E^{(i)}[({\mathbf {x}}_k-\hat{{\mathbf {x}}}_{k|k}^{(i)}+\hat{{\mathbf {x}}}_{k|k}^{(i)} -{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)\\&\times ({\mathbf {x}}_k-\hat{{\mathbf {x}}}_{k|k}^{(i)}+\hat{{\mathbf {x}}}_{k|k}^{(i)} -{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)^T]\\ =&{\mathbf {P}}_{k|k}^{(i)}+[(\hat{{\mathbf {x}}}_{k|k}^{(i)}-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k) (\hat{{\mathbf {x}}}_{k|k}^{(i)}-{\mathbf {F}}_{k-1}\hat{{\mathbf {x}}}_{k-1|k-1}-{\varvec{\psi }}_k)^T] \end{aligned} \end{aligned}$$

(80)

The expectation ${\mathbf {D}}_k^{(i)}$ in (45) can be calculated as follows

$$\begin{aligned} \begin{aligned} {\mathbf {D}}_k^{(i)}=&E_{\lambda _k}^{(i)}[({\mathbf {y}}_k-{\mathbf {H}}_k{\mathbf {x}}_k)({\mathbf {y}}_k-{\mathbf {H}}_k{\mathbf {x}}_k)^T]\\ =&E^{(i)}[({\mathbf {y}}_k-{\mathbf {H}}_k\hat{{\mathbf {x}}}_{k|k}^{(i)}+{\mathbf {H}}_k\hat{{\mathbf {x}}}_{k|k}^{(i)}-{\mathbf {H}}_k{\mathbf {x}}_k)\\&\times ({\mathbf {y}}_k-{\mathbf {H}}_k\hat{{\mathbf {x}}}_{k|k}^{(i)}+{\mathbf {H}}_k\hat{{\mathbf {x}}}_{k|k}^{(i)}-{\mathbf {H}}_k{\mathbf {x}}_k)^T]\\ =&({\mathbf {y}}_k-{\mathbf {H}}_k\hat{{\mathbf {x}}}_{k|k}^{(i)})({\mathbf {y}}_k-{\mathbf {H}}_k\hat{{\mathbf {x}}}_{k|k}^{(i)})^T+{\mathbf {H}}_kE^{(i)}[({\mathbf {x}}_k-\hat{{\mathbf {x}}}_{k|k}^{(i)})({\mathbf {x}}_k-\hat{{\mathbf {x}}}_{k|k}^{(i)})^T]{\mathbf {H}}_k^T\\ =&({\mathbf {y}}_k-{\mathbf {H}}_k\hat{{\mathbf {x}}}_{k|k}^{(i)})({\mathbf {y}}_k-{\mathbf {H}}_k\hat{{\mathbf {x}}}_{k|k}^{(i)})^T+ {\mathbf {H}}_k{\mathbf {P}}_{k|k}^{(i)}{\mathbf {H}}_k^T \end{aligned} \end{aligned}$$

(81)