.. _kalman-filter:

Kalman Filter
********************************************************************************

Consider a nonlinear system given by the stochastic differential equation

.. math:: \dot{{\bf x}} = {\bf f} ( {\bf x}(t) , t ) + {\bf G} (t) {\bf w} (t)

where

* :math:`{\bf w}` is a white gaussian process. :math:`{\bf w}(t)` ~ :math:`N ( {\bf 0} , {\bf Q}(t) )`
* :math:`{\bf f} ( {\bf x}(t) , t )` is a function vector of dimension :math:`n`
* :math:`{\bf x}(t)` is the state vector :math:`(n \times 1)`
* :math:`{\bf G}` is a known matrix :math:`(n \times s)`

In this example, the plant is represented by the nonlinear model described in :ref:`vehicle-simple-3dof`.

The system model used in the algorithm is based on the plant model. Moreover, we assume that the designer of the Kalman Filter do not possess full knowledge of the characteristic curve of the tire. Thus, the vehicle model of the algorithm is the same used to represent the real vehicle. However, the tire model used is linear (:ref:`tire-linear`). This way, the system model used in the Kalman Filter algorithm is a nonlinear vehicle model with linear tire model.

Defining the tire and nonlinear vehicle models with the default parameters. This model will represent the real vehicle system in the simulation environment.

.. code-block:: matlab
    
    TirePlant = TirePacejka;
    VehiclePlant = VehicleSimpleNonlinear;
    VehiclePlant.tire = TirePlant;
    
Lets simulate the vehicle maneuver to be estimated by the Kalman Filter.

The chosen simulation parameters are

.. code-block:: matlab
    
    T = 6;                              % Total simulation time [s]
    resol = 50;                         % Resolution
    TSPAN = 0:T/resol:T;                % Time span [s]
    
Initializing the simulator and simulating.

.. code-block:: matlab
    
    simulatorPlant = Simulator(VehiclePlant, TSPAN);
    simulatorPlant.dPSI0 = 0.35;
    simulatorPlant.Simulate
    
Retrieving state responses

.. code-block:: matlab
    
    XTPlant = simulatorPlant.XT;
    YTPlant = simulatorPlant.YT;
    PSIPlant = simulatorPlant.PSI;
    vTPlant = simulatorPlant.VEL;
    ALPHATPlant = simulatorPlant.ALPHAT;
    dPSIPlant = simulatorPlant.dPSI;
    
    XOUTPlant = [XTPlant YTPlant PSIPlant vTPlant ALPHATPlant dPSIPlant];
    
Generating the graphics of the maneuver.

.. code-block:: matlab
    
    gPlant = Graphics(simulatorPlant);
    gPlant.TractorColor = 'r';
    gPlant.Frame();
    
.. figure:: ../illustrations/frame/KalmanFilterFrame1.svg
    :align:   center
    :width: 70%

    Leegend

Now, the model used in thhe Kalman Filter algorithm is presented.

Initializing the tire and vehicle models

.. code-block:: matlab
    
    TireModel = TireLinear;
    VehicleModel = VehicleSimpleNonlinear;
    VehicleModel.tire = TireModel;
    
Note that the model used in the KF algorithm is the same as the one used to represent the plant except by the tire model. The plant has a nonlinear tire model while the KF model has a linear model.

Simulating the KF model with the same time span.

.. code-block:: matlab
    
    simulatorModel = Simulator(VehicleModel, TSPAN);
    simulatorModel.dPSI0 = 0.35;
    simulatorModel.Simulate;
    
Retrieving states

.. code-block:: matlab
    
    XTModel = simulatorModel.XT;
    YTModel = simulatorModel.YT;
    PSIModel = simulatorModel.PSI;
    vTModel = simulatorModel.VEL;
    ALPHATModel = simulatorModel.ALPHAT;
    dPSIModel = simulatorModel.dPSI;
    
The maneuver generated by the simplified model (with the same initial conditions) is illustrated below

.. code-block:: matlab
    
    gModel = Graphics(simulatorModel);
    gModel.TractorColor = 'g';
    gModel.Frame();
    
