A Novel Student’s t-based Kalman Filter with Colored Measurement Noise

Abstract

In this paper, a novel robust Student’s t-based Kalman filter (RSTKF) is proposed to solve the problem of a linear system with heavy-tailed process and measurement noises (HPMN) and colored measurement noise (CMN). The above problem is transformed into the filtering problem of a linear system with HPMN and white measurement noise after using the measurement differencing method and state augmentation approach. The augmentation state vector, the scale matrix and the auxiliary random variables are jointly estimated based on the variational Bayesian approach. Simulation results are provided to demonstrate the superiority of the proposed RSTKF by comparing with the existing filtering algorithms for systems with HPMN and CMN.

Appendix

If the joint PDF of $\zeta _k^{c(i+1)}$ and $\mathbf {y}_k$ conditioned on $\mathbf {y}_{1:k-1}$, i.e., $p({{\zeta }}_{k}^{c(i+1)},\mathbf {y}_k|\mathbf {y}_{k-1})$ is Gaussian, $p(\mathbf {x}_{k}^{c(i+1)}|\mathbf {y}_{k})$ and $p(\mathbf {x}_{k-1}^{c(i+1)}|\mathbf {y}_{k})$, respectively, can be computed as Gaussian with mean $\hat{\mathbf {x}}_{k|k}^{c(i+1)}$ and corresponding covariance matrix $\mathbf {P}_{k|k}^{c(i+1)}$ can be computed as Gaussian with mean $\hat{\mathbf {x}}_{k-1|k}^{c(i+1)}$ and corresponding covariance matrix $\mathbf {P}_{k-1|k}^{c(i+1)}$, and the covariance matrix $\mathbf {P}_{k-1,k|k}^{c(i+1)}$ as the unified form:

$$\begin{aligned} \mathbf {P}_{k|k-1}^{c\mathrm {xy}(i+1)}= & {} \left[ \mathbf {P}_{k|k-1}^{c(i+1)}\quad \mathbf {P}_{k-1,k|k-1}^{c(i+1){T}}\right] \mathbf {G}_{k}^{{T}} \end{aligned}$$

(75)

$$\begin{aligned} \mathbf {P}_{k|k-1}^{c\mathrm {yy}(i+1)}= & {} \mathbf {G}_{k}\hat{{\varvec{\Phi }}}_{k}^{(i+1)}\mathbf {G}_{k}^{{T}}+\hat{\mathbf {C}}_{k}^{(i+1)} \end{aligned}$$

(76)

$$\begin{aligned} \mathbf {K}_{k}^{c(i+1)}= & {} \mathbf {P}_{k|k-1}^{c\mathrm {xy}(i+1)}\left( \mathbf {P}_{k|k-1}^{c\mathrm {yy}(i+1)}\right) ^{-1} \end{aligned}$$

(77)

$$\begin{aligned} \hat{\mathbf {x}}_{k|k}^{c(i+1)}= & {} \hat{\mathbf {x}}_{k|k-1}^c+\mathbf {K}_{k}^{c(i+1)}\left( \mathbf {y}_{k}-\hat{\mathbf {y}}_{k|k-1}\right) \end{aligned}$$

(78)

$$\begin{aligned} \mathbf {P}_{k|k}^{c(i+1)}= & {} \mathbf {P}_{k|k-1}^{c(i+1)}-\mathbf {K}_{k}^{c(i+1)}\mathbf {P}_{k|k-1}^{c\mathrm {yy}}\left( {\mathbf {K}_{k}^{c(i+1)}}\right) ^{{T}} \end{aligned}$$

(79)

The state estimation vector $\hat{\mathbf {x}}_{k-1|k}^{c(i+1)}$ in (37) and the corresponding estimation error covariance matrix $\mathbf {P}_{k-1|k}^{c(i+1)}$ are formulated as

$$\begin{aligned} \mathbf {P}_{k-1,k|k-1}^{c\mathrm {xy}(i+1)}= & {} \left[ \mathbf {P}_{k-1,k|k-1}^{c(i+1)}\quad \mathbf {P}_{k-1|k-1}^{c(i+1){T}}\right] \mathbf {G}_{k} \end{aligned}$$

