A Multilane Tracking Algorithm Using IPDA with Intensity Feature

. 2021 Jan 11;21(2):461. doi: 10.3390/s21020461

Algorithm 1 Control Points Estimator

1:
$▹ Input : κ, N, Ψ$
2:
$▹ Output : A / / Set of control points$
3:
$▹ Sec tion variables : P_{i}, R_{i}, S_{i}, ψ_{i}$
4:
$▹ κ : Vector of curvature$
5:
$▹ ψ_{i} : Sets of all extracted lines in partition i$
6:
$▹ N : Number of partitions$
7:
$▹ P_{i} : Prior noise covariance$
8:
$▹ R_{i} : Measurement noise covariance$
9:
$▹ S_{i} : Set of partitions indexes eliminating i$
10:
fori=1; i<N; i++do
11:
for each $l \in ψ_{i}$ do
12:
for each $S_{i} \in {1 . . N} - {i}$ do
13:
Initialize( $P_{S_{i}}, R_{S_{i}}$ )
14:
${\bar{x}}_{S_{i}, l} \leftarrow$ Predict( $κ, l$ )
15:
${\hat{x}}_{S_{i}, l} \leftarrow$ MAPE( ${\bar{x}}_{l}, ψ_{S_{i}}, R_{S_{i}}, P_{S_{i}}$ ) //Update
16:
end for
17:
$A \leftarrow [\begin{matrix} A & {\hat{x}}_{l} \end{matrix}]$
18:
end for
19:
A=RemoveSimilarCurves(A)
20:
end for