clear
close all
load data_labsession7
A=[ a1 a2 a3 0  0  0;
    b1 0  0  0  0  0;
    0  b2 b3 0  0  0;
    c1 c2 c3 0  0  0;
    0  0  0  d1 0  0;
    0  0  0  0  d2 d3];
B=eye(6);
G=eye(6);
C= [ 1  1  1  0  0  0;
     0  0  0  1  1  1];
Q=sw2*eye(6);
R=sv2*eye(2);
% initialization
x = zeros(6,1);
P = 100^2*eye(6);
X(:,1)=x;
D(:,1)=diag(P);
% recursion
for k=1:100,
    % prediction
    xp = A*x + B*uA(k,:)';
    Pp = A*P*A' + G*Q*G';
        
    % correction
    K = Pp*C'*inv(C*Pp*C'+R);
    xc = xp + K*(yA(k+1,:)'-C*xp);
    Pc = Pp*(eye(6)-C'*K');
    x=xc;
    P=Pc;
    
    X(:,k+1)=x;
    D(:,k+1)=diag(P);
end
for i=1:6,
 figure, plot(t,xtrueA(:,i)-X(i,:)','r',t,3*sqrt(D(i,:)),'b--',t,-3*sqrt(D(i,:)),'b--')
end






