10. Optimal Control Systems Design

10.1 Linear Quadratic Regulator

<< ControlSystems`

tank = 
      StateSpace[{{-0.01, 0}, {0, -0.02}}, {{1, 1}, {-0.004, 
            0.002}}, {{0.01, 0}, {0, 1}}]

tankd = ToDiscreteTime[%, Sampled -> Period[5]]

q = DiagonalMatrix[{ .01, 100}]

r = DiagonalMatrix[{2,  .5}]

LQRegulatorGains[tankd, q, r]

StateFeedbackConnect[tankd, %]

SimulationPlot[%, {0, DiscreteDelta[t]}, {t, 200}, 
        PlotStyle -> {Dashing[{ .1,  .01,  .01,  .01}], 
            Dashing[{ .01}]}, PlotRange -> All];

SimulationPlot[tankd, {0, DiscreteDelta[t]}, {t, 200}, 
        PlotStyle -> {Thickness[ .001], Dashing[{ .05}]}];

Show[%, %%, PlotLabel -> "Regulated Vs. Original System", 
        PlotRange -> All];

aa=(({{-3, 1}, {0, 2}}));

bb=(({{1}, {0}}));

q = DiagonalMatrix[{1. , 1. }]; r = {{1. }};

LQRegulatorGains[StateSpace[aa, bb], q, r]

aa=(({{-3, 1}, {0, -2}}));

LQRegulatorGains[StateSpace[aa, bb], q, r]

10.2 Optimal Output Regulator

tank = 
      StateSpace[{{-0.01, 0}, {0, -0.02}}, {{1, 1}, {-0.004, 
            0.002}}, {{0.01, 0}, {0, 1}}]

q = DiagonalMatrix[{10,  .01}]

r = DiagonalMatrix[{2,  .2}]

LQOutputRegulatorGains[tank, q, r]

10.3 Riccati Equations

StateSpace[TransferFunction[s, (1/(s^2))]]

aa = %[[1]]; 
    bb = %[[2]];

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

RiccatiSolve[aa, bb, q, r]

StateSpace[TransferFunction[s, (1/(s (s +  .5)))]]

ss = ToDiscreteTime[%, Sampled -> Period[1]]

aa = ss[[1]]; 
    bb = ss[[2]];

DiscreteRiccatiSolve[aa, bb, q, r]

Inverse[Transpose[bb] . % . bb + r] . Transpose[bb] . % . aa

LQRegulatorGains[ss, q, r]

aa=(({{0, 0, 0}, {(Q/τ), -(1/τ), 0}, {0, 1, 0}}));

bb=(({{1}, {0}, {0}}));

q = DiagonalMatrix[{(1/((Subscript[δ, 0])^2)), 0, (1/((Subscript[ϕ, 0])^2))}]

r = {{(1/((Subscript[u, 0])^2))}}

{τ -> 1, Q -> 10, 
      (Subscript[u, 0]) -> π, (Subscript[δ, 0]) -> (π/12.) , (Subscript[ϕ, 0]) -> (π/180.) }

{aa, bb, q, r} = {aa, bb, q, r} /. %;

RiccatiSolve @@ %

Inverse[r] . Transpose[bb] . %

LQRegulatorGains[StateSpace[aa, bb], q, r]

10.4 Discrete Regulator by Emulation of Continuous Design

\(satellite = StateSpace[TransferFunction[1\/#\^2]]\)

q = {{1, 0}, {0, 0}}; r = {{0.01}};

LQRegulatorGains[satellite, q, r]

StateFeedbackConnect[
        satellite, %][[1]]

poles = Eigenvalues[%]

CharacteristicPolynomial[%%, s] // Expand // Chop

tn = ((2  π)/(Sqrt[Coefficient]))[%, s, 0]

ts = (tn/6)

DiscreteLQRegulatorGains[satellite, q, r, 
      Sampled -> Period[ts]]

Eigenvalues[
      StateFeedbackConnect[
          ToDiscreteTime[satellite, Sampled -> Period[ts]], %] // 
        First]

Log[%]/ts

Abs[((% - poles)/poles)]

10.5 Optimal Estimation

antenna = 
      StateSpace[{{0, 1}, {0, -5}}, {{0, 0}, {1, 0.1}}, {{1, 
            0}}]

q = {{100}}; r = {{1}};

LQEstimatorGains[antenna, q, r, {1}]

antennad = 
      ToDiscreteTime[antenna, Sampled -> Period[ .1]]

qd = rd = {{10}};

LQEstimatorGains[antennad, qd, rd, {1}]

10.6 Discrete Estimator by Emulation of Continuous Design

antenna = 
      StateSpace[{{0, 1}, {0, -5}}, {{0, 0}, {1, 0.1}}, {{1, 
            0}}]

DiscreteLQEstimatorGains[antenna, q, r, {1}, 
      Sampled -> Period[ .1]]

10.7 Kalman Estimator

StateSpace[{{a}}, {{(Subscript[b, 1]), (Subscript[b, 2])}}, {{(Subscript[c, 1])}, {(Subscript[c, 2])}}, {{(Subscript[d, 11]), 
          (Subscript[d, 12])}, {(Subscript[d, 21]), (Subscript[d, 22])}}]

ll = {{l}};

KalmanEstimator[%%, ll, {1}, {1}]

StateSpace[{{a}}, {{(Subscript[b, 1]), (Subscript[b, 2])}}, {{(Subscript[c, 1])}, {(Subscript[c, 2])}}, {{(Subscript[d, 11]), 
          (Subscript[d, 12])}, {(Subscript[d, 21]), (Subscript[d, 22])}}, Sampled -> True]

KalmanEstimator[%, ll, {1}, {1}]

antenna

q = {{1}}; r = {{100}};

ll = LQEstimatorGains[antenna, q, r, {1}]

antenna1 = 
      ParallelConnect[antenna, TransferFunction[1], {}, {1, 1}]

estimator = KalmanEstimator[%, ll, {1}, {1}]

filter = Subsystem[estimator, All, {1}]

ParallelConnect[antenna1, filter, {1, 1}, {}]

composite = FeedbackConnect[%, {1, 4, Positive}]

Needs["Statistics`ContinuousDistributions`"]

n = 100;

w = 
        Table[Random[
            NormalDistribution[
              0, (Sqrt[q])[[1, 1]]]], {n}];

v = 
        Table[Random[
            NormalDistribution[
              0, (Sqrt[r])[[1, 1]]]], {n}];

u = Sin[(2/20) π Range[n]] // N;

zeros = Table[0, {n}];

{y1, y2} = OutputResponse[composite, {u, w, v, zeros}];

y10 = y1 - v;

\(\(MultipleListPlot[y10, y1, y2, PlotRange -> {\(-10\), 10}, 
        PlotJoined -> True, SymbolShape -> None, 
        PlotLabel -> "Signals prior to and after the Kalman Filter", 
        PlotLegend -> {"Original", "Noise Added", "Filtered"}];\)\)

10.8 Optimal Controller

StateSpace[{{a}}, {{(Subscript[b, 1]), (Subscript[b, 2]), (Subscript[b, 3])}}, {{(Subscript[c, 1])}, {(Subscript[c, 2])}}, {{(Subscript[d, 11]), 
          (Subscript[d, 12]), (Subscript[d, 13])}, {(Subscript[d, 21]), (Subscript[d, 22]), (Subscript[d, 23])}}]

ll = {{l}}; kk = {{k}};

Controller[%%, ll, kk, {1}, {2}, {1}]

StateSpace[{{a}}, {{(Subscript[b, 1]), (Subscript[b, 2]), (Subscript[b, 3])}}, {{(Subscript[c, 1])}, {(Subscript[c, 2])}}, {{(Subscript[d, 11]), 
          (Subscript[d, 12]), (Subscript[d, 13])}, {(Subscript[d, 21]), (Subscript[d, 22]), (Subscript[d, 23])}}, 
      Sampled -> True]

Controller[%, ll, kk, {1}, {2}, {1}] // Simplify

satellite = 
      ToDiscreteTime[StateSpace[{{0, 1}, {0, 0}}, {{0}, {1}}, {{1, 0}}], 
        Sampled -> Period[T]]

kk = {{(Subscript[k, 1]), (Subscript[k, 2])}}; ll = {{(Subscript[l, 1])}, {(Subscript[l, 2])}};

Controller[satellite, ll, kk]