A=[0 1 0 0; 0 0 1 0 ; 0 0 0 1; -10^-11 -24010 -2539 -54];
B=[0;0;0;31940];
sys=ss(A,B,ones(4),[0;0;0;0]);
sys=c2d(sys,0.01);
[A,B,C,D] = ssdata(sys);

Q=[1000 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
R=1000;   

N=100;
P=Q;

x=[10;10;10;10];

figure;
hold on;

for j=1:N
   xx(:,j)=x;
   for k = j:N
      Kp=inv(R+B'*P*B)*B'*P*A;     
      P=A'*P*A+Q-A'*P*B*inv(R+B'*P*B)*B'*P*A;      
%      K(N-k+1,1)=Kp(1);
%      K(N-k+1,2)=Kp(2);
%      K(N-k+1,3)=Kp(3);
%      K(N-k+1,4)=Kp(4);
	eig(P);
   end
   u=-Kp*x;
   x=A*x+B*u;
   xx(:,j+1)=x;
%   plot(j,x(1),j,x(2),j,x(3),j,x(4));  
end
  plot(xx')