## 3 Problem Statement and Results
### Kalman Filter for Trajectory Smoothing
#### 3.5.2 Measurements Variability
In the first set of graphs, we will show how the trajectory of a vehicle (departing from a lateral position \(x=5\) m and having at most \(t_{f}=10\) s to reach the optimum lateral position) is affected by the presence of additive Gaussian noise in the states and in the sensor measurements. We will establish a state dispersion variance \(\sigma_{z}^{2}=0.1\) for all the examples and will focus only on the influence of noise produced by measurement sensors (measurement variance \(\sigma_{z}^{2}\)). We will include a comparison between the True Trajectory (optimum determined trajectory), the measured trajectory (sensed trajectory while executing the maneuver), and the filtered and smoothed trajectory for the two proposed performance measures \(J_{D1}\) and \(J_{D2}\), and for a set of values of the measurement variance \(\sigma_{z}^{2}=\{0.1,1,5\}\). If we now draw our attention to Figure 11, we can fairly understand how measurement noise affects the shape of trajectories by producing a dispersion of the optimum path due to the sensed positions along the course (mild dispersion for \(\sigma_{z}^{2}=0.1\), medium dispersion for \(\sigma_{z}^{2}=1\) and high dispersion for \(\sigma_{z}^{2}=5\)). We can notice that both Filtering and Smoothing reduce the dispersion effect, but Smoothing reduces the still significant variability that the filtered trajectory obtains, by using the measurement of the present instant \(\mathbf{z}\left(k\right)\). Differences between \(J_{D1}\) and \(J_{D2}\), if present, are not clear in Figure 11.  
Now we turn our attention to the evaluation of the Mean Square Error (MSE) between the optimum trajectory, and the measured, filtered and smoothed trajectories for the interval \(\sigma_{z}^{2}\in[0,5]\), for both \(J_{D1}\) and \(J_{D2}\). Due to the variability to which the MSE is subject, we have used regression techniques to represent the Degree-2 polynomial, which averages the MSE evolution during the evaluated interval. This will make the visualization of graphs easier for comparison purposes. On the other hand, we will also represent the averaged evolution (with a Degree-2 polynomial like with MSE) of the lateral distance with respect to the optimum lateral position (which we call LDP) at the last time step \(t_{f}\) in order to quantify how far the trajectory ends from the desired position. The associated graph will represent this magnitude for the same interval \(\sigma_{z}^{2}\in[0,5]\) as in the last case, and will show the percentage, with respect to the total width of the road \(W\), of distance far from the optimum lateral position. Figure 11: _Kalman filter_ effect on trajectories for \(J_{D1}\) and \(J_{D2}\) under measurement noise (\(\sigma_{z}\in\{0.1,1,5\}\)). If we have now a look at Figures 12 and 13, we will see the evolution of both the MSE and LDP for the functional \(J_{D1}\). For the MSE, we can notice that for lower values of the measurement variance \(\sigma_{z}\sim 0.1\), sensing noise does not affect the trajectory remarkably, since the measured, filtered and smoothed paths obtain a very low MSE with respect to the optimum trajectory. As \(\sigma_{z}\) increases, it is easily noticeable how necessary it is to use at least Filtering and, if possible, Smoothing in order to correct the path dispersion introduced  
Figure 13: Percentage of distance with respect to the lateral optimum position under measurement noise (\(\sigma_{z}\in\{0.1,1,5\}\)), for \(J_{D1}\). Figure 12: Degree-2 polynomials for regression of MSE under measurement noise (\(\sigma_{z}\in\{0.1,1,5\}\)), for \(J_{D1}\).