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

%% Discrete Kalman Filter

for nm=1:3
% nm=2;

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

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

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

%Kalman gain
Kg_tot=[];

%Variance
Var_m=(5.08986e-06)*9.81;
Var_pro=1;

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

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

%Dynamic coefficient matrix
A=[1    t   (t^2)/2
   0    1   t
   0    0   1      ];

%Measurement sensitivity matrix
H=[0    0   1];

%Covariance matrix of measurement
R=Var_m;

%Covariance matrix of process noise
% Q=5e-12;
Q=Var_pro*[(t^5)/20  (t^4)/8    (t^3)/6
           (t^4)/8   (t^3)/6    (t^2)/2
           (t^3)/6   (t^2)/2    t      ];

%An estimate of the value of x-1
x_kmenos1=x_tot(:,end);
% x_kmenos1(3,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;
z=Acc(i+1,nm,modulo)-aceler_inicial(:,nm,:);

%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

hold on
% Definição do gráfico 1 (Posição)
figure(3)
if nm==1
%     plot(log_tempo(:,:,modulo),x_tot(1,:),'k')
    x_tot_all(nm,:)=x_tot(1,:);
elseif nm==2
%     plot(log_tempo(:,:,modulo),x_tot(1,:),'g')
    x_tot_all(nm,:)=x_tot(1,:);
elseif nm==3
%     plot(log_tempo(:,:,modulo),x_tot(1,:),'m')
    x_tot_all(nm,:)=x_tot(1,:);
end
grid on
ylabel('Posição (m)');xlabel('Tempo (s)');
legend('X_m_e_d_2_0','Y_m_e_d_2_0','Z_m_e_d_2_0','X_K_a_l','Y_K_a_l','Z_K_a_l');
% legend('X_m_e_d_T','Y_m_e_d_T','Z_m_e_d_T','X_K_a_l','Y_K_a_l','Z_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')
% plot(log_tempo(:,:,modulo),log_pos(:,1),log_tempo(:,:,modulo),log_pos(:,2),log_tempo(:,:,modulo),x_tot_all(3,:),log_tempo(:,:,modulo),x_tot_all(1,:),log_tempo(:,:,modulo),x_tot_all(2,:),log_tempo(:,:,modulo),log_pos(:,3))
% plot(log_tempo(:,:,modulo),log_pos(:,1),log_tempo(:,:,modulo),x_tot_all(2,:),log_tempo(:,:,modulo),log_pos(:,3),log_tempo(:,:,modulo),x_tot_all(1,:),log_tempo(:,:,modulo),log_pos(:,2),log_tempo(:,:,modulo),x_tot_all(3,:))
end