Output tracking for nonlinear ODE

Question

I am trying to compute an output tracking controller for the following system.

ode = {x1', x2', x3', x4', x5', x6'} ==
      {Cos[x3] (Cos[x3 - x6] + 10 ((x4 - x1) Cos[x3] + (x5 - x2) Sin[x3])),
       Sin[x3] (Cos[x3 - x6] + 10 ((x4 - x1) Cos[x3] + (x5 - x2) Sin[x3])),
       u + 4/625 ((x5 - x2) Cos[x3] - (x4 - x1) Sin[x3]) - 4/25 Sin[x3 - x6],
       Cos[x6],
       Sin[x6],
       u}

First, I transform the ODE into an affine state space model using some output function I want to track.

model = AffineStateSpaceModel[
           NonlinearStateSpaceModel[{ode[[2]], x4 + x5 - x1 - x2},
              {x1, x2, x3, x4, x5, x6}, {u}]]

Then, I determine the number of decay rates for the output which is 2 in this case.

Total[SystemsModelVectorRelativeOrders[model]]

Afterwards, I am using AsymptoticOutputTracker to compute a controller that tracks the constant 1.

ctrl = AsymptoticOutputTracker[model, {1}, {-1, -2}]

Finally, I obtain the closed-loop model.

ctrlModel = SystemsModelStateFeedbackConnect[model, ctrl]

However, I can neither simulate the model nor transform this model into linear state space form.

StateSpaceModel[ctrlModel]

yields a number of errors including "Infinite expression 1/0 encountered".

OutputResponse[{ctrlModel, {0, 0, 0, 0, 0, 0}}, {0}, {t, 0, 1}]

does as well.

How can I solve this problem?

Answer 1

The controller blows up at the origin.

ctrl /. Thread[{x1, x2, x3, x4, x5, x6} -> 0]

During evaluation of In[88]:= Power::infy: Infinite expression 1/0 encountered.

{ComplexInfinity}

From a point not exactly at the origin, the desired tracking can be seen.

OutputResponse[{ctrlModel, {0.001, 1, 0, 0, 0, 0}}, {0}, {t, 0, 5}];
Plot[%, {t, 0, 5}]

It may be possible to avoid the singularity at the origin by choosing some other set of variables to model the system.