M4-T6 — Tracking, Integral Action and Feedforward

Learning objectives

After this lesson, you should be able to:

• Compute the steady-state error of a state-feedback loop for a constant command and a constant disturbance.
• Augment the model with an integral state and design the gain by LQR.
• Explain the trade between the integral weight and the speed and overshoot of the response.
• Use feedforward for what is known and the integral for what is not.

The intuition

UAV M5-T16: Disturbance rejection and wind compensation added an integral term to a quadrotor controller as a recipe. This lesson shows why it is needed in the state-space framework and how to design it with the machinery of the last lessons, and the difference between the three things a tracking loop can do.

The problem with plain state feedback

A regulator $u = -\mathbf{K}\mathbf{x}$ makes the origin an equilibrium. To track a position command $r$ the usual first move is to feed back the error in that position: $u = -\mathbf{K}(\mathbf{x} - \mathbf{x}_r)$. That works when the model is exact and nothing pushes. A steady lateral acceleration of 0.5 m/s² (a wind, a thrust misalignment) moves the equilibrium: the loop settles where the gimbal effort of the correction balances the push, and that is 1.57 m for a 1 m command, an error of 0.57 m. Raising the gains shrinks the error but never removes it.

The integral state

The fix is to add a state that accumulates the position error, $\dot{x}_I = x - r$, and to include it in the feedback. At any steady state the derivative of every state is zero, including $x_I$; that forces $x = r$ no matter what constant disturbance acts. The loop finds by itself the constant effort that balances the wind and keeps whatever it needs in the integral. The new state is part of the model, so the gain is found by the same LQR design with a fifth weight, and the response follows from the closed-loop poles.

Feedforward for what is known

An integral state learns slowly. If part of the input is known (the thrust needed to hover, the acceleration that a planned trajectory calls for, a wind measured by a sensor) it should be applied directly, as a feedforward term, and the feedback left to deal with what remains. The feedforward does not change the stability of the loop; it makes the error that feedback has to remove small to begin with.

Feedback with an integral state makes the steady-state error zero for constant commands and disturbances, at the price of an extra state and a slower response. Feedforward makes the error small before feedback acts.

Definitions

Integral state $x_I$

The accumulated tracking error: $\dot{x}_I = y - r$ for the measured output $y$ and command $r$.

Augmented model

The plant states and $x_I$ together: $\dot{\mathbf{z}} = \mathbf{A}_a\mathbf{z} + \mathbf{B}_a u + \mathbf{B}_r r$ with $\mathbf{z} = (\mathbf{x}, x_I)$.

Internal model principle

To reject or follow a signal of a given type with zero error the loop must contain a model of that signal: an integrator for constants, an oscillator for sinusoids.

Feedforward $u_{ff}$

A term of the input computed from the command or from a measured disturbance, not from the error.

Derivation: the offset without an integral, and why the integral removes it

Hopper's lateral model with a constant wind acceleration $a_w$ entering the velocity equation and the gimbal acting through $\mathbf{B}$.

1. $$\dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}u + \mathbf{E}a_w, \qquad u = -\mathbf{K}\mathbf{x} + k_1 r$$

   The plant with a constant disturbance, and state feedback with a reference gain on the position command; the usual choice is $k_1 = K_1$, feeding back the position error.

2. $$\mathbf{0} = (\mathbf{A} - \mathbf{B}\mathbf{K})\mathbf{x}_{ss} + \mathbf{B}k_1 r + \mathbf{E}a_w$$

   At a steady state the derivative of the state is zero.

3. $$\mathbf{x}_{ss} = -(\mathbf{A} - \mathbf{B}\mathbf{K})^{-1}(\mathbf{B}k_1 r + \mathbf{E}a_w)$$

   The closed loop is stable, so its matrix is invertible and the steady state is unique. For Hopper this gives a position of 1.569 m for $r = 1$ and $a_w = 0.5$.

4. $$x_{ss} \ne r \text{ unless } \mathbf{E}a_w = 0 \text{ and the model is exact}$$

   The term in $a_w$ shifts the equilibrium. Any modelling error in $\mathbf{B}$ or a constant bias does the same.

With the integral state

1. $$\mathbf{0} = (\mathbf{A} - \mathbf{B}\mathbf{K}_x)\mathbf{x}_{ss} - \mathbf{B}K_I x_{I,ss} + \mathbf{E}a_w, \qquad 0 = \mathbf{C}\mathbf{x}_{ss} - r$$

   The augmented model at a steady state: the last row is the derivative of the integral state.

