Algorithm 1 Data Association

Input    $P^{seg}$, $P^{rad}$, C, D Output $P^{sta}$ 1: $P^{seg}$ is projected onto radar plane $\to P^{se{g}^{\prime }}$ 2: $P^{se{g}^{\prime }}$→ KD tree 3: for $p_j^{rad}\in P^{rad}$ do 4:     KNN search( $P^{se{g}^{\prime }},p_j^{rad},R_{search}$ ) $\to P^{nb}$ 5:     for $P_k^{nb}\in P^{nb}$ do 6:        Count point number in $P_k^{nb}\to c$ 7:        Compute centroid of all points in $P_k^{se{g}^{\prime }}\to e_k$ 8:        Compute distance between $p_j^{rad}$ and $e_k\to d$ 9:        if$c>C$ && $d<D$ then 10:          $P^{seg}=P^{seg}-P_k^{seg}$ 11:        end if 12:   end for 13: end for 14: $P^{seg}\to P^{sta}$