MAE 280 B - Linear Control Design
Mauricio de Oliveira
LQG Control

Hyperlink["http://fccr.ucsd.edu/~mauricio/courses/mae280b"]

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 *);

Gravitational force constant

k = G M;

Angular velocity (rad/s)

ω = Sqrt[k/((R0+r)^3)]

“Ground” velocity (m/s)

v = ω*(R0+r)

Linearized System Realization

A = {
{0,0,1,0},
{0,0,0,1},
{3(ω^2),0,0,2(r+R0)ω},
{0,0,((-2ω)/(r+R0)),0}
};
Bu = {
{0,0},
{0,0},
{(1/m),0},
{0,(1/(m(r+R0)))}
};
Cy={
{1,0,0,0},
{0,r+R0,0,0}
};
Dyu=ConstantArray[0,{2,2}];

Add noise input

Bw=Bu;
Dyw=ConstantArray[0,{2,2}];

BB=ArrayFlatten[{{Bu,Bw}}];
DD=ArrayFlatten[{{Dyu,Dyw}}];

linearizedSystem=StateSpaceModel[{A,BB,Cy,DD}]

Scaling

Apply similarity transformation

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

transformedLinearizedSystem=StateSpaceTransform[linearizedSystem,Inverse[T]]

SISO LQG Design (tangential control + θ measurement)

tanPlusThetaSystem=SystemsModelExtract[transformedLinearizedSystem,{2,3,4},2];

sisoSystem=SystemsModelExtract[tanPlusThetaSystem,1,1];

Map[ColumnForm,TransferFunctionPoles[sisoSystem],2]

Map[ColumnForm,TransferFunctionZeros[sisoSystem],2]

BodePlot[sisoSystem,GridLines->Automatic,PlotLayout->"List"]

Clear[k]
SystemsModelSeriesConnect[TransferFunctionModel[k],sisoSystem];
RootLocusPlot[%,{k,0,1},PlotRange->{-0.005,0.005}]

Two Riccati Equations Solution

{AA,BB,CC,DD}=Normal[tanPlusThetaSystem];

BBu=BB[[All,{1}]];
BBw=BB[[All,{2,3}]];

DDyu=DD[[All,{1}]];
DDyw=DD[[All,{2,3}]];

State - feedback

ρ=1;
Q=IdentityMatrix[4];
R=ρ IdentityMatrix[1];

KK=-LQRegulatorGains[StateSpaceModel[{AA,BBu}],{Q,R}];
MatrixForm[KK]

State Estimation

Ww=0.1IdentityMatrix[2];

Wv=0.1IdentityMatrix[1];

FF=-Transpose[LQRegulatorGains[StateSpaceModel[{Transpose[AA],Transpose[CC]}],{BBw.Ww.Transpose[BBw],Wv}]];
MatrixForm[FF]

Controller

sisoController=StateSpaceModel[{AA+BBu.KK+FF.CC,-FF,KK}]

LQG solution

LQGRegulator[{tanPlusThetaSystem,1,1},{Ww,Wv},{Q,R}]

Closed - loop Analysis

Add v component

BBwv=ArrayFlatten[{{BBw,ConstantArray[0,{4,1}]}}];
DDwv=ArrayFlatten[{{ConstantArray[0,{1,2}],IdentityMatrix[1]}}];
W=ArrayFlatten[{{Ww,0},{0,Wv}}];

BBwv.W.Transpose[DDwv]
DDwv.W.Transpose[DDwv]

Compute Cz and Dzu

Cz=ArrayFlatten[{{MatrixPower[Q,1/2]},{ConstantArray[0,{1,4}]}}];
Dzu=ArrayFlatten[{{ConstantArray[0,{4,1}]},{MatrixPower[R,1/2]}}];

Transpose[Cz].Dzu
Transpose[Dzu].Dzu

Closed - loop data

Acl=ArrayFlatten[{{AA,BBu.KK},{-FF.CC,AA+BBu.KK+FF.CC}}];
Bcl=ArrayFlatten[{{BBwv},{-FF.DDwv}}];
Ccl=ArrayFlatten[{{Cz,Dzu.KK}}];

Eigenvalues[Acl]//ColumnForm

Cost through Controllability Gramian

X=LyapunovSolve[Transpose[Acl],-Transpose[Ccl].Ccl];
Tr[Bcl.W.Transpose[Bcl].X]

State covariance matrix

