Motion Estimation of Autonomous Vehicle in Noisy Surroundings Using Kalman Filter

Abstract

Self-driving vehicles, autonomous driving, cyber-physical systems are not now unrealistic. Extensive research is being funded by the government and industry popularised this area of research. It has a massive potential to change the way of transportation and mobility. In this paper, we did the real-time state estimation of autonomous car’s position and velocity in an environment interference and noisy sensors. These states are needed to maintain the inter-vehicle gap and cruise system. Avoidance of noise and environment conditions in the controller design can cause a catastrophic condition. Kalman filter algorithms are used here for online computation and updates for real-time position measurements coming from GPS and states over the network. Longitudinal and lateral motion is captured with linear dynamics in 1D and 2D. Verification of the algorithm is done using the simulation for both the cases.

Appendix

For these models, both the predicted and the posterior densities are Gaussian. The mean and covariance of these are given by the Kalman filter equations.

$$\begin{aligned} x_{k}&=p(x_{k|k-1}) =A_{k-1}x_{k-1}+q_{k-1}\sim \mathcal {N}(x_{k};A_{k\text {-}1}x_{k\text {-}1},Q_{k-1}),\\&\qquad q_{k-1}\sim \mathcal {N}(\overline{q}_{k-1}=0,Q_{k-1})\\ y_{k}&=p(y_{k}|x_{k}) =C_{k}x_{k}+r_{k}\sim \mathcal {N}(y_{k};C_{k}x_{k},R_{k}),\\&\qquad r_{k}\sim \mathcal {N}(\overline{r}_{k}=0,R_{k}),x_{0}\sim \mathcal {N}(\overline{x}_{0},P_{0|0}) \end{aligned}$$

The Kalman filter recursively computes,

$$\begin{aligned} p(x_{k}|y_{1:k\text {-}1})&=\mathcal {N}(x_{k};\hat{x}_{k|k\text {-}1},P_{k|k\text {-}1})\quad \text {Prediction step}\\ p(x_{k}|y_{1:k})&=\mathcal {N}(x_{k};\hat{x}_{k|k},P_{k|k})\quad \text {update step} \end{aligned}$$

• Prediction step

  $$\begin{aligned} \hat{x}_{k|k-1}&=A\hat{x}_{k-1|k-1}\\ P_{k|k-1}&=AP_{k-1|k-1}A^{T}+Q_{k-1} \end{aligned}$$

• Update step

• Innovation: $v_{k}=z_{k}^{T}-C\hat{x}_{k|k-1}$

• Innovation covariance: $S_{k}=CP_{k|k-1}C^{T}+R_{k}$

• Kalman gain: $K_{k}=P_{k|k-1}C^{T}S_{k}^{-1}$

• posterior mean: $\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_{k}v_{k}$

• posterior covariance: $P_{k|k}=(I-K_{k}C)P_{k|k-1}=P_{k|k-1}-K_{k}S_{k}K_{k}^{T}$