% Exemple de moyenne recursive pour un capteur
% (c) Philippe Giguere, 2010

clearvars    % Effacer toutes les variables dans matlab
nStep = 50;

%Xtrue = 5.0*ones(1,nStep); % Valeur veritable
Xtrue = 1.0 + 0.1*(1:nStep);
Scapteur = 1.0;     % bruit sur le capteur z
xhat = 0.0;  % valeur estime 

% Boucle de capture d'echantillons
for k=1:nStep
    z(k) = Xtrue(k) + Scapteur*randn; % Mesure bruitee 
    if (k==1)
        % Premier echantillon, on initialise
        xhat = z(k);
        P    = Scapteur^2;
    else
        % Equations (4.4) du manuel, page 104
        w = P(k-1)/(P(k-1)+Scapteur^2);
        xhat(k) = xhat(k-1) + w*(z(k)-xhat(k-1));
        P(k) = (1-w)*P(k-1);
    end
    mz(k) = mean(z(1:k));
end

% ---------- Tracer la figure -------------
clf;
myFontSize = 16;
h(1) = plot(xhat,'LineWidth',2);
hold on;
h(2) = plot(z,'r*');
ylim([0 7]);
c = axis;
h(4) = plot(Xtrue,'LineStyle','--','LineWidth',2);
h(3) = plot(mz,'o');

legend(h,{'Moyenne Recursive' 'Mesure z(k)' 'Moyenne' 'Valeur Veritable'},'Location','SouthEast');
xlabel('Iteration k','Fontsize',myFontSize);
ylabel('Valeur','Fontsize',myFontSize);
set(gca,'Fontsize',myFontSize-2);
