x(:,1)=[10;10;10;10];
clear e;
clear p;
clear K;
T=1000;
Q=[1000 0 0 0; 0 1 0 0;0 0 1 0;0 0 0 1];
p{T}=Q;

for t=(T-1):-1:1
   if t==100
      Q=Q*1e6;
   end
   
   
   p{t}=A'*p{t+1}*A+Q-A'*p{t+1}*B*inv(R+B'*p{t+1}*B)*B'*p{t+1}*A;
   K{t}=inv(R+B'*p{t+1}*B)*B'*p{t+1}*A;
   e(:,T-t)=eig(A-B*K{t});
   
end

for t=1:T-1
   x(:,t+1)=A*x(:,t)-B*(K{t}*x(:,t));
end