## 3 Problem Statement and Results
### Kalman Filter for Trajectory Smoothing
#### 3.5.2 Measurements Variability  
At time \(k\) an observation (or measurement) \(z\left(k\right)\) of the true state \(x\left(k\right)\) is made according to:  
\[\mathbf{z}\left(k\right)=\mathbf{H}\cdot\mathbf{x}\left(k\right)+\mathbf{v} \left(k\right) \tag{11}\]  
where \(\mathbf{H}\) is the observation model that maps the true state space onto the observed space; and \(\mathbf{v}\left(k\right)\) is the observation noise, which is assumed to be zero mean Gaussian white noise with covariance \(\mathbf{R}\left(k\right)\):  
\[\mathbf{v}\left(k\right)\sim N\left(0,\mathbf{R}\left(k\right)\right) \tag{12}\]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 \(\mathbf{w}\left(k\right)\) (Gaussian noise process of mean 0 and covariance \(\mathbf{Q}\left(k\right)\), Equation (10)). Furthermore, we consider that there exists some sort of measurement sensing error, represented by variable \(\mathbf{v}\left(k\right)\) (Gaussian noise process of mean 0 and covariance \(\mathbf{R}\left(k\right)\), Equation (12)). The position and speed in the extended System (9) is described by:  
\[\mathbf{x}\left(k\right)=\left[ \begin{array}{c}x\\ \dot{x}\end{array} \right] \tag{13}\]  
with \(\dot{x}\) being the derivative of lateral position with respect to time (speed \(v\left(k\right)\)). Let us now express the \(\mathbf{F}\), \(\mathbf{B}\) and \(\mathbf{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:  
\[\mathbf{F}=\left[ \begin{array}{cc}1&\Delta t\\ 0&1\end{array} \right]\ \ \mathbf{B}=\left[ \begin{array}{c}\Delta t^{2}\\ \Delta t\end{array} \right]\ \ \mathbf{H}=\left[ \begin{array}{cc}1&0\end{array} \right] \tag{14}\]  
Focusing on the covariance matrices regarding process and measurement noise, we get:  
\[\mathbf{Q}=\mathbf{B}\mathbf{B}^{\mathrm{T}}\sigma_{x}^{2}=\left[ \begin{array} []{cc}\Delta t^{4}&\Delta t^{3}\\ \Delta t^{3}&\Delta t^{2}\end{array} \right]\sigma_{x}^{2} \tag{15}\]  
\[\mathbf{R}=\mathrm{E}\left[\mathbf{v}\left(k\right)\mathbf{v}^{\mathrm{T}} \left(k\right)\right]=\left[\sigma_{z}^{2}\right] \tag{16}\]  
From this set of Expressions we are now capable of evaluating how different values of the measurement variance \(\sigma_{z}^{2}\) and the state dispersion variance \(\sigma_{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:  
1. **Filtering**. By means of this process, the Kalman Filter predicts the new values \(\mathbf{x}\left(k\right)\) of the states taking into account the states' history until the instant \(\left(k-1\right)\). (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.) 2. **Smoothing**. In this second process, the Kalman Filter estimates the new values \(\mathbf{x}\left(k\right)\) of the states taking into account, apart from the states' history until the instant \(\left(k-1\right)\), the current measurement of the states: \(\mathbf{z}\left(k\right)\). 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.