.. figure:: ../illustrations/frame/KalmanFilterFrame2.svg
    :align:   center
    :width: 70%

    Leegend

Comparing state response of both models. The following lines plot the time response of each state of the model.

.. code-block:: matlab
    
    f1 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,XTPlant,'r')
    plot(TSPAN,XTModel,'r--')
    legend('Plant','Model')
    xlabel('Time [s]')
    ylabel('Distance X [m]')
    
    f2 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,YTPlant,'g')
    plot(TSPAN,YTModel,'g--')
    legend('Plant','Model')
    xlabel('Time [s]')
    ylabel('Distance Y [m]')
    
    f3 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,PSIPlant,'b')
    plot(TSPAN,PSIModel,'b--')
    legend('Plant','Model')
    xlabel('Time [s]')
    ylabel('PSI [rad]')
    
    f4 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,vTPlant,'c')
    plot(TSPAN,vTModel,'c--')
    legend('Plant','Model')
    xlabel('Time [s]')
    ylabel('vT [m/s]')
    
    f5 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,ALPHATPlant,'m'),
    plot(TSPAN,ALPHATModel,'m--'),
    legend('Plant','Model')
    xlabel('Time [s]')
    ylabel('ALPHAT [rad/s]')
    
    f6 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,dPSIPlant,'k')
    plot(TSPAN,dPSIModel,'k--')
    legend('Plant','Model')
    xlabel('Time [s]')
    ylabel('dPSI [rad/s]')
    
The generated figures can be seen below.

.. figure:: ../illustrations/plot/KalmanFilterFig1.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig2.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig3.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig4.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig5.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig6.svg
    :align:   center
    :width: 40%

    Leegend

Comparing longitudinal and lateral acceleration. The acceleration of both models are calculated as

.. code-block:: matlab
    
    saidasPlant = [XTPlant YTPlant PSIPlant vTPlant ALPHATPlant dPSIPlant];
    matDerivEstadosPlant = zeros(size(saidasPlant));
    for i = 1:size(saidasPlant,1)
        auxil = simulatorPlant.Vehicle.Model(1,saidasPlant(i,:),TSPAN);
        matDerivEstadosPlant(i,:) = auxil';
    end
    
    dXTPlant = matDerivEstadosPlant(:,1);
    dYTPlant = matDerivEstadosPlant(:,2);
    dPSIPlant = matDerivEstadosPlant(:,3);
    dvTPlant = matDerivEstadosPlant(:,4);
    dALPHATPlant = matDerivEstadosPlant(:,5);
    ddPSIPlant = matDerivEstadosPlant(:,6);
    
    ddXPlant = dvTPlant.*cos(PSIPlant + ALPHATPlant) - vTPlant.*(dPSIPlant + dALPHATPlant).*sin(PSIPlant + ALPHATPlant);
    ddYPlant = dvTPlant.*sin(PSIPlant + ALPHATPlant) + vTPlant.*(dPSIPlant + dALPHATPlant).*cos(PSIPlant + ALPHATPlant);
    
    ACELNumPlant = [(ddXPlant.*cos(PSIPlant) - ddYPlant.*sin(PSIPlant))  (-ddXPlant.*sin(PSIPlant) + ddYPlant.*cos(PSIPlant))];
    
    saidasModel = [XTModel YTModel PSIModel vTModel ALPHATModel dPSIModel];
    matDerivEstadosModel = zeros(size(saidasModel));
    for i = 1:size(saidasModel,1)
        auxil = simulatorModel.Vehicle.Model(1,saidasModel(i,:),TSPAN);
        matDerivEstadosModel(i,:) = auxil';
    end
    
    dXTModel = matDerivEstadosModel(:,1);
    dYTModel = matDerivEstadosModel(:,2);
    dPSIModel = matDerivEstadosModel(:,3);
    dvTModel = matDerivEstadosModel(:,4);
    dALPHATModel = matDerivEstadosModel(:,5);
    ddPSIModel = matDerivEstadosModel(:,6);
    
    ddXModel = dvTModel.*cos(PSIModel + ALPHATModel) - vTModel.*(dPSIModel + dALPHATModel).*sin(PSIModel + ALPHATModel);
    ddYModel = dvTModel.*sin(PSIModel + ALPHATModel) + vTModel.*(dPSIModel + dALPHATModel).*cos(PSIModel + ALPHATModel);
    
    ACELNumModel = [(ddXModel.*cos(PSIModel) - ddYModel.*sin(PSIModel))  (-ddXModel.*sin(PSIModel) + ddYModel.*cos(PSIModel))];
    