(80)

$$\begin{aligned} \mathbf {K}_{k-1}^{cs(i+1)}= & {} \mathbf {P}_{k-1,k|k-1}^{c\mathrm {xy}(i+1)}\left( \mathbf {P}_{k|k-1}^{c\mathrm {yy}(i+1)}\right) ^{-1} \end{aligned}$$

(81)

$$\begin{aligned} \hat{\mathbf {x}}_{k-1|k}^{c(i+1)}= & {} \hat{\mathbf {x}}_{k-1|k-1}^c+\mathbf {K}_{k-1}^{cs(i+1)}\left( \mathbf {y}_{k}-\hat{\mathbf {y}}_{k|k-1}\right) \end{aligned}$$

(82)

$$\begin{aligned} \mathbf {P}_{k-1|k}^{c(i+1)}= & {} \mathbf {P}_{k-1|k-1}^{c(i+1)}-\mathbf {K}_{k-1}^{cs(i+1)}\mathbf {P}_{k|k-1}^{c\mathrm {yy}(i+1)}\left( {\mathbf {K}_{k-1}^{cs(i+1)}}\right) ^{{T}} \end{aligned}$$

(83)

The covariance matrix $\mathbf {P}_{k-1,k|k}^{c(i+1)}$ in (38) is given by

$$\begin{aligned} \mathbf {P}_{k-1,k|k}^{c(i+1)}=\mathbf {P}_{k-1,k|k-1}^{c(i+1)}-\mathbf {K}_{k-1}^{cs(i+1)}\mathbf {P}_{k|k-1}^{c\mathrm {yy}(i+1)}\left( {\mathbf {K}_{k}^{c(i+1)}}\right) ^{{T}} \end{aligned}$$

(84)

Proof

Since the joint PDF of $\zeta _k^{c(i+1)}$ and $\mathbf {y}_k$ conditioned on $\mathbf {y}_{1:k-1}$ is Gaussian, thus the PDF of $\mathbf {y}_k$ conditioned on $\mathbf {y}_{1:k-1}$ is also Gaussian. Then $p({{\zeta }}_{k}^{c(i+1)},\mathbf {y}_k|\mathbf {y}_{k-1})$ and $p(\mathbf {y}|\mathbf {y}_{k-1})$ can be formulated as

$$\begin{aligned} p({{\zeta }}_{k}^{c(i+1)},\mathbf {y}_k|\mathbf {y}_{k-1})= & {} \mathbf {N}\left( \left[ \begin{array}{cc} {{\zeta }}_{k}^{c(i+1)} \\ \mathbf {y}_k \end{array}\right. \right] ;\left[ \begin{array}{cc} \hat{{{\zeta }}}_{k|k-1}^{c(i+1)} \\ \hat{\mathbf {y}}_{k|k-1} \end{array}\right] , \left. \left[ \begin{array}{cc} \mathbf {P}_{k|k-1}^{c{{\zeta \zeta }}(i+1)} &{} \mathbf {P}_{k-1,k|k-1}^{c\zeta \mathrm {y}} \\ (\mathbf {P}_{k-1,k|k-1}^{c\zeta \mathrm {y}})^{\mathbf {T}} &{}\mathbf {P}_{k|k-1}^{c\mathrm {yy}} \end{array}\right] \right) \end{aligned}$$

(85)

$$\begin{aligned} p(\mathbf {y}_k|\mathbf {y}_{k-1})= & {} \mathbf {N}(\mathbf {y};\hat{\mathbf {y}}_{k|k-1},\mathbf {P}_{k|k-1}^{c\mathrm {yy}}) \end{aligned}$$

(86)

According to Bayes’ rule, we have

$$\begin{aligned} p({{\zeta }}_{k}^{c(i+1)}|\mathbf {y}_{k})=\frac{p({{\zeta }}_{k}^{c(i+1)},\mathbf {y}_k|\mathbf {y}_{k-1})}{p(\mathbf {y}_k|\mathbf {y}_{k-1})} \end{aligned}$$

