# Real-Time Hybrid Multi-Sensor Fusion Framework for Perception in Autonomous Vehicles
## 4 Hybrid Sensor Fusion Algorithm Overview
### Testing Obstacle Detection and Tracking using Kalman Filtering
#### 4.2.3 Extended Kalman Filtering  
After the semantic road segmentation using our FCNx network, in this section, we show the implementation of single object tracking via our vehicle embedded computer using the traditional Extended Kalman Filter (EKF) algorithm. This is to showcase the real-world implementation of object tracking, predicting and maintaining their states (2-dimensional positions and velocities) over time. State of the object being tracked are: \(P_{x}\), \(P_{y}\) (the object's positions in x and y-direction), and \(V_{x}\), \(V_{y}\) (the object's velocities in x and y-direction). For state tracking, we use a state estimation method called Kalman filtering (KF). A standard KF involves three steps: initialization, prediction, and update. For the prediction step, we use the object's current state \(\hat{x}_{k}\) (2D-position and 2D-velocity) and assume that the object has a constant velocity (CV) motion model, to estimate the state at the next time step \(\hat{x}_{k+1}\) using state transition function \(\mathbf{F}\) in Equation (4). Object covariance \(\mathbf{P}\) represents the state estimate uncertainty in Equation (5). We also model the noise in KF, the state transition noise \(\mathbf{\hat{\xi}}\), motion noise covariance \(\mathbf{Q}\), measurement noise covariance \(\mathbf{R}\). After getting the measurement from our LiDAR or radar sensor, we map the state to the actual measured value of the sensors \(\mathbf{z}\) using the measurement function \(\mathbf{H}\) using Equation (6), we calculate the residual covariance from Equation (7). Figure 11: LiDAR data processing pipeline. Images from left to right: Dense LiDAR point cloud data, downsampled and filtered LiDAR data, segmented point cloud into road and obstacles, clustered obstacles in the segmented point cloud. The center dark spot is the ego vehicle. \[\hat{x}_{k+1}=F\cdot\hat{x}_{k}+\hat{\xi} \tag{4}\]  
\[P_{k+1}=F\cdot P_{k}\cdot F^{T}+Q \tag{5}\]  
\[\hat{y}_{k+1}=\hat{z}_{k+1}-H\cdot\hat{x}_{k+1} \tag{6}\]  
\[\hat{s}_{k+1}=H\cdot P_{k+1}\cdot H^{T}+R \tag{7}\]  
Kalman gain \(K\), which combines the uncertainty of our predicted state \(P_{k+1}\) with the uncertainty of sensor measurements \(S_{k+1}\). Kalman gain will give more weight on the less uncertain value; either the predicted state \(\hat{x}_{k+1}\) or the sensor measurement \(\hat{z}_{k+1}\). We calculate the updated state estimate as well as its covariance from Equations (9) and (10). \[K=P_{k+1}\cdot H^{T}\cdot{S_{k+1}}^{-1} \tag{8}\]  
\[\hat{x}_{k+1}=\hat{x}_{k}+K\cdot\hat{y} \tag{9}\]  
\[P_{k+1}=(I-K\cdot H)\cdot P_{k} \tag{10}\]  
The standard Kalman filter can only be used when our models are linear (motion model or the measurement model.) with the assumption that our data has a Gaussian distribution. When data is Gaussian and motion-based, and measurement models are linear, the output data will also have Gaussian distribution. However, in a system, the motion model or measurement model or both can be nonlinear. When either or both these models do not follow a Gaussian distribution and are nonlinear, the input states will go through a nonlinear transformation; Hence, the standard KF may not converge, and its equations are not valid. In the case of nonlinear models, we have to use a nonlinear state estimator like an Extended Kalman filter (EKF). EKF fixes this problem by linearizing the nonlinear functions (state transition and measurement functions) around the mean of the current state estimate using Taylor series expansion and taking the Jacobian of our functions [40]. In EKF, the system equations are:  
\[\hat{x}_{k+1}=f(\hat{x}_{k},\hat{a}_{k})+\hat{\xi} \tag{11}\]  
\[\hat{z}_{k+1}=h(\hat{x}_{k+1})+\hat{\epsilon} \tag{12}\]  
where \(f(\hat{x}_{k},\hat{a}_{k})\) and \(h(\hat{x}_{k+1})\) are the nonlinear state transition and measurement functions respectively.