As seen above, the RSU data is not error-free. Also, there are parts of measurements where the RSU cannot see the vehicle and it does not report any data during that time. This can occur due to a shadowing effect or other disturbances.

In order to tackle those problems, a Kalman filter has been introduced for the RSU data. A Kalman filter can produce a statistically optimal path estimate for some given noisy path observations given that the process model, process noise and measurement noise are known or can be estimated [17].

### Dynamic model

To describe the vehicle dynamics, we use a constant velocity model (CV) ([18], p. 338), as depicted below:

\begin{array}{ll}{x}_{k}& =A{x}_{k-1}+B{u}_{k-1}+{w}_{k-1}\phantom{\rule{2em}{0ex}}\end{array}

(1)

\begin{array}{ll}{z}_{k}& =H{x}_{k}+{\nu}_{k},\phantom{\rule{2em}{0ex}}\end{array}

(2)

where *x*_{
k
}= [*p*_{
k
}*v*_{
k
}]^{T} is the state of the system, *u*_{
k
} is an optional control input, *w*_{
k
} is the process noise, *z*_{
k
} is the measurement, *ν*_{
k
} is the measurement noise and

\begin{array}{ll}A& =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right],B=0\phantom{\rule{2em}{0ex}}\end{array}

(3)

\begin{array}{ll}H& =\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]=I,\phantom{\rule{2em}{0ex}}\end{array}

(4)

where *Δ* *t* is the time between each measurement observation. Moreover, the process noise *w*_{
k
} and measurement noise *ν*_{
k
} are assumed to be normally distributed with mean *μ* = 0 and covariance {\sigma}_{w}^{2}=Q and {\sigma}_{\nu}^{2}=R[19]. *H* is the identity matrix since there are measurements for both components of the state (position *p*_{
k
} and velocity *v*_{
k
}), and it is assumed that these measurements are independent.

### Implementation

The Kalman filter implementation utilizes a recursive algorithm which consists of a time update, where the next state of the system is ‘predicted’, and a measurement update, where the earlier prediction is ‘corrected’.

During the time update, the following operations are performed:

\begin{array}{ll}{\widehat{x}}_{k}^{-}& =A{\widehat{x}}_{k-1}\phantom{\rule{2em}{0ex}}\end{array}

(5)

\begin{array}{ll}{P}_{k}^{-}& ={\mathit{\text{AP}}}_{k-1}{A}^{T}+Q.\phantom{\rule{2em}{0ex}}\end{array}

(6)

Also, when there is a measurement observation at a certain time slot, a measurement update is being performed with the following operations:

\begin{array}{ll}{K}_{k}& ={P}_{k}^{-}{({P}_{k}^{-}+R)}^{-1}\phantom{\rule{2em}{0ex}}\end{array}

(7)

\begin{array}{ll}{\widehat{x}}_{k}& ={\widehat{x}}_{k}^{-}+{K}_{k}\left({z}_{k}-{\widehat{x}}_{k}^{-}\right)\phantom{\rule{2em}{0ex}}\end{array}

(8)

\begin{array}{ll}{P}_{k}& =\left(I-{K}_{k}\right){P}_{k}^{-}.\phantom{\rule{2em}{0ex}}\end{array}

(9)

If a measurement observation is missing, then the same operations are being performed, but with Kalman gain set to zero,

\begin{array}{ll}{K}_{k}& =0\phantom{\rule{2em}{0ex}}\end{array}

(10)

\begin{array}{ll}{\widehat{x}}_{k}& ={\widehat{x}}_{k}^{-}\phantom{\rule{2em}{0ex}}\end{array}

(11)

\begin{array}{ll}{P}_{k}& ={P}_{k}^{-}.\phantom{\rule{2em}{0ex}}\end{array}

(12)