(87)

Let ${\Sigma }= \left[ \begin{array}{cc} \mathbf {P}_{k|k-1}^{c{{\zeta \zeta }}(i+1)} &{} \mathbf {P}_{k-1,k|k-1}^{c\zeta \mathrm {y}} \\ (\mathbf {P}_{k-1,k|k-1}^{c\zeta \mathrm {y}})^{\mathrm {T}} &{}\mathbf {P}_{k|k-1}^{c\mathrm {yy}} \end{array}\right]$, we obtain

$$\begin{aligned} p({{\zeta }}_{k}^{c(i+1)},\mathbf {y}_k|\mathbf {y}_{k-1})=\frac{1}{\sqrt{|2\pi {\Sigma }|}}\mathrm {exp}\left( -\frac{1}{2}\left[ (\hat{{{\zeta }}}_{k|k-1}^{c(i+1)})^{T}\right. \right. \left. (\hat{\mathbf {y}}_{k|k-1})^{T}\right] \left. {\Sigma }^{-1}\left[ \begin{array}{cc} \hat{{{\zeta }}}_{k|k-1}^{c(i+1)} \\ \hat{\mathbf {y}}_{k|k-1} \end{array}\right] \right) \end{aligned}$$

(88)

According to (88), we rewrite ${\Sigma }$ as follows:

$$\begin{aligned} {\Sigma }= \left[ \begin{array}{cc} \mathbf {I}_{2n} &{} \mathbf {K}_{k}^{c\zeta (i+1)} \\ \mathbf {0}_{m\times 2n} &{} \mathbf {I}_{m} \end{array}\right] \left[ \begin{array}{cc} \hat{{\varvec{\Phi }}}_{k}^{(i+1)} &{} \mathbf {0}_{2n\times m} \\ \mathbf {0}_{m\times 2n} &{}\mathbf {P}_{k-1|k-1}^{c\mathrm {yy}} \end{array}\right] \left[ \begin{array}{cc} \mathbf {I}_{2n} &{} \mathbf {0}_{2n\times m} \\ (\mathbf {K}_{k}^{c\zeta (i+1)})^{\mathrm{T}} &{} \mathbf {I}_{m} \end{array}\right] \end{aligned}$$

(89)

where $\mathbf {K}_{k}^{c\zeta (i+1)}$, ${\varvec{\Phi }}_{k|k}^{(i+1)}$ and ${\varvec{\Phi }}_{k|k}^{(i+1)}$ are given by

$$\begin{aligned} \mathbf {K}_{k}^{c\zeta (i+1)}= & {} \mathbf {P}_{k|k-1}^{c\zeta {\mathrm {y}}}\left( \mathbf {P}_{k|k-1}^{c\mathrm {yy}}\right) ^{-1} \end{aligned}$$

(90)

$$\begin{aligned} \mathbf {P}_{k|k-1}^{c\mathrm {yy}}= & {} \mathbf {G}_{k}\hat{{\varvec{\Phi }}}_{k}^{(i+1)}\mathbf {G}_{k}^{{T}}+\hat{\mathbf {C}}_{k}^{(i+1)} \end{aligned}$$

(91)

$$\begin{aligned} {\varvec{\Phi }}_{k|k}^{(i+1)}= & {} \hat{{\varvec{\Phi }}}_{k}^{(i+1)}-\mathbf {K}_{k}^{c\zeta (i+1)}\mathbf {P}_{k|k-1}^{c\mathrm {yy}}(\mathbf {K}_{k}^{c\zeta (i+1)})^{T} \end{aligned}$$

(92)

$|{\Sigma }|$ and ${\Sigma }^{-1}$ can be obtained as follows, respectively:

$$\begin{aligned} \left| {\Sigma }\right|= & {} \left| {\varvec{\Phi }}_{k|k}^{(i+1)}\right| \left| \mathbf {P}_{k|k-1}^{c\mathrm {yy}}\right| \end{aligned}$$

(93)