The longitudinal and lateral acceleration are plotted with

.. code-block:: matlab
    
    f7 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,ACELNumPlant(:,1),'r')
    plot(TSPAN,ACELNumPlant(:,2),'g')
    plot(TSPAN,ACELNumModel(:,1),'r--')
    plot(TSPAN,ACELNumModel(:,2),'g--')
    xlabel('time [s]')
    ylabel('acc. [m/s]')
    l = legend('AX Plant','AY Plant','AX Model','AY Model');
    set(l,'Location','NorthEast')
    
The resulting comparison is illustrated in the figure below

.. figure:: ../illustrations/plot/KalmanFilterFig7.svg

Comparing the maneuvers of the plant and the model.

.. code-block:: matlab
    
    gPlant.Frame();
    gModel.Frame('nextplot','add');
    
.. figure:: ../illustrations/frame/KalmanFilterFrame3.svg
    :align:   center
    :width: 70%

    Leegend

The general expression of the linearized model is optained using the symbolic processor of Octave/Matlab.

Defining symbols

.. code-block:: matlab
    
    syms XT YT PSI vT ALPHAT dPSI mT IT a b K
    
The tire slip angles are

.. code-block:: matlab
    
    ALPHAF = atan((vT * sin(ALPHAT) + a * dPSI)/(vT * cos(ALPHAT))); % Front
    ALPHAR = atan((vT * sin(ALPHAT) - b * dPSI)/(vT * cos(ALPHAT))); % Rear
    
The Lateral forces

.. code-block:: matlab
    
    FyF = -K*ALPHAF;                    % Front
    FyR = -K*ALPHAR;                    % Rear
    
State equations

.. code-block:: matlab
    
    f1e = vT * cos(ALPHAT + PSI);
    f2e = vT * sin(ALPHAT + PSI);
    f3e = dPSI;
    f4e = (FyF * sin(ALPHAT) + FyR * sin(ALPHAT))/(mT);
    f5e = (FyF * cos(ALPHAT) + FyR * cos(ALPHAT) - mT * vT * dPSI) / (mT * vT);
    f6e = (FyF * a - FyR * b) / IT;
    
    f = [f1e ; f2e ; f3e ; f4e ; f5e ; f6e];
    
State vector

.. code-block:: matlab
    
    States = [XT ; YT ; PSI ; vT ; ALPHAT ; dPSI];
    
The linearized system is written as

.. math:: \dot{\bf x} = {\bf F} {\bf x}

where :math:`{\bf F}` is the dynamic matrix of the linear model obtained after the truncation of the expansion of the Taylor series. Thus, the :math:`{\bf F}` matrix is

.. math:: {\bf F} = \left[ \frac{\partial f_i}{\partial x_j} \right]_{n \times n}

where :math:`i` e :math:`j` indicate the equations and state variables used in the calculations of at the position :math:`(i,j)` of the jacobian matrix.

Calculating the jacobian matrix.

.. code-block:: matlab
    
    F = jacobian(f,States);
    F = simplify(F);
    
Defining a matlab function to retrieve the numerical value of F. This way is faster than using "subs".

.. code-block:: matlab
    
    Ffunc = matlabFunction(F,'Vars',{PSI vT ALPHAT dPSI mT IT a b K});
    
Hypothetically, the measured quantities are:

* Position X
* Position Y
* Yaw rate
* Longitudinal acceleration
* Lateral acceleration

For this, we use:

.. math:: \dot{x} = v_{\rm T} \cos \left( \psi + \alpha_{\rm T} \right)

.. math:: \dot{y} = v_{\rm T} \sin \left( \psi + \alpha_{\rm T} \right)

Longitudinal and lateral acceleration are

