#### 3.5.2. Measurements Variability

At time k an observation (or measurement)

z (

k) of the true state

x (

k) is made according to:

where

**H** is the observation model that maps the true state space onto the observed space; and

v (

k) is the observation noise, which is assumed to be zero mean Gaussian white noise with covariance

**R**(

k):

We can observe that in comparison with the initial proposal of System (1), we now model our problem as a linear system of equations whose state variables are corrupted by additive Gaussian noise, represented by variable **w**(k) (Gaussian noise process of mean 0 and covariance **Q**(k), Equation (10)). Furthermore, we consider that there exists some sort of measurement sensing error, represented by variable **v**(k) (Gaussian noise process of mean 0 and covariance **R**(k), Equation (12)).

The position and speed in the extended System (9) is described by:

with

being the derivative of lateral position with respect to time (speed

v (

k)).

Let us now express the

**F**,

**B** and

**H** matrices by extracting the correspondence of both the controls and the state variables with Equations (10) and (12). After some easy operations we get:

Focusing on the covariance matrices regarding process and measurement noise, we get:

From this set of Expressions we are now capable of evaluating how different values of the measurement variance

σ_{z}^{2 }and the state dispersion variance

σ_{x}^{2 }affect the evolution of trajectories. With the Kalman Filter, it is possible to minimize the counterproductive effect of both noise processes by using two trajectory regeneration procedures enumerated next:

**Filtering.** By means of this process, the Kalman Filter predicts the new values x (k) of the states taking into account the states’ history until the instant (k − 1). (Although the term “Kalman Filter” regards all techniques to reduce the influence of noise on the states of a dynamical system, we must not get confused with Filtering, which, as well as Smoothing, describes a specific procedure to fix or reduce trajectory dispersion caused by any noise process.)

**Smoothing.** In this second process, the Kalman Filter estimates the new values x (k) of the states taking into account, apart from the states’ history until the instant (k − 1), the current measurement of the states: z (k).

We can deduce from the previous two comments that with Smoothing, the estimated trajectory will reduce the dispersion with respect to Filtering, since we count on more updated information to estimate the current position and speed of the vehicle.

Now it is time to graphically visualize some application examples of the previous concepts. 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

σ_{x}^{2 }= 0.1 for all the examples and will focus only on the influence of noise produced by measurement sensors (measurement variance

σ_{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

σ_{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

σ_{z}^{2} = 0.1, medium dispersion for

σ_{z}^{2} = 1 and high dispersion for

σ_{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 z (

k). Differences between

J_{D1} and

J_{D2}, if present, are not clear in

Figure 11.

**Figure 11.**
Kalman filter effect on trajectories for J_{D1} and J_{D2} under measurement noise (σ_{z} = {0.1, 1, 5}).

**Figure 11.**
Kalman filter effect on trajectories for J_{D1} and J_{D2} under measurement noise (σ_{z} = {0.1, 1, 5}).

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 σ_{z}^{2 }∈ [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 σ_{z}^{2 }∈ [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.

If we have now a look at

Figure 12 and

Figure 13, we will see the evolution of both the MSE and LDP for the functional

J_{D1}.

**Figure 12.**
Degree-2 polynomials for regression of MSE under measurement noise (σ_{z} ∈ {0.1, 1, 5}), for J_{D1}.

**Figure 12.**
Degree-2 polynomials for regression of MSE under measurement noise (σ_{z} ∈ {0.1, 1, 5}), for J_{D1}.

**Figure 13.**
Percentage of distance with respect to the lateral optimum position under measurement noise (σ_{z} ∈ {0.1, 1, 5}), for J_{D1}.

**Figure 13.**
Percentage of distance with respect to the lateral optimum position under measurement noise (σ_{z} ∈ {0.1, 1, 5}), for J_{D1}.

For the MSE, we can notice that for lower values of the measurement variance

σ_{z} ∼ 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

σ_{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 by noise. Surprisingly, the LDP shows very similar results for both Filtering and Smoothing, since both tend to reach the optimum final position regardless of the evolution of the trajectory. Analyzing now

Figure 14 and

Figure 15, we can come to the same conclusions as for the

J_{D1} functional. More importantly, we can see that differences between using Filtering and Smoothing for

J_{D1} and

J_{D2} are not as remarkable. This implies that the shape of the traced trajectory does not influence the performance of the Kalman filter.

**Figure 14.**
Degree-2 polynomials for regression of MSE under measurement noise (σ_{z} ∈ {0.1, 1, 5}), for J_{D2}.

**Figure 14.**
Degree-2 polynomials for regression of MSE under measurement noise (σ_{z} ∈ {0.1, 1, 5}), for J_{D2}.

**Figure 15.**
Percentage of distance with respect to the lateral optimum position under measurement noise (σ_{z} ∈ {0.1, 1, 5}), for J_{D2}.

**Figure 15.**
Percentage of distance with respect to the lateral optimum position under measurement noise (σ_{z} ∈ {0.1, 1, 5}), for J_{D2}.