$$\begin{aligned} {\Sigma }^{-1}= & {} \left[ \begin{array}{cc} \mathbf {I}_{2n} &{} \mathbf {0}_{2n\times m} \\ -(\mathbf {K}_{k}^{c\zeta (i+1)})^{\mathrm{T}} &{} \mathbf {I}_{m} \end{array}\right] \left[ \begin{array}{cc} ({\varvec{\Phi }}_{k|k}^{(i+1)})^{-1} &{} \mathbf {0}_{2n\times m} \\ \mathbf {0}_{m\times 2n} &{}(\mathbf {P}_{k-1|k-1}^{c\mathrm {yy}})^{-1} \end{array}\right] \left[ \begin{array}{cc} \mathbf {I}_{2n} &{} -\mathbf {K}_{k}^{c\zeta (i+1)} \\ \mathbf {0}_{m\times 2n} &{} \mathbf {I}_{m} \end{array}\right] \nonumber \\ \end{aligned}$$

(94)

Substituting (92) and (93) into (89), and with the predicted measurement PDF $p(\mathbf {y}_k|\mathbf {y}_{k-1})$ formulated in (86), we obtain

$$\begin{aligned}&p({{\zeta }}_{k}^{c(i+1)},\mathbf {y}_k|\mathbf {y}_{k-1})=\frac{1}{\sqrt{\left| 2\pi {\varvec{\Phi }}_{k|k}^{(i+1)}\right| \left| 2\pi \mathbf {P}_{k|k-1}^{c\mathrm {yy}}\right| }}\mathrm {exp}\left\{ -\frac{1}{2}\left( \tilde{{{\zeta }}}_{k|k-1}^{c(i+1)}-\mathbf {K}_{k}^{c\zeta (i+1)}\right. \right. \nonumber \\&\qquad \times \left. \tilde{\mathbf {y}}_{k|k-1}\right) ^{T}\left. {{\varvec{\Phi }}_{k|k}^{(i+1)}}^{-1}\right. \left( \tilde{{{\zeta }}}_{k|k-1}^{c(i+1)}\right. \left. \left. -\mathbf {K}_{k}^{c\zeta (i+1)}\tilde{\mathbf {y}}_{k|k-1}\right) -\frac{1}{2}\tilde{\mathbf {y}}_{k|k-1}^{T}\mathbf {P}_{k|k-1}^{c\mathrm {yy}}\tilde{\mathbf {y}}_{k|k-1}\right\} \nonumber \\&\quad =\mathrm {N}\left( {{\zeta }}_{k}^{c(i+1)};\hat{{{\zeta }}}_{k|k}^{c(i+1)},{\varvec{\Phi }}_{k|k}^{(i+1)}\right) p(\mathbf {y}_k|\mathbf {y}_{k-1}) \end{aligned}$$

(95)

where $\hat{{{\zeta }}}_{k|k}^{c(i+1)}=\hat{{{\zeta }}}_{k|k-1}^{c(i+1)}-\mathbf {K}_{k}^{c\zeta (i+1)}(\mathbf {y}_{k}-\hat{\mathbf {y}}_{k|k-1})$. Employing (94) into (87), we obtain

$$\begin{aligned} p({{\zeta }}_{k}^{c(i+1)}|\mathbf {y}_{k})=\mathrm {N}\left( {{\zeta }}_{k}^{c(i+1)};\hat{{{\zeta }}}_{k|k}^{c(i+1)},{\varvec{\Phi }}_{k|k}^{(i+1)}\right) \end{aligned}$$

(96)

Substituting (37) and (38) into (95), the mean vector $\hat{\mathbf {x}}_{k|k}^{c(i+1)}$ and corresponding covariance matrix $\mathbf {P}_{k|k}^{c(i+1)}$ and the mean vector $\hat{\mathbf {x}}_{k-1|k}^{c(i+1)}$ and corresponding covariance matrix $\mathbf {P}_{k-1|k}^{c(i+1)}$, and the covariance matrix $\mathbf {P}_{k-1,k|k}^{c(i+1)}$ are formulated in (75)–(84). $\square$