%UA, DEM, MIEM
%Nuno Silva, 51164
%29 de Março, 2011

%% Discrete Kalman Filter

x_kalman(:,:,1,1)=zeros(2,size(database.id.mod2.gyro.data,1));
x_kalman(:,:,2,1)=zeros(2,size(database.id.mod2.gyro.data,1));

% for modulo=2:3

    run show_data
    
for nm=1:2
    % nm=2;

    I=eye(2,2);
    % modulo=2;
    eixo=nm;

    %A posteriori estimate error covariance
    P=zeros(2,2);

    %Estimate Matrix
    x_tot=zeros(2,1);

    %Kalman gain
    Kg_tot=[];

    %Variance
    Var_m=0.24;
    Var_pro=0.227;

    if nm==1
        if modulo==2
            bias=1;
        elseif modulo==3
            bias=0.8;
        end
    elseif nm==2
        if modulo==2
            bias=2;
        elseif modulo==3
            bias=1.072;
        end
    end

    for i=1:size(database.id.(['mod' num2str(modulo)]).gyro.data,1)-1

        t=(database.id.(['mod' num2str(modulo)]).timestamp(i+1)-database.id.(['mod' num2str(modulo)]).timestamp(i))*tempo_estoiro_tmr1;

        %Dynamic coefficient matrix
        A=[1    t
           0    1];

        %Measurement sensitivity matrix
        H=[0    1];

        %Covariance matrix of measurement
        R=Var_m;

        %Covariance matrix of process noise
        % Q=Var_pro;
        Q=[0.0001  0
           0      0.0003];

        %An estimate of the value of x-1
        x_kmenos1=x_tot(:,end);
        % x_kmenos1(2,1)=A2(i,nm);

        %A posteriori estimate error covariance
        P_kmenos1=P;

        %A priori estimate of x
        x_priori=A*x_kmenos1;

        %A priori estimate error covariance
        P_priori=A*P_kmenos1*A'+Q;

        %Kalman gain
        Kg=P_priori*H'*(H*P_priori*H'+R)^-1;
        Kg_tot(end+1,:)=Kg;

        %Measurement
        % z=(database.id.(['mod' num2str(modulo)]).acel.data(i+1,eixo)-database.id.(['mod' num2str(modulo)]).acel.data(i,eixo))*9.81;

        if i<5
            z=database.id.(['mod' num2str(modulo)]).gyro.data(i+1,nm)+bias;
        else
            z=median(database.id.(['mod' num2str(modulo)]).gyro.data(i-3:i+1,nm))+bias;
        end

        %An estimate of the value of x
        x=x_priori+Kg*(z-H*x_priori);
        x_tot(:,end+1)=x;

        %Estimate error covariance
        P=(I-Kg*H)*P_priori;
    end
    
    x_kalman(:,:,nm,modulo)=x_tot;
    
    hold on
    % Definição do gráfico 1 (Posição)
    figure(6)
    if nm==1
        plot(log_tempo(:,:,modulo),x_tot(1,:),'k')
    elseif nm==2
        plot(log_tempo(:,:,modulo),x_tot(1,:),'g')
    end
    grid on
    ylabel('Posição angular (º)'),xlabel('Tempo (s)');
    legend('X','Y','X_K_a_l','Y_K_a_l');
    % 
    % % Definição do gráfico 1 (Posição)
    % figure(2)
    % plot(x_tot(3,:),'r')
    % grid on
    % ylabel('Aceleração (m)'),xlabel('Tempo (min)');
    % legend('X');
    % hold on; plot(log_acel(:,eixo),'b')

end

% end