clear all
clc

for k=1:3
A=[1,.1,.2;.3,1,.1;.2,.1,1];
B=[1,.5,.2;.5,1,.1];
tau=[.1,0,0;0,.1,0;0,0,.1];

x0=random('normal',3,1,3,1);

Q=[1,0,0;0,1,0;0,0,1];
V=[0.2,0;0,0.2];

n1=10;

for i=1:n1
    u1=random('normal',0,1,3,1);
    x=A*x0 + tau*u1;
    u2 = random('normal',0,0.2,2,1);
    y=B*x + u2;
    
    for j=1:3;
       X1(i,j)=x(j); 
    end

    for j=1:2;
       Y1(i,j)=y(j); 
    end
    x0=x; 
end;

M_0=[3;3;3];
P_0=[3,0,0;0,3,0;0,0,3];

M_1=zeros(3,1);
P_1=zeros(3,3);

for i=1:n1
    M=A*M_0;
    P = A*P_0*A' + tau*Q*tau';
    Pinv1=inv(V+B*P*B');
    M_1 = M + P*B'*Pinv1*(Y1(i,:)'-B*M);
    P_1 = P-P*B'*Pinv1*B*P;
    M1(i,:)=M_1;
    P_0 = P_1;
    M_0=M_1;
end




for i=1:n1
    sim1(i)=norm(X1(i,:)-M1(i,:));
end;

figure(1)
plot(sim1);
title(['Error: ||X(t) - Xhat(1)|| versus time for simulation ' int2str(k)]);
saveas(figure(1),['Simulated process- ' int2str(k) ' of x prob 2.png']);


end
