MAE 280 B
Linear Control Design

LMI Decentralized LQR Control

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;

Apply decentralized structure to the data

Another similarity to sort out state

T = {{1,0,0,0},{0,0,1,0},{0,1,0,0},{0,0,0,1}};
Att = T.At.Inverse[T];
Butt = T.But;
Bwtt = T.Bwt;

n = Length[Att];

Block diagonalize data

{{A1,A2},{A3,A4}}=Partition[Att,{2,2}];
{{A1,A2},{A3,A4}}//MatrixForm

{{B1,B2},{B3,B4}}=Partition[Butt,{2,1}]
{{B1,B2},{B3,B4}}//MatrixForm

{{Bw1},{Bw2}}=Partition[Bwtt,{2,2}]
{{Bw1},{Bw2}}//MatrixForm

Optimal LQR state feedback control

m = Length[Butt[[1]]];

LQR Solution

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

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

LMIs for LQR problem

SNC[a,b,c,d,bw,q,q1,x,l,z];
F={
{{a**x+b**l+x**tp[a]+tp[b**l],tp[c**x+d**l]},
{c**x+d**l,-1}},
-{{z,tp[bw]},
{bw,x}}
};

LMIs for LQR with decentralization

SNC[a1,a2,a3,a4,b1,b2,b3,b4,c1,c2,d1,d2,bw1,bw2,ww];
SNC[x1,x2,l1,l2];

blockRule={
a->{{a1,a2},{a3,a4}},
b->{{b1,b2},{b3,b4}},
c->{{c1,c2}},
d->{{d1,d2}},
bw->{{bw1},{bw2}},

x->{{x1,0},{0,x2}},
l->{{l1,0},{0,l2}},
z->{{z}}
};

productRule={
NonCommutativeMultiply->MatMult,
tp->tpMat
};

FBlock=Map[ArrayFlatten,F/.productRule/.blockRule];

vars=DeleteCases[Flatten[{z,x,l}/.blockRule],0]
obj={-ww,0,0,0,0};

NCSDPForm[FBlock,vars,obj]

{{Cz1,Cz2}}=Partition[Cz,{6,2}]
{{Cz1,Cz2}}//MatrixForm

{{Dzu1,Dzu2}}=Partition[Dzu,{6,1}]
{{Dzu1,Dzu2}}//MatrixForm

data={
a1->A1,a2->A2,a3->A3,a4->A4,
b1->B1,b2->B2,b3->B3,b4->B4,
c1->Cz1,c2->Cz2,
d1->Dzu1,d2->Dzu2,
bw1->Bw1,bw2->Bw2,
ww->W
};

{abc,rules}=NCSDP[FBlock,vars,obj,data,DebugLevel->0];

{Y,X,S,iters}=SDPSolve[abc,rules];

Construct the decentralized gain

{Z,X1,X2,L1,L2}=Y;

K1=L1.Inverse[X1]
K2=L2.Inverse[X2]

K=ArrayFlatten[{{K1,{{0,0}}},{{{0,0}},K2}}];
MatrixForm[K]

Evaluate closed - loop performance

Acl = Att+Butt.K;
Eigenvalues[Acl]

YY=LyapunovSolve[Acl,-Bwtt.W.Transpose[Bwtt]];
Map[Sqrt,Diagonal[YY]]

Try striping LQR gain

MatrixForm[Klqr]

Discard off - diagonals

Klqr1=Klqr[[{1},{1,2}]];
Klqr2=Klqr[[{2},{3,4}]];
KlqrDecen=ArrayFlatten[{{Klqr1,{{0,0}}},{{{0,0}},Klqr2}}];
MatrixForm[KlqrDecen]

Evaluate closed - loop performance

Acl = Att+Butt.KlqrDecen;
Eigenvalues[Acl]

YY=LyapunovSolve[Acl,-Bwtt.W.Transpose[Bwtt]];
Map[Sqrt,Diagonal[YY]]

Optimal LQR state feedback control (less control)

m = Length[Butt[[1]]];

LQR Solution

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

K = LQRegulatorGains[StateSpaceModel[{Att,Butt}],{Transpose[Cz].Cz,Transpose[Dzu].Dzu}];
Klqr = -K;
MatrixForm[Klqr]

LMIs for LQR with decentralization

{{Cz1,Cz2}}=Partition[Cz,{6,2}]
{{Cz1,Cz2}}//MatrixForm

{{Dzu1,Dzu2}}=Partition[Dzu,{6,1}]
{{Dzu1,Dzu2}}//MatrixForm

data={
a1->A1,a2->A2,a3->A3,a4->A4,
b1->B1,b2->B2,b3->B3,b4->B4,
c1->Cz1,c2->Cz2,
d1->Dzu1,d2->Dzu2,
bw1->Bw1,bw2->Bw2,
ww->W
};

{abc,rules}=NCSDP[FBlock,vars,obj,data];

{Y,X,S,iters}=SDPSolve[abc,rules];

Construct the decentralized gain

{Z,X1,X2,L1,L2}=Y;

K1=L1.Inverse[X1]
K2=L2.Inverse[X2]

K=ArrayFlatten[{{K1,{{0,0}}},{{{0,0}},K2}}];
MatrixForm[K]

Evaluate closed - loop performance

Acl = Att+Butt.K;
Eigenvalues[Acl]

System is closed - loop unstable!

Try striping LQR gain

MatrixForm[Klqr]

Discard off - diagonals

Klqr1=Klqr[[{1},{1,2}]];
Klqr2=Klqr[[{2},{3,4}]];
KlqrDecen=ArrayFlatten[{{Klqr1,{{0,0}}},{{{0,0}},Klqr2}}];
MatrixForm[KlqrDecen]

Evaluate closed - loop performance

Acl = Att+Butt.KlqrDecen;
Eigenvalues[Acl]//Re

System is closed - loop unstable!