Extended Kalman Filter-Based Vehicle Tracking Using Uniform Planar Array for Vehicle Platoon Systems

. 2024 Apr 7;24(7):2351. doi: 10.3390/s24072351

Algorithm 1 Vehicle tracking algorithm for driving platoon.

I. Initialization

1: Initial state vector, ${\hat{t}}_{u}^{0} = t_{u}^{0} + e_{u}^{ϵ}$ with $t_{u}^{0} = {[x_{u}^{0}, v_{u}^{0}]}^{T}, u \in {1, 2}$

2: Initial difference vector, ${\hat{t}}_{d}^{0} = {\hat{t}}_{2}^{0} - {\hat{t}}_{1}^{0} = {[Δ_{x}^{0}, Δ_{y}]}^{T} + (e_{2}^{ϵ} - e_{1}^{ϵ})$

3: Initial covariance matrix, ${\hat{Q}}_{u}^{0} = {(σ_{ϵ} T_{s})}^{2} t_{u}^{0} {(t_{u}^{0})}^{T}$

II. RF chain allocation, $(N_{1}, N_{2})$ , in Section 4.2

III. State prediction and estimation for primary vehicle using $N_{1}$ RF chains

4: Predict state vector and covariance, ${\hat{t}}_{1}^{ℓ | ℓ - 1} = A {\hat{t}}_{1}^{ℓ - 1} & {\hat{Q}}_{1}^{ℓ | ℓ - 1} = A {\hat{Q}}_{1}^{ℓ - 1} A^{T} + Q_{b}$

5: Update state vector, ${\hat{t}}_{1}^{ℓ} = {\hat{t}}_{1}^{ℓ | ℓ - 1} + {\tilde{K}}_{1} ({\tilde{r}}_{1} - {\tilde{Z}}_{1} (\sqrt{ρ_{1}} {\hat{h}}_{1}^{ℓ | ℓ - 1} + \sqrt{ρ_{2}} {\hat{h}}_{2}^{ℓ | ℓ - 1}))$

6: Update covariance matrix, ${\hat{Q}}_{1}^{ℓ} = (I_{2} - \sqrt{ρ_{1}} {\tilde{K}}_{1} {\tilde{Z}}_{1} {\tilde{D}}_{1}) {\hat{Q}}_{1}^{ℓ | ℓ - 1}$

IV. State prediction and estimation for secondary vehicle using $N_{2}$ RF chains

7: Predict difference vector and covariance, ${\hat{t}}_{d}^{ℓ | ℓ - 1} = A {\hat{t}}_{d}^{ℓ - 1} & {\hat{Q}}_{d}^{ℓ | ℓ - 1} = A {\hat{Q}}_{d}^{ℓ - 1} A^{T} + Q_{c}$

8: Predict state vector and covariance, ${\hat{t}}_{2}^{ℓ | ℓ - 1} = {\hat{t}}_{1}^{ℓ} + {\hat{t}}_{d}^{ℓ | ℓ - 1} & {\hat{Q}}_{2}^{ℓ | ℓ - 1} = {\hat{Q}}_{1}^{ℓ} + {\hat{Q}}_{d}^{ℓ | ℓ - 1}$

9: Update state vector, ${\hat{t}}_{2}^{ℓ} = {\hat{t}}_{2}^{ℓ | ℓ - 1} + {\tilde{K}}_{2} ({\tilde{r}}_{2} - {\tilde{Z}}_{2} (\sqrt{ρ_{1}} {\hat{h}}_{1}^{ℓ} + \sqrt{ρ_{2}} {\hat{h}}_{2}^{ℓ | ℓ - 1}))$

10: Update covariance matrix, ${\hat{Q}}_{2}^{ℓ} = (I_{2} - \sqrt{ρ_{2}} {\tilde{K}}_{2} {\tilde{Z}}_{2} {\tilde{D}}_{2}) ({\hat{Q}}_{1}^{ℓ} + {\hat{Q}}_{d}^{ℓ | ℓ - 1})$

V. State difference estimation

11: Update state difference vector, ${\hat{t}}_{d}^{ℓ} = {\hat{t}}_{2}^{ℓ} - {\hat{t}}_{1}^{ℓ}$

12: Update covariance matrix, ${\hat{Q}}_{d}^{ℓ} = {\hat{Q}}_{2}^{ℓ} - {\hat{Q}}_{1}^{ℓ}$