clear all;
clc;
syms p11 p12 p13 p14 p22 p23 p24 p33 p34 p44
A=[0,0,1,0;0,0,0,1;0,0,0,0;0,0,0,0]
B=[0,0;0,0;1,0;0,1]
Q=[350,0,0,0;0,350,0,0;0,0,5.6,0;0,0,0,5.6]
R=.05*[1,0;0,1]
P=[p11,p12,p13,p14;p12,p22,p23,p24;p13,p23,p33,p34;p14,p24,p34,p44]
row= P*A+transpose(A)*P+P*B*inv(R)*transpose(B)*P-Q;
eqn1=row(1,1)==0;
eqn2=row(1,2)==0;
eqn3=row(1,3)==0;
eqn4=row(1,4)==0;
eqn5=row(2,2)==0;
eqn6=row(2,3)==0;
eqn7=row(2,4)==0;
eqn8=row(3,3)==0;
eqn9=row(3,4)==0;
eqn10=row(4,4)==0;
sol = solve([eqn1, eqn2,eqn3,eqn4,eqn5,eqn6,eqn7,eqn8,eqn9,eqn10], [p11, p12, p13, p14, p22, p23, p24, p33, p34, p44]);
N=length(sol.p11);
for i=1:N

    rp11=single(sol.p11(i));   
    rp12=single(sol.p12(i));
    rp13=single(sol.p13(i));    
    rp14=single(sol.p24(i));
    
    rp22=single(sol.p22(i));
    rp23=single(sol.p23(i));
    rp24=single(sol.p24(i));
    
    rp33=single(sol.p33(i));
    rp34=single(sol.p34(i));
    
    rp44=single(sol.p44(i));
    
    P=[rp11,rp12,rp13,rp14;rp12,rp22,rp23,rp24;rp13,rp23,rp33,rp34;rp14,rp24,rp34,rp44];
    K=-inv(R)*transpose(B)*P;
    valEig=eig(A-B*K);
    if valEig(1)<0 && valEig(2)<0 && valEig(3)<0 && valEig(4)<0 
        'rozwiazanie'
        i
        P
        K
        valEig
    end
end

[Kmatlab,Smatlab,Ematlab]=lqr(A,B,Q,R,0)