Performance Enhancement of INS and UWB Fusion Positioning Method Based on Two-Level Error Model

. 2023 Jan 4;23(2):557. doi: 10.3390/s23020557

Algorithm 1: Two-Level Error Model EKF

State Variables:

x = {[p, v, q, δ p, δ v, δ q, δ a, δ w]}^{T}

Initialization:

x_{0}, P_{0}

Input:

x_{k - 1}, P_{k - 1},^{1} u_{k - 1}, z_{k}

Output:

{\hat{x}}_{k}, {\hat{P}}_{k}

1 : a_{k - 1} \leftarrow a_{m}^{k - 1} + δ a_{k - 1}

; w_{k - 1} \leftarrow w_{m}^{k - 1} + δ w_{k - 1}

;

u_{k - 1}^{+} \leftarrow {[a_{k - 1}, w_{k - 1}]}^{T}

2 : {\hat{x}}_{k}^{-} \leftarrow f_{k - 1} (x_{k - 1}, u_{k - 1}^{+})

3 : P_{k}^{-} \leftarrow F_{k - 1} P_{k - 1} F_{k - 1}^{T} + G_{k - 1}^{2} Q_{k - 1} G_{k - 1}^{T}

4 : K_{k} \leftarrow P_{k}^{-} H_{k}^{T} {(H_{k} P_{k}^{-} H_{k}^{T} +^{3} R_{k})}^{- 1}

5 : {\hat{x}}_{k}^{+} \leftarrow {\hat{x}}_{k}^{-} + K_{k} [z_{k} - h_{k} ({\hat{x}}_{k}^{-}, 0)]

6 : {\hat{P}}_{k} \leftarrow (I - K_{k} H_{k}) P_{k}^{-}

7 : {\hat{p}}_{k} \leftarrow {\hat{p}}_{k}^{+} + δ {\hat{p}}_{k}^{+}

; {\hat{v}}_{k} \leftarrow {\hat{v}}_{k}^{+} + δ {\hat{v}}_{k}^{+}

; {\hat{q}}_{k} \leftarrow {\hat{q}}_{k}^{+} \otimes δ {\hat{q}}_{k}^{+}

8 : {\hat{x}}_{k} \leftarrow {[{\hat{p}}_{k}, {\hat{v}}_{k}, {\hat{q}}_{k}, δ {\hat{p}}_{k}^{+}, δ {\hat{v}}_{k}^{+}, δ {\hat{q}}_{k}^{+}, δ {\hat{a}}_{k}^{+}, δ {\hat{w}}_{k}^{+}]}^{T}

9 : R e t u r n : {\hat{x}}_{k}, {\hat{P}}_{k}