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

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)

Noise variances

W = 0.1 IdentityMatrix[2];

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}
};
Bw=Bu = {
{0,0},
{0,0},
{(1/m),0},
{0,(1/(m(r+R0)))}
};
Cy={
{1,0,0,0},
{0,r+R0,0,0}
};
linearizedSystem=StateSpaceModel[{A,Bu,Cy}]

Open loop system eigenvalues

Eigenvalues[A]

LQR Design with Radial Control

linearizedSystemRadial=SystemsModelExtract[linearizedSystem,{1},All]

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

Cheapest control (using radial control only)

ρ = 1. (10^6);
K=LQRegulatorGains[linearizedSystemRadial,{Q,ρ R}]

Recall the system is not controllable from the radial input!

LQR Design with Tangential Control

linearizedSystemTangential=SystemsModelExtract[linearizedSystem,{2},All]

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

Cheapest control (using tangential control only)

ρ = 1. (10^6);
K=LQRegulatorGains[linearizedSystemTangential,{Q,ρ R}]

The Riccati Solution X

X = RiccatiSolve[{A, Bu[[All,2;;2]]}, {Q, ρ R},Method->"Eigensystem"];
MatrixForm[X]

is badly scaled!

A better choice of coordinates

The transformation scales the angular coordinates

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

The new coordinate

y=Tx,    x=(T^(-1))y

has the same units.

linearizedSystem

We apply the similarity transformation

transformedLinearizedSystem=StateSpaceTransform[linearizedSystem,Inverse[T]]

and redo the design.

LQR Design with Tangential Control
(cheap control)

transformedLinearizedSystemTangential=SystemsModelExtract[transformedLinearizedSystem,{2},All]

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

Cheapest control (using tangential control only)

ρ = 1. (10^6);
Kt=-LQRegulatorGains[transformedLinearizedSystemTangential,{Q,ρ R}];
MatrixForm[Kt]

We compute the solution “by hand” by first computing the Riccati equation solution:

X = RiccatiSolve[{T.A.Inverse[T], T.Bu[[All,2;;2]]}, {Q, ρ R}];
MatrixForm[X]

then the feedback gain :

-Inverse[ρ R].Transpose[T.Bu[[All,2;;2]]].X;
MatrixForm[%]

Conditioning is good enough to get rid of the warnings :)

Closed loop performance

We can recompute the gains in the original coordinates

K = Kt.T

The closed loop eigenvalues are

Acl = A + Bu[[;;,2;;2]].K;
ColumnForm[Eigenvalues[Acl]]

which are all stable.

The optimal value of the cost function is:

J=Tr[X.T.Bw.W.Transpose[T.Bw]]

The state covariance matrix is

Y = LyapunovSolve[Acl,-Bw.W.Transpose[Bw]];
MatrixForm[Y]

from which we compute the state variance :

Sqrt[Tr[Y]]

Componentwise :

Map[Sqrt,Diagonal[Y]]

In anticipation of things to come :

J
Tr[(Q+ρ Transpose[Kt].R.Kt).T.Y.Transpose[T]]

In scaled coordinates :

Yt = LyapunovSolve[T.Acl.Inverse[T],-T.Bw.W.Transpose[T.Bw]];
MatrixForm[Yt]

Map[Sqrt,Diagonal[Yt]]

J
Tr[(Q+ρ Transpose[Kt].R.Kt).Yt]

LQR Design with Tangential Control
(with better performance)

transformedLinearizedSystemTangential=SystemsModelExtract[transformedLinearizedSystem,{2},All]

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

Not-so-cheap control (using tangential control only)

ρ = 1.;
Kt=-LQRegulatorGains[transformedLinearizedSystemTangential,{Q,ρ R}];
MatrixForm[Kt]

We compute the solution of the Riccati equation:

X = RiccatiSolve[{T.A.Inverse[T], T.Bu[[All,2;;2]]}, {Q, ρ R}];
MatrixForm[X]

for evaluating the closed loop performance

We can recompute the gains in the original coordinates

K = Kt.T

The closed loop eigenvalues are

Acl = A + Bu[[;;,2;;2]].K;
ColumnForm[Eigenvalues[Acl]]

which are all stable.

The optimal value of the cost function is:

J=Tr[X.T.Bw.W.Transpose[T.Bw]]

The state covariance matrix is

Y = LyapunovSolve[Acl,-Bw.W.Transpose[Bw]];
MatrixForm[Y]

from which we compute the state variance :

Sqrt[Tr[Y]]

Componentwise :

Map[Sqrt,Diagonal[Y]]

In anticipation of things to come :

J
Tr[(Q+ρ Transpose[Kt].R.Kt).T.Y.Transpose[T]]

In scaled coordinates :

Yt = LyapunovSolve[T.Acl.Inverse[T],-T.Bw.W.Transpose[T.Bw]];
MatrixForm[Yt]

Map[Sqrt,Diagonal[Yt]]

J
Tr[(Q+ρ Transpose[Kt].R.Kt).Yt]

LQR Design with Tangential and Radial Control
(with better performance)

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

Not-so-cheap control (using tangential control only)

ρ = 1.;
Kt=-LQRegulatorGains[transformedLinearizedSystem,{Q,ρ R}];
MatrixForm[Kt]

We compute the solution of the Riccati equation:

X = RiccatiSolve[{T.A.Inverse[T], T.Bu}, {Q, ρ R}];
MatrixForm[X]

for evaluating the closed loop performance

We can recompute the gains in the original coordinates

K = Kt.T

The closed loop eigenvalues are

Acl = A + Bu.K;
ColumnForm[Eigenvalues[Acl]]

which are all stable.

The optimal value of the cost function is:

J=Tr[X.T.Bw.W.Transpose[T.Bw]]

The state covariance matrix is

Y = LyapunovSolve[Acl,-Bw.W.Transpose[Bw]];
MatrixForm[Y]

from which we compute the state variance :

Sqrt[Tr[Y]]

Componentwise :

Map[Sqrt,Diagonal[Y]]

In anticipation of things to come :

J
Tr[(Q+ρ Transpose[Kt].R.Kt).T.Y.Transpose[T]]

In scaled coordinates :

Yt = LyapunovSolve[T.Acl.Inverse[T],-T.Bw.W.Transpose[T.Bw]];
MatrixForm[Yt]

Map[Sqrt,Diagonal[Yt]]

J
Tr[(Q+ρ Transpose[Kt].R.Kt).Yt]