An Improved SINS Alignment Method Based on Adaptive Cubature Kalman Filter

. 2019 Dec 13;19(24):5509. doi: 10.3390/s19245509

Algorithm 1: One-step of the proposed adaptive cubature Kalman filter.

Inputs:

{\hat{x}}_{k - 1 | k - 1}

P_{k - 1 | k - 1}

z_{k}

{\hat{u}}_{k - 1 | k - 1}

{\hat{U}}_{k - 1 | k - 1}

{\tilde{Q}}_{k - 1}

τ

ξ

, N.

Time update

1. Calculate cubature points based on

{\hat{x}}_{k - 1 | k - 1}

and

P_{k - 1 | k - 1}

χ_{k ∣ k - 1}^{x (j)} = f (χ_{k - 1}^{(j)}), (j = 1, 2, \dots, 2 n_{x})

{\hat{x}}_{k ∣ k - 1} = \frac{1}{2 n_{x}} \sum_{j = 1}^{2 n_{x}} χ_{k ∣ k - 1}^{x (j)}

{\tilde{P}}_{k ∣ k - 1} = \frac{1}{2 n_{x}} \sum_{j = 1}^{2 n_{x}} [(χ_{k ∣ k - 1}^{x (j)} - {\hat{x}}_{k ∣ k - 1}) {(χ_{k ∣ k - 1}^{x (j)} - {\hat{x}}_{k ∣ k - 1})}^{T}] + g ({\hat{x}}_{k | k - 1}) {\tilde{Q}}_{k - 1} g {({\hat{x}}_{k | k - 1})}^{T}

Iterated measurement update

5. Initialization:

{\hat{x}}_{k | k}^{(0)} = {\hat{x}}_{k | k - 1}

{\hat{P}}_{k | k}^{(0)} = {\tilde{P}}_{k | k - 1}

{\hat{T}}_{k | k - 1} = τ {\tilde{P}}_{k | k - 1}

{\hat{t}}_{k | k - 1} = n_{x} + τ + 1

{\hat{u}}_{k | k - 1} = ξ ({\hat{u}}_{k - 1 | k - 1} - n_{z} - 1) + n_{z} + 1

{\hat{U}}_{k | k - 1} = ξ {\hat{U}}_{k - 1 | k - 1}

For

i = 0 : N - 1

6. Update

q^{(i + 1)} (P_{k | k - 1}) = IW (P_{k | k - 1}; {\hat{t}}_{k}^{(i + 1)}, {\hat{T}}_{k}^{(i + 1)})

{\hat{t}}_{k}^{(i + 1)} = {\hat{t}}_{k | k - 1} + 1

{\hat{T}}_{k}^{(i + 1)} = A_{k}^{(i)} + {\hat{T}}_{k | k - 1}

, where

A_{k}^{(i)} = P_{k | k}^{(i)} + ({\hat{x}}_{k | k}^{(i)} - {\hat{x}}_{k | k - 1}) {({\hat{x}}_{k | k}^{(i)} - {\hat{x}}_{k | k - 1})}^{T}

7. Update

q^{(i + 1)} (R_{k}) = IW (R_{k}; {\hat{u}}_{k}^{(i + 1)}, {\hat{U}}_{k}^{(i + 1)})

{\hat{u}}_{k}^{(i + 1)} = {\hat{u}}_{k | k - 1} + 1

{\hat{U}}_{k}^{(i + 1)} = B_{k}^{(i)} + {\hat{U}}_{k | k - 1}

, where

B_{k}^{(i)} = (z_{k} - H {\hat{x}}_{k | k}^{(i)}) {(z_{k} - H {\hat{x}}_{k | k}^{(i)})}^{T} + H P_{k | k}^{(i)} H^{T}

8. Update

q^{(i + 1)} (x_{k}) = N (x_{k}; {\hat{x}}_{k | k}^{(i + 1)}, {\hat{P}}_{k | k}^{(i + 1)})

{\hat{P}}_{k | k - 1}^{(i + 1)} = {(({\hat{t}}_{k}^{(i + 1)} - n_{x} - 1) {({\hat{T}}_{k}^{(i + 1)})}^{- 1})}^{- 1}

{\hat{R}}_{k}^{(i + 1)} = {(({\hat{u}}_{k}^{(i + 1)} - n_{z} - 1) {({\hat{U}}_{k}^{(i + 1)})}^{- 1})}^{- 1}

9. Calculate the mean and variance of posterior PDF,

K_{k}^{(i + 1)} = {\hat{P}}_{k | k - 1}^{(i + 1)} H^{T} {(H {\hat{P}}_{k | k - 1}^{(i + 1)} H^{T} + {\hat{R}}_{k}^{(i + 1)})}^{- 1}

{\hat{x}}_{k ∣ k}^{(i + 1)} = {\hat{x}}_{k ∣ k - 1} + K_{k}^{(i + 1)} (z_{k} - H {\hat{x}}_{k ∣ k - 1})

{\hat{P}}_{k | k}^{(i + 1)} = {\hat{P}}_{k | k - 1}^{(i + 1)} - K_{k}^{(i + 1)} H {\hat{P}}_{k | k - 1}^{(i + 1)}

End for

10.

{\hat{x}}_{k | k} = {\hat{x}}_{k | k}^{(N)}

P_{k | k} = {\hat{P}}_{k | k}^{(N)}

{\hat{u}}_{k | k} = {\hat{u}}_{k}^{(N)}

{\hat{U}}_{k | k} = {\hat{U}}_{k}^{(N)}

Outputs:

{\hat{x}}_{k | k}

P_{k | k}

{\hat{u}}_{k | k}

{\hat{U}}_{k | k}