clear all
clc

n=2;
sys = drss(n,1,1);
A=sys.A;
B=sys.B;
Qx = eye(n);
R = 1;
Qf = eye(n);

[Kss,Pss] = dlqr(A,B,Qx,R)

%% Yalmip LMI inf horizon OK

P = sdpvar(2,2,'symmetric');
F = [P>=0, [A'*P*A+Qx-P, A'*P*B; B'*P*A, R+B'*P*B ] >=0 , R+B'*P*B>0];
optimize(F,-trace(P));
Pf = value(P)
Kf = inv(R+B'*Pf_m*B)*B'*Pf_m*A


%% SDP formulation. LMI from (A+BK)P(A+BK)'- P with X:= P^-1, L:=KP^-1

X = sdpvar(n,n);
L = sdpvar(1,n,'full' );

MAT = [(-A*X-B*L)'-A*X-B*L, X, L'; X, inv(Qx), zeros(n,1); L, zeros(1,n), inv(R)]
c = [X>=0, MAT>=0]
optimize(c,trace(X));
Kf_sdp = value(L)*inv(value(X))


%% using cvx
cvx_begin
   variable X(n,n)
   variable L(1,n)
   minimize (trace(X))
   subject to
      [(-A*X-B*L)'-A*X-B*L, X, L'; X, inv(Qx), zeros(n,1); L, zeros(1,n), inv(R)]>=0
      X>=0
cvx_end
Kf_cvx = L*inv(X)