# Tracking Method of GM-APD LiDAR Based on Adaptive Fusion of Intensity Image and Point Cloud
## 3 The Adaptive Fusion of KCF and Kalman Filter
### Target Tracking Method Based on Kalman Filter  
Compared with correlation filtering algorithms, Kalman filter can predict the target position of the next frame by using information such as target velocity and acceleration, which can better estimate the state of random dynamic systems, achieving accurate target position prediction in the case of motion blur and occlusion [21]. Then, the point cloud target obtained by the KCF tracking algorithm is fused with the former one to form the target tracking results of each frame, enhancing the perceptual reliability. As a real-time recursive algorithm, Kalman filter models linear dynamic systems based on state transition equation and observation equation. The state transition equation is shown in Equation (20):  
\[X_{t}=A_{t}X_{t-1}+B_{t}u_{t}+\omega_{t} \tag{20}\]  
where \(X_{t}\) and \(Z_{t}\) are the state of the system at time \(t\) and the observation of that state, respectively; \(A_{t}\) is a state transition matrix, which describes the transition process of the target from \(X_{t-1}\) to state \(X_{t}\); \(u_{t}\) is a input variable; \(B_{t}\) is a gating matrix, which controls \(u_{t}\); \(\omega_{t}\) is a process noise matrix, which is used to simulate the Gaussian distribution noise in the state transition process. The measurement equation is as follows:  
\[Z_{t}=H_{t}X_{t}+v_{t} \tag{21}\]  
where \(H_{t}\) is an observation matrix, where the elements represent the degree of influence of the system state variables on the observed variables; \(v_{t}\) is the observation noise matrix exploited to simulate the Gaussian distribution noise in the observation process. Let the mean of \(\omega\) and \(v\) be 0 and the covariance be \(Q\) and \(R\), respectively, then their probability density distribution is:  
\[\left\{ \begin{array}{l}P(\omega)\sim N(0,Q)\\ P(v)\sim N(0,R)\end{array} \right. \tag{22}\]The Kalman filtering process is divided into two stages: prediction and update, as shown in Figure 3. In the prediction phase, the optimal estimation and state transition matrix of the target state of the previous moment are used to predict the target state of the current moment. Because the influence of the system noise is ignored in this stage, the predicted state will deviate from the actual state. In the update phase, the forecast state is adjusted by the observation data at the current time and the state observation matrix to obtain the optimal estimate of the target state [22]. Stage 1: Prediction  
The state prediction of the target to be tracked at time \(t\) is as follows:  
\[\overline{X}_{t}=A_{t}\hat{X}_{t-1}+\omega_{t} \tag{23}\]  
\[\overline{P}_{t}=A_{t}\hat{P}_{t-1}A_{t}^{T}+Q_{t} \tag{24}\]  
where \(\hat{X}_{t-1}\) is the optimal estimate of the target state at time \(t-1\), \(\overline{X}_{t}\) is the prediction estimate at the current time, \(\hat{P}_{t-1}\) is the covariance matrix corresponding to the optimal estimate of the target state at time \(t-1\), and \(\overline{P}_{t}\) is the covariance matrix corresponding to the estimated value at the current time. Stage 2: Update  
Update the status of the target to be tracked at time \(t\) as follows:  
\[\left\{ \begin{array}{l}K_{t}=\overline{P}_{t}H_{t}^{T}\left(H_{t}\overline{ P}_{t}H_{t}^{T}+R_{t}\right)^{-1}\\ \hat{X}_{t}=\overline{X}_{t}+K_{t}\left(Z_{t}-H_{t}\overline{X}_{t}\right)\\ \hat{P}_{t}=(I_{t}-K_{t}H_{t})\overline{P}_{t}\end{array} \right. \tag{25}\]  
where \(K_{t}\) is the Kalman gain matrix of the optimal estimation; \(Z_{t}\) is the current observation value; \(I_{t}\) is a unit matrix. In the target tracking algorithm proposed in this paper, the target to be tracked is represented as a seven-dimensional vector \(\left(x,y,z,\theta,v_{x},v_{y},v_{z}\right),\) where \(\left(x,y,z\right)\) represents the three-dimensional coordinates of the center point of the vehicle, respectively, \(\left(v_{x},v_{y},v_{z}\right)\) represents the velocity components in the three directions during the driving process, respectively, and \(\theta\) represents the direction of the vehicle.