Here, {\widehat{x}}_{k}^{-} and {P}_{k}^{-} are priori (before measurement update) and {\widehat{x}}_{k} and *P*_{
k
} are posteriori (after measurement update) state estimate and error covariance, respectively. *K*_{
k
} is the Kalman gain. *B* and *H* are not seen in the equations above since *B* = 0 and *H* = *I*.

### Parameter estimation

The process noise covariance *Q*, measurement noise covariance *R*, initial error covariance *P*_{0} and initial state vector {\widehat{x}}_{0}={\left[{\widehat{p}}_{0}\phantom{\rule{1em}{0ex}}{\widehat{v}}_{0}\right]}^{T} are Kalman filter parameters that have to be estimated.

The process noise is often unknown and that complicates the estimation of the filter parameters. However, since the GPS data is much more accurate than the RSU data, it is assumed that the GPS data is error-free in order to get an acceptable filter parameter estimation. Also, as Welch and Bishop [19] state, ‘*Often times, superior filter performance (statistically speaking) can be obtained by tuning the filter parameters Q and R*’.

*Q* and *R*

The process noise covariance *Q* was calculated from the distribution of the GPS data noise and tuned in a way that minimizes the root mean square measurement error (RMSE):

\begin{array}{ll}{Q}_{x}& =1{0}^{-3}\left[\begin{array}{cc}5.4212442813& 0\\ 0& 8.1657541509\end{array}\right]\phantom{\rule{2em}{0ex}}\end{array}

(13)

\begin{array}{ll}{Q}_{y}& =1{0}^{-5}\left[\begin{array}{cc}7.0235962884& 0\\ 0& 0.0130178705\end{array}\right].\phantom{\rule{2em}{0ex}}\end{array}

(14)

The measurement noise covariance *R* was similarly calculated by looking at the distribution of the error between the GPS and RSU data:

\begin{array}{ll}{R}_{x}& =\left[\begin{array}{cc}2.4824824996& 0\\ 0& 6.3782090266\end{array}\right]\phantom{\rule{2em}{0ex}}\end{array}

(15)

\begin{array}{ll}{R}_{y}& =1{0}^{-1}\left[\begin{array}{cc}1.4246746491& 0\\ 0& 0.1792773482\end{array}\right].\phantom{\rule{2em}{0ex}}\end{array}

(16)

Both distributions were assumed to be Gaussian.

#### Initial error covariance *P*_{0}

The initial error covariance *P*_{0} was set equal to the process noise covariance *Q* and tuned by a factor that minimizes the root mean square measurement error (RMSE):

\begin{array}{ll}{P}_{0}^{x}& =\left[\begin{array}{cc}50& 0\\ 0& 6000\end{array}\right]{Q}_{x}\phantom{\rule{2em}{0ex}}\end{array}

(17)

\begin{array}{ll}{P}_{0}^{y}& =\left[\begin{array}{cc}200& 0\\ 0& 4000\end{array}\right]{Q}_{y}.\phantom{\rule{2em}{0ex}}\end{array}

(18)

#### Initial state vector{\widehat{x}}_{0}

The initial state vector contains two components; the initial position {\widehat{p}}_{0} and the initial velocity {\widehat{v}}_{0}. The initial position was set equal to the first measurement observation {\widehat{p}}_{0}={z}_{0} and the initial velocity was set equal to the mean velocity for each coordinate; {\widehat{v}}_{0}^{x}=-10.37 m/s and {\widehat{v}}_{0}^{y}=0.06 m/s.

### Results

The aim with Kalman filtering was to smoothen and predict the path for gaps in the RSU data. As it can be seen in Figures 11, 12 and 13, the goal has been reached.

The RMS measurement error for the X coordinate has decreased from RMSE_{
x
}= 248 cm to RMSE_{
x
}= 146 cm. The RMS measurement error for the Y coordinate has remained the same at RMSE_{
y
}= 38 cm. However, for most of the measurements, as the one depicted in Figures 11 and 12, the error has decreased. It has increased for a handful of measurements which brings the RMSE up.