.. math:: \ddot{x} = \dot{v}_{\rm T} \cos \left( \psi + \alpha_{\rm T} \right) - v_{\rm T} \left( \dot{\psi} + \dot{\alpha}_{\rm T} \right) \sin \left( \psi + \alpha_{\rm T} \right)

.. math:: \ddot{y} = \dot{v}_{\rm T} \sin \left( \psi + \alpha_{\rm T} \right) + v_{\rm T} \left( \dot{\psi} + \dot{\alpha}_{\rm T} \right) \cos \left( \psi + \alpha_{\rm T} \right)

Thus

.. code-block:: matlab
    
    ddX = f4*cos(PSI + ALPHAT) - vT*(dPSI + f5)*sin(PSI + ALPHAT);
    ddY = f4*sin(PSI + ALPHAT) + vT*(dPSI + f5)*cos(PSI + ALPHAT);
    
The equations above describe the dynamics from :math:`\{ O {\bf i} {\bf j} {\bf k} \}`. In the moving reference frame :math:`\{ O {\bf t}_x {\bf t}_y {\bf t}_z \}` the equations are

.. math:: {\bf a} = \left( \ddot{x} \cos \psi - \ddot{y} \sin \psi \right) {\bf t}_x + \left( - \ddot{x} \sin \psi + \ddot{y} \sin \psi \right) {\bf t}_y

Thus,

.. code-block:: matlab
    
    ACEL = [ddX*cos(PSI) - ddY*sin(PSI) ; -ddX*sin(PSI) + ddY*cos(PSI)];
    ACEL = simplify(ACEL);
    
The nonlinear observation equation is given by

.. math:: {\bf z}_k = {\bf h} ({\bf x}_k) + {\bf v}_k

with :math:`{\bf v}_k $ ~ $ N ( {\bf 0} , {\bf R}_k )`.

The linear version is given by

.. math:: {\bf z}_k = {\bf H} {\bf x}_k + {\bf v}_k

where

.. math:: {\bf H} = \left[ \frac{\partial h_i}{\partial x_j} \right]_{m \times n}

that is, the output matrix :math:`{\bf H}` is the jacobian matrix of the ACEL equation in relation to the system states.

.. code-block:: matlab
    
    medNonlinear = [XT ; YT ; f6 ; ACEL];
    
    H = jacobian(medNonlinear,States);
    H = simplify(H);
    
Again, defining matlab functions to retrieve the numerical value of medNonlinearfunc and H.

.. code-block:: matlab
    
    medNonlinearfunc = matlabFunction(medNonlinear,'Vars',{XT YT PSI vT ALPHAT dPSI mT IT a b K});
    
    Hfunc = matlabFunction(H,'Vars',{PSI vT ALPHAT dPSI mT IT a b K});
    
Implementing the algorithm

Noise matrix

.. code-block:: matlab
    
    
    G = eye(6);                         % Identity matrix (6 x 6)
    
Covariance matrices

.. code-block:: matlab
    
    Q = eye(6);
    R = eye(5);
    
Matrix

.. code-block:: matlab
    
    P0 = eye(6);
    
Retrieving the initial conditions defined above.

.. code-block:: matlab
    
    X0Num = simulatorModel.X0;
    Y0Num = simulatorModel.Y0;
    PSI0Num = simulatorModel.PSI0;
    VEL0Num = simulatorModel.V0;
    ALPHAT0Num = simulatorModel.ALPHAT0;
    dPSI0Num = simulatorModel.dPSI0;
    
    x0 = [ X0Num ; Y0Num ; PSI0Num ; VEL0Num ; ALPHAT0Num ; dPSI0Num ];
    x0 = zeros(6,1);
    x0(4)=20;
    
Retrieving the vehicle parameters.

.. code-block:: matlab
    
    mTNum = VehicleModel.mT;
    ITNum = VehicleModel.IT;
    aNum = VehicleModel.a;
    bNum = VehicleModel.b;
    KNum = TireModel.k;
    
    parameters = [mTNum ITNum aNum bNum KNum];
    
Sample time

.. code-block:: matlab
    
    sampleTime = 0.1;
    
    t = 0:sampleTime:T;                 % Array of observation instants
    
