Linear Quadratic Integrator

Question

With a given linear drive system

Needs["VariationalMethods`"]
params = {jb -> 870 10^-5 + 75 10^-6, p -> 2, mw -> 0.75, mb -> 10, 
   r1 -> 0.000001, r2 -> 0.000001, 
   jm -> 7800 0.002 \[Pi] (0.165/2)^4 1/2, r3 -> 0.165/2, k1 -> 1, 
   k2 -> 1}; 
displacement = 0;

ke = 1/2 mw (p/(2 \[Pi]) \[Alpha]'[t])^2 + 1/2 jm \[Alpha]'[t]^2  + 
   1/2 mb (p/(2 \[Pi]) \[Alpha]'[t])^2;
kp = mb g h1 + mb g (h1 + h2);
l = ke - kp;
eqns = EulerEquations[l, {\[Alpha][t]}, t] // FullSimplify;
deqns = {First@eqns[[1]] == -r1 \[Alpha]'[t] - f[t] + 4.3 u[t]};
MatrixForm[deqns]

symmodelqr = 
  StateSpaceModel[
    deqns, {{\[Alpha][t], 0}, {\[Alpha]'[t], 0}}, {u[t], 
     f[t]}, {\[Alpha]'[t] 2 \[Pi]/60, f[t]}, t] // FullSimplify;
model = symmodelqr /. params

Where $x(t) = \frac{\alpha(t)}{2\pi}$, $u(t)$ is an input between 1-10 to control a servo drive and $f(t)$ is a force disturbance.

I am attempting to design a linear quadratic integrator force controller,

In the same form as above.

I first designed a lqr controller on the system...a disturbance force will be measured, and with a given motor input u(t) the system should attempt to slow down the linear drive (or back off entirely), to reduce the forces, which appears to work well.

init = {0, 0};
times = RandomInteger[{10, 60}, 3];
disturb = (*{0,Table[1 UnitBox[(t-times[[i]])/
  25]\[ExponentialE]^(-1(t-times[[i]])^2),{i,1,Length@
  times}]};*)
  {0, 1 UnitBox[(t - 15)/25] - 1 UnitBox[(t - 40)/25]};
q = DiagonalMatrix[{1, 10}];
r = 1 {{1}};
lineargains = 
  Join[Last@CoefficientArrays[LQRegulatorGains[{model, 1}, {q, r}]] //
     Normal, {ConstantArray[0, 2]}];
First@lineargains
controlmodel = SystemsModelStateFeedbackConnect[model, lineargains];
{alphadot, force} = 
  StateResponse[{controlmodel, init}, disturb, {t, 60}];
Plot[{alphadot, force, disturb[[2]] }, {t, 0, 60}, 
 PlotLegends -> "Expressions", PlotRange -> All, FrameStyle -> Black, 
 Frame -> True]
(*{1., 5.47913}*)

I now have a specific speed profile, or rather, a force profile that I want to give as a reference signal $r$ Where the drive should attempt to keep a quadratically growing force reference tracked, again lowering or increasing the drive speed to keep this in check.

feedback = StateSpaceModel[{{}, {}, {{}}, {{1, -1}}}];
integrator = TransferFunctionModel[1/s, s];
tracker = 
 SystemsModelMerge@
  SystemsConnectionsModel[{feedback, integrator, 
    model}, {{1, 1} -> {2, 1}, {2, 1} -> {3, 1}, {3, 1} -> {1, 
      2}}, {{1, 1}, {3, 2}}, {{3, 1}, {3, 2}}]

My attempt at this however appears to produce nonsense.

init2 = {0, 25, 25};
times2 = RandomInteger[{10, 60}, 3];
disturb2 = {1 UnitStep[t], 
   0 UnitBox[(t - 10)/5] - 0 UnitBox[(t - 40)/5]};
q2 = DiagonalMatrix[{1000, 10, 100}];
r2 = 1 {{1}};
lineargains = 
  Join[Last@
     CoefficientArrays[LQRegulatorGains[{tracker, 1}, {q2, r2}]] // 
    Normal, {ConstantArray[0, 3]}];
First@lineargains
controlmodel2 = SystemsModelStateFeedbackConnect[tracker, lineargains];
{int, alphadot2, force2} = 
  StateResponse[{controlmodel2, init2}, disturb2, {t, 100}];
Plot[{int, alphadot2, force2, disturb2 }, {t, 0, 100}, 
 PlotLegends -> "Expressions", PlotRange -> All, FrameStyle -> Black, 
 Frame -> True, ImageSize -> Large]
(*{31.7673, 3.16228, 45.8435}*)

The Integrator, the way I understand it, should give and keep a tracked signal to the input of the SSM $u(t)$ To keep disturbance forces constant. However, it appears to not do this at all.

Clearly something is wrong. My suspicion is that I've either designed the wrong Statespacemodel in general for force control, or my inputs/outputs are incorrect, or that my SystemsConnectionModel is built incorrectly, However, I can't seem to get anything that makes sense. Are there better eyes out there than mine?

What is the correct way to build an LQI in mma?

(I am completely open to suggestions on how to do it differently or better, however ideally an LQI implementation would be preferred)

Answer 1

For this model it's not possible to design a controller that will track $\alpha '$.

{aa, bb, cc, dd} = Normal[model];
StateSpaceModel[{ArrayFlatten[{{0, {{0, -2 π/60}}}, {0, aa}}], ArrayFlatten[{{0}, {bb}}]}]
ControllableModelQ[%]

However a controller can be designed that tracks $\alpha$.

StateSpaceModel[{ArrayFlatten[{{0, {{1, 0}}}, {0, aa}}], ArrayFlatten[{{0}, {bb}}]}]
ControllableModelQ[%]

Because the controller is using state feedback you will also need an observer. An example of how to do all this can be seen in the 'Differential input controller design' section of the documentation for a path following robot. There the reference is $0$ for the robot to stay on course and the disturbance is the path.