Y=LyapunovSolve[Acl,-Bcl.W.Transpose[Bcl]];
Tr[Transpose[Ccl].Ccl.Y]

Sqrt[Tr[Y,List]]

Frequency - domain

BodePlot[sisoSystem,GridLines->Automatic,PlotLayout->"List"]

BodePlot[sisoController,GridLines->Automatic,PlotLayout->"List"]

G=SystemsModelSeriesConnect[sisoController,sisoSystem];BodePlot[G,GridLines->Automatic,PlotLayout->"List"]

Map[ColumnForm,TransferFunctionPoles[G],2]

Map[ColumnForm,TransferFunctionZeros[G],2]

Clear[k]
SystemsModelSeriesConnect[TransferFunctionModel[-k],G];
RootLocusPlot[%,{k,0,10},PlotRange->{-1,1},PlotPoints->500]

Clear[k]
SystemsModelSeriesConnect[TransferFunctionModel[-k],G];
RootLocusPlot[%,{k,0,10},PlotRange->{-.01,.01},PlotPoints->100]

MIMO LQG Design (tangential/radial control + r/θ measurement)

Two Riccati Equations Solution

{AA,BB,CC,DD}=Normal[transformedLinearizedSystem];

BBu=BB[[All,{1,2}]];
BBw=BB[[All,{3,4}]];

DDyu=DD[[All,{1,2}]];
DDyw=DD[[All,{3,4}]];

State - feedback

ρ=1;
Q=IdentityMatrix[4];
R=ρ IdentityMatrix[2];

KK=-LQRegulatorGains[StateSpaceModel[{AA,BBu}],{Q,R}];
MatrixForm[KK]

State Estimation

Ww=0.1IdentityMatrix[2];

Wv=0.1IdentityMatrix[2];

FF=-Transpose[LQRegulatorGains[StateSpaceModel[{Transpose[AA],Transpose[CC]}],{BBw.Ww.Transpose[BBw],Wv}]];
MatrixForm[FF]

Controller

mimoController=StateSpaceModel[{AA+BBu.KK+FF.CC,-FF,KK}]

LQG solution

LQGRegulator[{transformedLinearizedSystem,{1,2},{1,2}},{Ww,Wv},{Q,R}]

Closed - loop Analysis

Add v component

BBwv=ArrayFlatten[{{BBw,ConstantArray[0,{4,2}]}}];
DDwv=ArrayFlatten[{{ConstantArray[0,{2,2}],IdentityMatrix[2]}}];
W=ArrayFlatten[{{Ww,0},{0,Wv}}];

BBwv.W.Transpose[DDwv]
DDwv.W.Transpose[DDwv]

Compute Cz and Dzu

Cz=ArrayFlatten[{{MatrixPower[Q,1/2]},{ConstantArray[0,{2,4}]}}];
Dzu=ArrayFlatten[{{ConstantArray[0,{4,2}]},{MatrixPower[R,1/2]}}];

Transpose[Cz].Dzu
Transpose[Dzu].Dzu

Closed - loop data

Acl=ArrayFlatten[{{AA,BBu.KK},{-FF.CC,AA+BBu.KK+FF.CC}}];
Bcl=ArrayFlatten[{{BBwv},{-FF.DDwv}}];
Ccl=ArrayFlatten[{{Cz,Dzu.KK}}];

Eigenvalues[Acl]//ColumnForm

Cost through Controllability Gramian

X=LyapunovSolve[Transpose[Acl],-Transpose[Ccl].Ccl];
Tr[Bcl.W.Transpose[Bcl].X]

Eigenvalues[X]

State covariance matrix

Y=LyapunovSolve[Acl,-Bcl.W.Transpose[Bcl]];
Tr[Transpose[Ccl].Ccl.Y]

Sqrt[Tr[Y,List]]

Eigenvalues[Y]

Frequency - domain

BodePlot[transformedLinearizedSystem,GridLines->Automatic,PlotLayout->"List"]

BodePlot[mimoController,GridLines->Automatic,PlotLayout->"List"]

G=SystemsModelSeriesConnect[transformedLinearizedSystem,mimoController];BodePlot[G,GridLines->Automatic,PlotLayout->"List"]

GG=SystemsModelExtract[TransferFunctionModel[G],{1,2},{1,2}]

Map[ColumnForm,TransferFunctionPoles[GG],2]

Map[ColumnForm,TransferFunctionZeros[GG],2]

TransferFunctionCancel[TransferFunctionModel[GG]]