Preallocation

.. code-block:: matlab
    
    XOUTopt = zeros(length(t) + 1,length(States));  % Estimate of the states after update
    Popt = zeros(length(t) + 1,1);                  % Covariance matrix after update
    Pantes = zeros(length(t) + 1,1);                % Covariance matrix before update
    KKalmanopt = ones(length(t) + 1,1);             % Kalman gain
    
Using the first values

.. code-block:: matlab
    
    XOUTopt(1,:) = x0';
    XOUTantes(1,:) = x0';
    Pantes(1,1) = norm(P0);
    Popt(1,1) = norm(P0);
    
Error distribution

.. code-block:: matlab
    
    pesos = [5; 5; 0.1; 0.5; 0.5];
    
Iterations

.. code-block:: matlab
    
    for j = 1:length(t)
        % Index varying through all instants of observation
    
        % Time span for the propagation phase
        tspan = t(j):sampleTime/100:t(j)+sampleTime;
    
        % Measures of the interation
        z = [interp1(TSPAN,XTPlant(:,1),t(j)) ;
             interp1(TSPAN,YTPlant(:,1),t(j)) ;
             interp1(TSPAN,ddPSIPlant(:,1),t(j)) ;
             interp1(TSPAN,ACELNumPlant(:,1),t(j)) ;
             interp1(TSPAN,ACELNumPlant(:,2),t(j))];
    
        z = z + pesos.*(rand(5,1)-0.5);
    
        Fnum = Ffunc(PSI0Num,VEL0Num,ALPHAT0Num,dPSI0Num,mTNum,ITNum,aNum,bNum,KNum);
        Hnum = Hfunc(PSI0Num,VEL0Num,ALPHAT0Num,dPSI0Num,mTNum,ITNum,aNum,bNum,KNum);
    
        % Propagation cycle
        % Transforming matrix PMat0 (6 x 6) in P0 (1 x 36)
        P0 = reshape(P0',[1 36]);
    
        [TOUT,Pout] = ode45(@(t,P) IntCov(t,P,Fnum,G,Q),tspan,P0);
    
        Pmatrix = reshape(Pout(end,:),[6 6])';
    
        simulatorKalman = Simulator(VehicleModel, tspan);
    
        % Initial conditions
        simulatorKalman.X0 = x0(1);
        simulatorKalman.Y0 = x0(2);
        simulatorKalman.PSI0 = x0(3);
        simulatorKalman.V0 = x0(4);
        simulatorKalman.ALPHAT0 = x0(5);
        simulatorKalman.dPSI0 = x0(6);
    
        % Simulation
        simulatorKalman.Simulate()
    
        XTKalman = simulatorKalman.XT;
        YTKalman = simulatorKalman.YT;
        PSIKalman = simulatorKalman.PSI;
        vTKalman = simulatorKalman.VEL;
        ALPHATKalman = simulatorKalman.ALPHAT;
        dPSIKalman = simulatorKalman.dPSI;
    
        XOUTKalman = [XTKalman YTKalman PSIKalman vTKalman ALPHATKalman dPSIKalman];
    
        % Update cycle
    
        ACELKalman = medNonlinearfunc(XTKalman(end),
                                      YTKalman(end),
                                      PSIKalman(end),
                                      vTKalman(end),
                                      ALPHATKalman(end),
                                      dPSIKalman(end),
                                      mTNum,
                                      ITNum,
                                      aNum,
                                      bNum,
                                      KNum);
    
        KKalman = Pmatrix*Hnum' / (Hnum*Pmatrix*Hnum' + R);
    
        XKalman = XOUTKalman(end,:)' + KKalman*(z - ACELKalman);
        PKalman = Pmatrix - KKalman*Hnum*Pmatrix;
    
        x0 = XKalman;
        P0 = PKalman;
    
        XOUTopt(j+1,:) = XKalman';
        XOUTantes(j+1,:) = XOUTKalman(end,:);
        Popt(j+1) = norm(PKalman);
        Pantes(j+1) = norm(Pmatrix);
        KKalmanopt(j+1) = norm(KKalman);
    
    end
    
