% Filtre Kalman non lineaire EKF
% Pour un robot 2D et un GPS bruité.
% (c) 2013 Philippe Giguere
% ATTENTION! Dans ce code, je ne fais pas de wrap2pi pour les angles, car
% je n'ai pas de mesure provenant d'un capteur qui retourne des valeurs
% entre +- pi. Cela reste donc à corriger.

clearvars
clf
myFontSize = 14;
dT = 0.1;  % Plus je prends des mesures frequentes, plus petite est l'erreur

TestCase = 3;

if (TestCase == 1)
    BiaisOmega = 0; % Petit biais dans la vitesse angulaire
    ErreurAngleDepart = 0;
elseif (TestCase ==2)
    BiaisOmega = 0; % Petit biais dans la vitesse angulaire
    ErreurAngleDepart = 0.4;
elseif (TestCase ==3)
    BiaisOmega = 0.02; % Petit biais dans la vitesse angulaire
    ErreurAngleDepart = 0;
end

% Bruits (en théorie inconnus) dans le système
Somega   = 0.1; % Bruit de système, en rad/s, sur la rotation du véhicule
SV       = 0.2; % Bruit de système, en m/s, sur le déplacement du véhicule

SPosition = 12; % Bruit de mesure, en mètres, provenant d'un capteur de position x-y
xVrai    = [0 0 0]'; % Position de départ du véhicule

% Variables connues
Cv = [(2*SV)^2 0; 0 (2*Somega)^2]; % Bruits sur la propagation
Cw = [SPosition.^2 0; 0 SPosition.^2]; % Bruits sur la mesure

% Actions du systeme. On fera tourner la voiture en rond.
V     = 10; % m/s
omega = 0.1; % rad/s;

% Initialiser le filtre
P = [100^2 0 0; 0 100^2 0; 0 0 0.5^2]; % Erreurs initiales en mètres ou radians
X = xVrai;
X = [ 0 0 ErreurAngleDepart]';
XI = X;

% Parametre de la simulation
nStep = 6000;
Ax = zeros(1,nStep);
Ay = zeros(1,nStep);
AEx = zeros(1,nStep);
AEy = zeros(1,nStep);
AZ  = zeros(2,nStep);
AGyro = zeros(1,nStep);
AError = zeros(1,nStep);
AErrorInt = zeros(1,nStep);
ATime = zeros(1,nStep);
Xint = zeros(3,nStep);
AErrorAngle = zeros(1,nStep);
AErrorAngleAveugle = zeros(1,nStep);