2. $$\mathbf{C}\mathbf{x}_{ss} = r$$

   The last row alone fixes the position at the command, whatever $a_w$ is. It cannot be satisfied unless the integral state has taken the value that balances the disturbance, and it does that by itself.

3. $$x_{I,ss} = \text{ whatever balances } \mathbf{E}a_w$$

   The remaining rows then determine $x_{I,ss}$ so that the input holds the effort that cancels the wind. The integral has memorized the disturbance.

An integrator in the loop forces the output to its command at any steady state, because the integrator can be at rest only when its input, the error, is zero. The internal model principle says the same for other signals: the loop must contain a model of what it is to follow.

A worked example

The offset and its removal, on the same Hopper lateral channel.

1. Without an integral. With the LQR gain of the last lessons and the position error fed back, a 0.5 m/s² wind acceleration and a 1 m command settle at 1.569 m, an error of +0.569 m, with the pitch held at −0.052 rad to balance the wind.
2. Augment. Add the integral of the position error as a fifth state and give it a weight $q_I$. Solve LQR on the five-state model.
3. Result for three weights. $q_I = 0.05$ gives gains (0.208, 0.307, 1.735, 0.379, 0.034), no overshoot, a 90% rise in 8.2 s and a slowest pole at −0.23. $q_I = 0.25$ gives 4.6 s and −0.62. $q_I = 1.0$ gives 3.3 s, a slowest pole at −0.92 and an overshoot of 0.76%.
4. All three end at a position error below 10⁻⁴ m after 40 s, simulated with the wind acting.
5. The trade. A larger weight on the integral state moves the slowest pole faster and shortens the rise, and at the high end begins to overshoot, because the integral winds up during the rise. The designer chooses the weight for the speed needed with the overshoot allowed.

import gnckit as g

gr, lam, b = 9.81, 0.7, 19.62
A = [[0, 1, 0, 0], [0, 0, gr, 0], [0, 0, 0, 1], [0, 0, lam ** 2, 0]]
B = [0, gr, 0, b]
Q = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 25, 0], [0, 0, 0, 4]]
R = [[1 / 0.15 ** 2]]
K = g.lqr(A, [[v] for v in B], Q, R)[0][0]
aw, r = 0.5, 1.0                                   # a constant lateral acceleration disturbance (m/s^2) and a 1 m position command
E = [0, 1, 0, 0]

Acl = g.ss_closed_loop(A, B, K)
rhs = [-(B[i] * K[0] * r + E[i] * aw) for i in range(4)]                   # steady state of x' = (A - BK) x + B K1 r + E aw
xss = g.mat_solve(Acl, rhs)
print(f'state feedback with no integral: the position settles at {xss[0]:.3f} m for a command of {r:.1f} m (error {xss[0] - r:+.3f} m), pitch {xss[2]:+.4f} rad')

A5 = [[A[i][j] if i < 4 and j < 4 else 0.0 for j in range(5)] for i in range(5)]
A5[4][0] = 1.0                                     # fifth state: the integral of the position error
B5 = B + [0.0]
print('with an integral state of weight qI:')
for qi in (0.05, 0.25, 1.0):
    Q5 = [[Q[i][j] if i < 4 and j < 4 else 0.0 for j in range(5)] for i in range(5)]
    Q5[4][4] = qi
    K5 = g.lqr(A5, [[v] for v in B5], Q5, R)[0][0]
    def f(t, s):
        u = -sum(K5[i] * s[i] for i in range(5))
        d = g.mat_vec(A5, s)
        out = [d[i] + B5[i] * u for i in range(5)]
        out[1] += aw
        out[4] -= r
        return out
    xs = g.integrate(f, [0.0] * 5, 40.0, 0.001)[1]
    t90 = next(k * 0.001 for k, s in enumerate(xs) if s[0] >= 0.9 * r)
    slow = min(abs(complex(p).real) for p in g.mat_eigvals(g.ss_closed_loop(A5, B5, K5)))
    print(f'  qI = {qi:4.2f}: position error after 40 s {xs[-1][0] - r:+.1e} m, overshoot {max(0.0, 100 * (max(s[0] for s in xs) / r - 1)):.2f}%, 90% in {t90:.1f} s, slowest pole {slow:.2f}')