Comparison

.. code-block:: matlab
    
    f8 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,XOUTPlant(:,1),'r')
    p = plot(t(2:end),XOUTopt(2:end-1,1),'r--');
    set(p,'Marker','*','MarkerSize',3)
    l = legend('Plant','Estim');
    set(l,'Location','SouthEast')
    xlabel('tempo [s]')
    ylabel('x [m]')
    
    f9 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,XOUTPlant(:,2),'g')
    p = plot(t(2:end),XOUTopt(2:end-1,2),'g--');
    set(p,'Marker','*','MarkerSize',3)
    l = legend('Plant','Estim');
    set(l,'Location','SouthEast')
    xlabel('tempo [s]')
    ylabel('y [m]')
    
    f10 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,XOUTPlant(:,3),'b')
    p = plot(t(2:end),XOUTopt(2:end-1,3),'b--');
    set(p,'Marker','*','MarkerSize',3)
    l = legend('Plant','Estim');
    set(l,'Location','SouthEast')
    xlabel('tempo [s]')
    ylabel('PSI [rad]')
    
    f11 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,XOUTPlant(:,4),'c')
    p = plot(t(2:end),XOUTopt(2:end-1,4),'c--');
    set(p,'Marker','*','MarkerSize',3)
    l = legend('Plant','Estim');
    set(l,'Location','SouthEast')
    xlabel('tempo [s]')
    ylabel('vT [m/s]')
    
    f12 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,XOUTPlant(:,5),'m')
    p = plot(t(2:end),XOUTopt(2:end-1,5),'m--');
    set(p,'Marker','*','MarkerSize',3)
    l = legend('Plant','Estim');
    set(l,'Location','SouthEast')
    xlabel('tempo [s]')
    ylabel('ALPHAT [rad/s]')
    
    f13 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    plot(TSPAN,XOUTPlant(:,6),'k')
    p = plot(t(2:end),XOUTopt(2:end-1,6),'k--');
    set(p,'Marker','*','MarkerSize',3)
    l = legend('Plant','Estim');
    set(l,'Location','SouthEast')
    xlabel('tempo [s]')
    ylabel('dPSI [rad/s]')
    
Cov. do erro

.. code-block:: matlab
    
    f14 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    p = plot(t(2:end),KKalmanopt(2:end-1),'r');
    set(p,'Marker','*','MarkerSize',3)
    ylabel('ganho de kalman')
    xlabel('tempo [s]')
    
    f15 = figure;
    ax = gca;
    set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
    p = plot(t(2:end),Popt(2:end-1),'r');
    set(p,'Marker','*','MarkerSize',3)
    p = plot(t(2:end),Pantes(2:end-1),'g');
    set(p,'Marker','*','MarkerSize',3)
    l = legend('+','-');
    set(l,'Location','NorthEast')
    ylabel('cov. erro')
    xlabel('tempo [s]')
    
The resulting plots can be seen below

.. figure:: ../illustrations/plot/KalmanFilterFig8.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig9.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig10.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig11.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig12.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig13.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig14.svg
    :align:   center
    :width: 40%

    Leegend

.. figure:: ../illustrations/plot/KalmanFilterFig15.svg
    :align:   center
    :width: 40%

    Leegend

Estimated trajectory

.. code-block:: matlab
    
    gKalman = Graphics(simulatorKalman);
    gKalman.Simulator.TSpan = t;
    gKalman.Simulator.XT = XOUTopt(1:end-1,1);
    gKalman.Simulator.YT = XOUTopt(1:end-1,2);
    gKalman.Simulator.PSI = XOUTopt(1:end-1,3);
    gKalman.Simulator.VEL = XOUTopt(1:end-1,4);
    gKalman.Simulator.ALPHAT = XOUTopt(1:end-1,5);
    gKalman.Simulator.dPSI = XOUTopt(1:end-1,6);
    gKalman.TractorColor = 'b';
    
    gPlant.Frame();
    gKalman.Frame('nextplot','add');
    
.. figure:: ../illustrations/frame/KalmanFilterFrame4.svg
    :align:   center
    :width: 70%

    Leegend