MAE 280 B
Linear Control Design

LMI LQR Control, second form

Mauricio de Oliveira & Chris Nelson

<<NC` 
<<NCAlgebra`

<<NCSDP`
<<SDPSylvester`

LMI LQR Control

Satellite problem data

m = 100 (* 100 kg *);
r = 300*10^3 (* 300 km *);
R0 = 6.37*10^6 (* 6.37 10^3 km *);
G = 6.673*10^(-11) (* 6.673 N m^2/kg^2 *);
M = 5.98*10^(24) (* 5.98 10^24 kg *);
k = G*M; (* gravitational force constant *)
w = Sqrt[k/((R0+r)^3)]; (* angular velocity (rad/s) *)
v = w*(R0+r); (* "ground" velocity (m/s) *)

Linearlized system matrices

A = {{0,0,1,0},{0,0,0,1},{3*w^2,0,0,2*(r+R0)*w},{0,0,-2*w/(r+R0),0}};
Bu = {{0,0},{0,0},{1/m,0},{0,1/(m*r)}};
Bw = {{0,0},{0,0},{1/m,0},{0,1/(m*r)}};

Noise variances

W = 0.1*IdentityMatrix[2];

Scale

T = DiagonalMatrix[{1,r,1,r}];

Similarity transformation

At = T.A.Inverse[T];
But = T.Bu;
Bwt = T.Bw;

n = Length[At];

Optimal LQR state feedback control (using u1 and u2)

LQR Solution

m = 2;

Cz = Join[IdentityMatrix[n],ConstantArray[0,{m,n}]];
Dzu = Join[ConstantArray[0,{n,m}],IdentityMatrix[m]];

K = LQRegulatorGains[StateSpaceModel[{At,But}],{Transpose[Cz].Cz,Transpose[Dzu].Dzu}];
Klqr = -K

LMI solution (second form)

EE=NullSpace[ArrayFlatten[{{Transpose[But],Transpose[Dzu]}}]];
EE1=EE[[All,1;;n]];
EE2=EE[[All,(n+1);;2n+m]];

SNC[a,bw,q,x,z,c,e1,e2,ww];
F={{{a**x+x**tp[a]+q,x**tp[c]},{c**x,-1}},-{{z,tp[bw]},{bw,x}},-x+q};
vars={z,x};
obj={-1,0};

FF=F;
FF[[1]]=NCExpand[Part[MatMult[tpMat[{{e1},{e2}}],F[[1]],{{e1},{e2}}],1,1]];

NCSDPForm[FF,vars,obj]

data={a-> At,bw-> Bwt[[All,{2}]],c-> Cz,e1->Transpose[EE1],e2->Transpose[EE2],q-> 10^(-7)*IdentityMatrix[n]};

{abc,rules}=NCSDP[FF,vars,obj,data,UserRules->{tp[q]->q},DebugLevel->0];

{Y,Xd,Sd,iters}=SDPSolve[abc,rules];

{Z,X}=Y;

Reconstruct controller

YY=Inverse[X];
QQ=ArrayFlatten[{{YY.At+Transpose[At].YY,Transpose[Cz]},{ Cz,-IdentityMatrix[m+n]}}];
BB=ArrayFlatten[{{Transpose[But].YY, Transpose[Dzu]}}];
CC=ArrayFlatten[{{IdentityMatrix[n], ConstantArray[0,{n,m+n}]}}];

mu=10^7
Max[Eigenvalues[QQ-mu Transpose[BB].BB]]
Max[Eigenvalues[QQ-mu Transpose[CC].CC]]

Phi=QQ-mu Transpose[BB].BB-mu Transpose[CC].CC;
XX=mu^2 CC.Inverse[Phi].Transpose[BB];
Klmi=Transpose[XX];
MatrixForm[Klmi]

MatrixForm[Klqr]

Acl=At+But.Klmi;
Eigenvalues[Acl]