for iStep = 1:nStep
    clf
    subplot(3,3,[1 2 4 5 7 8]);
    % Simulation du systeme
    time = iStep*dT;

    % Integration sans correction
    XI(1) = XI(1) + V*cos(XI(3))*dT;
    XI(2) = XI(2) + V*sin(XI(3))*dT;
    XI(3) = XI(3) + (omega+BiaisOmega)*dT;
    Xint(:,iStep)= XI;
    
    % Je fais deplacer le robot sur un cercle
    xVrai(1) = xVrai(1) + (V+SV*randn)*cos(xVrai(3))*dT;
    xVrai(2) = xVrai(2) + (V+SV*randn)*sin(xVrai(3))*dT;
    xVrai(3) = xVrai(3) + (omega+Somega*randn)*dT;
    
    h(1) = plot(xVrai(1),xVrai(2),'o','MarkerFaceColor','b','MarkerSize',10); % Position reele du robot
    hold on;
    line([xVrai(1) xVrai(1)+5*cos(xVrai(3))],[xVrai(2) xVrai(2)+5*sin(xVrai(3))],'LineWidth',3); % To show heading
    axis([xVrai(1)-30 xVrai(1)+30 xVrai(2)-30 xVrai(2)+30]);
    grid on;
    %axis equal
       
    % Je simule la reponse du capteur pseudo-GPS
    z = [xVrai(1) + SPosition*randn; xVrai(2) + SPosition*randn;];
 
    % ====== Propagation =========
    %U = [V*dT omega*dT]';
    X(1) = X(1) + V*cos(X(3))*dT;
    X(2) = X(2) + V*sin(X(3))*dT;
    X(3) = X(3) + omega*dT;

    % J'estime la mesure du pseudo-GPS, qui est ni plus ni moins que les
    % valeurs estimee
    zhat = [X(1); X(2)];

    % ========= Calcul des matrices Jacobiennes =============
    F = [1 0 -V*sin(X(3))*dT; 0 1 V*cos(X(3))*dT; 0 0 1];
    G = [dT*cos(X(3)) dT*sin(X(3)) 0; 0 0 dT]';
    H = [1 0 0; 0 1 0]; % Ici c'est un système linéaire entre l'estimé et la mesure

    P = F*P*F' + G*Cv*G';  % Propagation covariance
    
    % ======== Mise-a-jour ========
    K = P*H'/(H*P*H'+Cw);   % 
    r = (z-zhat);           % 
    X = X + K*r;
    P = (eye(3)-K*H)*P;

    % Je trace la position estimée par Kalman
    h(2) = plot(X(1),X(2),'go','MarkerFaceColor','g','MarkerSize',10); % Position estimée du robot
    hl = line([X(1) X(1)+5*cos(X(3))],[X(2) X(2)+5*sin(X(3))],'LineWidth',3); % To show heading
    set(hl,'Color','g');

    % Je trace la position estimée à l'aveugle
    h(3) = plot(XI(1),XI(2),'ko','MarkerFaceColor','k','MarkerSize',10); % Position estimée du robot
    hl = line([XI(1) XI(1)+5*cos(XI(3))],[XI(2) XI(2)+5*sin(XI(3))],'LineWidth',3); % To show heading
    set(hl,'Color','k');

    % Incertitude
    text(0.1, 0.22, sprintf('$$\\sigma_{x}=$$%.4f m',sqrt(P(1,1))),'Units','normalize','Interpreter','latex','FontSize',16,'BackgroundColor','w');
    text(0.1, 0.15, sprintf('$$\\sigma_{y}=$$%.4f m',sqrt(P(2,2))),'Units','normalize','Interpreter','latex','FontSize',16,'BackgroundColor','w');
    text(0.1, 0.08, sprintf('$$\\sigma_{\\theta}=$$%.4f rad',sqrt(P(3,3))),'Units','normalize','Interpreter','latex','FontSize',16,'BackgroundColor','w');

    % Erreur
    text(0.5, 0.22, sprintf('$$\\varepsilon_{x}=$$%.4f m',X(1) - xVrai(1)),'Units','normalize','Interpreter','latex','FontSize',16,'BackgroundColor','w');
    text(0.5, 0.15, sprintf('$$\\varepsilon_{y}=$$%.4f m',X(2)-xVrai(2)),'Units','normalize','Interpreter','latex','FontSize',16,'BackgroundColor','w');
    text(0.5, 0.08, sprintf('$$\\varepsilon_{\\theta}=$$%.4f rad',X(3)-xVrai(3)),'Units','normalize','Interpreter','latex','FontSize',16,'BackgroundColor','w');

    h(4) = plot(z(1),z(2),'rs','MarkerFaceColor','r','MarkerSize',16);
    legend(h,{'Position réelle' 'Estimé Kalman' 'Estimé aveugle' 'Mesure instantanée'},'Location','NorthEast','FontSize',14);
    xlabel('Position x (m)','FontSize',14);
    ylabel('Position y (m)','FontSize',14);

    % Cueillette des donnees pour les graphiques
    Ax(iStep)           = xVrai(1);
    Ay(iStep)           = xVrai(2);
    AEx(iStep)          = X(1);
    AEy(iStep)          = X(2);
    AError(iStep)       = sqrt((xVrai(1)-X(1))^2+(xVrai(2)-X(2))^2);
    AErrorAngle(iStep)  = X(3) - xVrai(3);
    AErrorAngleAveugle(iStep)  = XI(3) - xVrai(3);  
    AErrorInt(iStep)    = sqrt((xVrai(1)-XI(1))^2+(xVrai(2)-XI(2))^2);
    AZ(:,iStep) = z;
    ATime(iStep) = time;
    
    subplot(3,3,[3 6]);
    h(4) = plot(AZ(1,1:iStep),AZ(2,1:iStep),'rs','MarkerSize',2);
    hold on
    h(1) = plot(Ax(1:iStep),Ay(1:iStep),'b-','LineWidth',2);
    h(2) = plot(AEx(1:iStep),AEy(1:iStep),'g-','LineWidth',2); set(h(2), 'Color', [0 0.5 0]);
    h(3) = plot(Xint(1,(1:iStep)),Xint(2,(1:iStep)),'k-','LineWidth',1);
    xlabel('x','FontSize',myFontSize);
    ylabel('y','FontSize',myFontSize);
    axis([-150 150 -50 250]);
    
    subplot(3,3,9);
    hl(1) = plot(AErrorAngle(1:iStep),'g-','LineWidth',2);
    hold on;
    hl(2) = plot(AErrorAngleAveugle(1:iStep),'k-','LineWidth',2);
    c = axis;
    line(c(1:2),[0 0],'Color','k');
    ylabel('Erreur angle (rad)','FontSize',14);
    xlabel('Temps (dt)','FontSize',14);
    legend(hl,{'Kalman' 'Aveugle'},'Location','NorthWest');
    ylim([-0.3 1]);
    
    drawnow;
end

figure(1);
clf
h(1) = plot(Ax,Ay,'r-','LineWidth',3);
hold on
h(2) = plot(AEx,AEy,'k-','LineWidth',3);
h(3) = plot(Xint(1,:),Xint(2,:),'g-','LineWidth',3);
xlabel('x','FontSize',myFontSize);
ylabel('y','FontSize',myFontSize);
plot(Xb,Yb,'o','MarkerFaceColor','b','MarkerSize',10);
text(Xb+0.2,Yb,'Repere','FontSize',myFontSize);
set(gca,'FontSize',myFontSize-2);
axis equal;
legend(h,{'Vraie' 'Kalman' 'Integree'});
figure(2)
subplot(2,1,1);
hist(AErrorInt,20);
std(AErrorInt)
xlabel('Erreur integration','FontSize',myFontSize);
set(gca,'FontSize',myFontSize-2);
title(sprintf('Erreur moyenne = %.2f',mean(AErrorInt)),'FontSize',myFontSize);
c = axis;

subplot(2,1,2);
hist(AError,20);
xlim(c(1:2));
std(AError)
xlabel('Erreur Kalman','FontSize',myFontSize);
title(sprintf('Erreur moyenne = %.2f',mean(AError)),'FontSize',myFontSize);
set(gca,'FontSize',myFontSize-2);