state feedback with no integral: the position settles at 1.569 m for a command of 1.0 m (error +0.569 m), pitch -0.0523 rad
with an integral state of weight qI:
  qI = 0.05: position error after 40 s -6.8e-05 m, overshoot 0.00%, 90% in 8.2 s, slowest pole 0.23
  qI = 0.25: position error after 40 s -3.3e-11 m, overshoot 0.00%, 90% in 4.6 s, slowest pole 0.62
  qI = 1.00: position error after 40 s -2.3e-14 m, overshoot 0.76%, 90% in 3.3 s, slowest pole 0.92

In robotics

Almost every real tracking loop has an integral state or something equivalent.

• Thrust and mass errors. The weight of Hopper is known to a few per cent and falls as it burns. A constant bias in the hover thrust is what the vertical integral absorbs, which is what the I term of a PX4 or ArduPilot controller does too.
• Wind and aerodynamic forces. A steady crosswind or a slow change of an aerodynamic force is a constant disturbance on the time scale of the loop. The integral state counters it with no model of the wind.
• Windup. The integral keeps accumulating while the actuator is saturated, and then overshoots. The anti-windup measures of M3-T7: Digital Control: Sampling, Delay and Anti-Windup apply to the augmented state exactly as to a PID integral term.
• Feedforward from guidance. A guidance law that commands an acceleration along a trajectory should send that acceleration to the control loop as a feedforward term. The loop then follows a moving command with an error of the order of the model error, not of the acceleration itself.
• Cost in margin. The integrator adds phase lag at low frequency. The delay margin and the phase margin of the augmented loop must be checked, as in M3-T4: The Nyquist Criterion and Stability Margins.

The quadrotor discussion is in UAV M5-T15: Guidance and control integration; module 5 of this track uses feedforward again, as the inverse of a known nonlinearity.

Common misconceptions

Key takeaways

1. A state-feedback loop without an integral settles away from the command when a constant disturbance acts: Hopper settles at 1.569 m for a 1 m command with a 0.5 m/s² wind.
2. Adding the integral of the error as a state gives zero steady-state error for every constant command and constant disturbance.
3. The augmented gain is designed by LQR with a weight $q_I$ on the integral state; the integral weight sets the speed of the slowest pole.
4. For Hopper, $q_I = 0.05, 0.25, 1.0$ gives 90% rise times of 8.2, 4.6 and 3.3 s with overshoots of 0, 0 and 0.76%.
5. Feedforward applies what is known directly, and feedback with an integral removes what is not.
6. The integral adds low-frequency phase lag and windup, which the margins and the anti-windup of module 3 must cover.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• K. J. Åström and R. M. Murray, Feedback Systems, 2nd edition, Princeton University Press, 2021 , the chapters on state feedback (reference tracking, integral action). Free to read as the authors' PDF; the book is copyrighted.
• MIT 16.323 Principles of Optimal Control (OpenCourseWare) , the notes on the LQR with integral action and tracking. Free; licence CC BY-NC-SA 4.0.
• Control Tutorials for MATLAB and Simulink (University of Michigan, CMU, UCSD) , worked examples of state feedback with a reference gain and with integral action. Free to read; licence not stated on the site.
• G. F. Franklin, J. D. Powell and A. Emami-Naeini, Feedback Control of Dynamic Systems, 8th edition, Pearson, 2019, the sections on tracking and integral control in state space. Paid.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Related content

Nothing below is required to finish this lesson.

• Before
  M4-T5: LQG and Why the Guarantees Disappear

  Combine LQR with a Kalman filter, show by example that the result can have arbitrarily small margins, and recover them by loop-transfer recovery.

• Lab
  M4-L1: LQR for Hopper and Its Margins

  Design an LQR for Hopper's lateral channel (position, velocity, pitch, pitch rate), then break the loop at the gimbal and measure the margins it actually has with the actuator lag included.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M5-T7: LQR control
• UAV M5-L2: Implement LQR
• Navigation M5-T6: LQR and MPC for Path Tracking
• UAV M3-T4: Kalman Filter
• UAV M3-T12: Observability and consistency
• Linear Algebra M6-T1: Eigenvalues and Eigenvectors
• Linear Algebra M6-T4: Positive Definite Matrices
• Linear Algebra M9-T7: Linear Kalman Filter Mathematics
• Robot Learning M4-T4: Dynamic Programming and Value Iteration
• Probability for Robotics (module 7 derives the Kalman filter)

This course leaves the following to the pages that already teach them:

• The derivation of the Kalman filter
• LQR applied to a linearized quadrotor
• LQR applied to path tracking of a ground robot