% Filtre Kalman non lineaire EKF
% Pour un robot 2D et un beacon de distance
% (c) 2010 Philippe Giguere

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

% Variables inconnues décrivant le système.
SBeacon  = 0.01; % m
Somega   = 0.2; % rad/s
SV       = 0.2; % m/s
xVrai    = [0 0 0]';
Xb       = 0;   % position du beacon
Yb       = 3;   % position du beacon

% Variables connues
Cv = [(2*SV)^2 0; 0 (2*Somega)^2]; % Bruits
Cw = SBeacon.^2;

% Actions du systeme 
V     = 0.4; % m/s
omega = 0.1; % rad/s;


% Initialiser le filtre
P = [SV^2 0 0; 0 SV^2 0; 0 0 Somega^2];
X = xVrai;
XI = X;

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


for iStep = 1:nStep
    % 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*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;
       
    % Je simule la reponse du capteur de distance
    z = sqrt((xVrai(1)-Xb)^2 + (xVrai(2)-Yb)^2) + SBeacon*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 distance
    zhat = sqrt((X(1)-Xb)^2 + (X(2)-Yb)^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 = [(X(1)-Xb)/zhat (X(2)-Yb)/zhat 0];

    P = F*P*F' + G*Cv*G';  % Propagation covariance
    
    % ======== Mise-a-jour ========
    K = P*H'/(H*P*H'+Cw);   % matrice [3x1]
    r = (z-zhat);           % matrice [1]
    X = X + K*r;
    P = (eye(3)-K*H)*P;
    
    % 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);
    AErrorInt(iStep)    = sqrt((xVrai(1)-XI(1))^2+(xVrai(2)-XI(2))^2);
    ATime(iStep) = time;
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);

