% Filtre Kalman pour chariot
% (c) Philippe Giguere 2010
clearvars
clf
myFontSize = 14;

% Variables inconnues décrivant le système.
Sx      = 0.01;
Slaser  = 0.1;
xVrai = 0;

% Variables connues
P = 0.1;
x = xVrai;
Cv = (Sx)^2;
Cw = Slaser^2;
    
% Tracer les distributions pour les deux inputs
Bins = -0.4:0.001:0.4;
subplot(2,5,[6 7]);
PasDistrib = Sx*randn(1,100000);
hist(PasDistrib,Bins)
xlim([-0.4 0.4]);
c = axis();
title('Distribution des erreurs sur deplacements','FontSize',myFontSize);
xlabel('Erreur','FontSize',myFontSize);
set(gca,'YTick',[]);
ylabel('Decompte histogramme','FontSize',myFontSize);
text(-0.3,4000,sprintf('Sigma=%.2f m',Sx),'FontSize',myFontSize-2);
set(gca,'FontSize',myFontSize);

subplot(2,5,[8 9]);
LaserDistrib = Slaser*randn(1,100000);
hist(LaserDistrib,Bins)
set(gca,'YTick',[]);
axis(c);
title('Distribution des erreurs sur mesures laser','FontSize',myFontSize);
text(-0.3,4000,sprintf('Sigma=%.2f m',Slaser),'FontSize',myFontSize-2);
xlabel('Erreur','FontSize',myFontSize);
ylabel('Decompte histogramme','FontSize',myFontSize);
set(gca,'FontSize',myFontSize);

V  = 1;     % Vitesse de 1 m/s
dt = 0.1;

for iStep = 1:40
    % Simulation du systeme
    xVrai = xVrai + V*dt + Sx*randn;
    z  = xVrai + Slaser*randn; % mesure laser
    
    % Propagation
    x  = x + V*dt;
    P  = P + Cv;
    
    % Mise-a-jour (update)
    K  = P/(P+Cw);
    x  = x + K*(z-x);
    P  = (eye(1)-K)*P;
    
    Ax(iStep) = xVrai;
    AP(iStep) = P;
    AK(iStep) = K;
    AEx(iStep) = x;
    ALaser(iStep) = z;
    ErreurEx(iStep) = abs(x-xVrai);
    ErreurLaser(iStep) = abs(z-xVrai);
    ATime(iStep) = iStep*dt;
    
    subplot(2,5,[1 2]);
    plot(ATime,ALaser,'r*');
    xlabel('Temps (s)','FontSize',myFontSize);
    ylabel('Mesure laser (m)','FontSize',myFontSize);
    set(gca,'FontSize',myFontSize);

    subplot(2,5,[3 4]);
    h(1) = plot(ATime,Ax,'k','LineWidth',2);
    hold on;
    h(3) = plot(ATime,AEx,'go','MarkerFaceColor','g');
    h(2) = plot(ATime,ALaser,'r*');
    xlabel('Temps (s)','FontSize',myFontSize);
    ylabel('Position (m)','FontSize',myFontSize);
    legend(h,{'Position reelle' 'Laser' 'Kalman'},'Location','NorthWest');
    set(gca,'FontSize',myFontSize);
end

subplot(2,5,5);
plot(ATime,AK,'b-','LineWidth',2);
xlabel('Temps (s)','FontSize',myFontSize);
ylabel('Gain K','FontSize',myFontSize);
set(gca,'FontSize',myFontSize);
xlim([0 4]);

subplot(2,5,10);
plot(ATime,AP,'g-','LineWidth',2);
xlabel('Temps (s)','FontSize',myFontSize);
ylabel('Variance P','FontSize',myFontSize);
set(gca,'FontSize',myFontSize);
xlim([0 4]);

mean(ErreurEx)
mean(ErreurLaser)

