Student’s t-Kernel-Based Maximum Correntropy Kalman Filter

View full-text article in PMC

. 2022 Feb 21;22(4):1683. doi: 10.3390/s22041683

Algorithm 1: The implementation pseudocode for one time-step of the STKKF.

Inputs: ${\hat{x}}_{k - 1 ∣ k - 1}$ , $P_{k - 1 ∣ k - 1}$ , $Q_{k - 1}$ , $R_{k}$ , v, $σ$ , $ε$ .

Time update:

1. ${\hat{x}}_{k ∣ k - 1} = F_{k} {\hat{x}}_{k - 1 ∣ k - 1}$ .

2. $P_{k ∣ k - 1} = F_{k} P_{k - 1 ∣ k - 1} F_{k}^{T} + Q_{k - 1}$ .

Measurement update:

1. $B_{p} = C h o l (P_{k ∣ k - 1}), B_{r} = C h o l (R_{k})$ , ${\hat{x}}_{k} (l = 0) = {\hat{x}}_{k ∣ k - 1}$ .

2. $e_{x} = B_{p}^{- 1} ({\hat{x}}_{k} (l) - {\hat{x}}_{k ∣ k - 1}), e_{y} = B_{r}^{- 1} (y_{k} - H_{k} {\hat{x}}_{k} (l))$ .

3. $Λ_{x} = diag (\frac{v σ^{2} S_{v, σ} (e_{x, 1})}{v σ^{2} + e_{x, 1}^{2}}, \dots, \frac{v σ^{2} S_{v, σ} (e_{x, n})}{v σ^{2} + e_{x, n}^{2}})$ ,

$Λ_{y} = diag (\frac{v σ^{2} S_{v, σ} (e_{y, 1})}{v σ^{2} + e_{y, 1}^{2}}, \dots, \frac{v σ^{2} S_{v, σ} (e_{y, m})}{v σ^{2} + e_{y, m}^{2}})$ .

4. ${\tilde{P}}_{k ∣ k - 1} = B_{p} Λ_{x}^{- 1} B_{p}^{T}, {\tilde{R}}_{k} = B_{r} Λ_{y}^{- 1} B_{r}^{T}$ .

5. ${\tilde{K}}_{k} = {\tilde{P}}_{k ∣ k - 1} H_{k}^{T} {(H_{k} {\tilde{P}}_{k ∣ k - 1} H_{k}^{T} + {\tilde{R}}_{k})}^{- 1}$ .

6. ${\hat{x}}_{k} (l + 1) = {\hat{x}}_{k ∣ k - 1} + {\tilde{K}}_{k} (y_{k} - H_{k} {\hat{x}}_{k ∣ k - 1})$ .

7. Check $\frac{∥{\hat{x}}_{k} (l + 1) - {\hat{x}}_{k} (l)∥}{∥{\hat{x}}_{k} (l)∥} \leq ε$ , where if the termination condition is met, then set ${\hat{x}}_{k | k} = {\hat{x}}_{k} (l + 1)$ and go to Step 8; otherwise, set $l = l + 1$ and return to Step 2, and continue the next iteration.

8. $P_{k ∣ k} = (I - {\tilde{K}}_{k} H_{k}) P_{k ∣ k - 1} {(I - {\tilde{K}}_{k} H_{k})}^{T} + {\tilde{K}}_{k} R_{k} {\tilde{K}}_{k}^{T}$ .

Outputs: ${\hat{x}}_{k ∣ k}$ , $P_{k ∣ k}$ .