Level 6 · Signals, Systems & Control: State and Estimation

Level 5 ended by writing a mass on a spring as a vector, its loop's poles the eigenvalues of one matrix. This last level works that way throughout. A system's state is everything about its past that matters for its future, and x_{k+1} = A x_k + B u_k steps any linear system forward with one matrix product. A controller that feeds back the whole state can put the poles where it likes, or, with LQR, where a stated price of error and effort says they belong.

Sensors see only part of the state, with noise, so the second half estimates the rest. The Kalman filter blends a model's prediction with each reading in proportion to how far each can be trusted. Last comes the model itself, fitted to a log by least squares, and refitted over a moving window when the system changes.

The other paths meet here. The Kalman filter is the Statistics path's estimation: it weighs two estimates by the inverses of their variances. LQR minimises a quadratic cost, as the Machine Learning path's optimisers do, and system identification is that path's least-squares regression.

State: everything the future needs

A system in state-space form is

The first equation moves the state on: x_k is a column of n numbers, A an n \times n matrix and B a column. The second is what the sensor reads, with C a row. Row i of A says how the next value of state i depends on all of today's, x_{k+1,i} = \sum_j A_{ij} x_{k,j} + B_i u_k, which numpy writes A @ x + B * u. Two tanks in a row, a pump filling the upper one and a sensor only in the lower:

import numpy as np
A = np.array([[0.9, 0.0], [0.08, 0.95]])      # per minute: the upper tank keeps 90 %, 8 % falls into the lower
B = np.array([0.5, 0.0])                      # the pump fills the upper tank
C = np.array([0.0, 1.0])                      # only the lower tank has a sensor
x = np.zeros(2)
ys = [float(C @ x)]
for u in [1] * 5 + [0] * 35:                  # the pump runs for 5 minutes
    x = A @ x + B * u
    ys.append(float(C @ x))
print(np.round(ys[:7], 3).tolist(), f"peak {max(ys):.3f} m at minute {int(np.argmax(ys))}")
plot(ys, kind="step", xlabel="minute", ylabel="lower tank (m)")

[0.0, 0.0, 0.04, 0.114, 0.217, 0.343, 0.49] peak 1.031 m at minute 16

The lower tank peaks eleven minutes after the pump stops: the unmeasured upper tank is still draining. Build the new state entirely from the old; with lists, never overwrite x[0] while x[1] still needs it.

Practice: A Lift's State, One Step at a Time

Companion form, and back

An equation of order n needs n states. With a[0] = 1 (divide through first) and b[0] = 0, the companion form puts the negated a[1:] in the first row of A, ones just below the diagonal to shift the memory down, B = [1, 0, ...] and C = b[1:]. A shock absorber rig, y[k] = 1.2 y[k-1] - 0.5 y[k-2] + 0.3 u[k-1] + 0.1 u[k-2], kicked once:

import numpy as np
a, b = [1, -1.2, 0.5], [0, 0.3, 0.1]          # sampled every 5 ms
A = np.array([[-a[1], -a[2]], [1.0, 0.0]])
B, C = np.array([1.0, 0.0]), np.array(b[1:])
x, ys = np.zeros(2), []
for u in [1, 0, 0, 0, 0, 0]:
    ys.append(float(C @ x))
    x = A @ x + B * u
print(np.round(ys, 4).tolist(), np.round(np.poly(A), 4).tolist())

[0.0, 0.3, 0.46, 0.402, 0.2524, 0.1019] [1.0, -1.2, 0.5]

The equation gives the same: 0.3, then 1.2 * 0.3 + 0.1 = 0.46, and so on. Back again, for any A: the denominator is np.poly(A), the characteristic polynomial \det(zI - A). The numerator comes from the impulse response h_0 = 0, h_m = C A^{m-1} B (a kick puts B into the state, and A carries it on), since a convolved with h is b. For the tanks, a = [1, -1.85, 0.855] and h starts 0, 0, 0.04, so b = [0, 0, 0.04]: two samples of dead time.

Common mistakes: forgetting the minus signs on a; dividing a but not b by a[0].

Practice: A Flight Log's Equation in Companion Form, The Crane Model Back as a Difference Equation

Eigenvalues: poles and modes

The eigenvalues of A are the poles, and each comes with an eigenvector: a state along eigenvector v stays along it, multiplied by the eigenvalue z every step. Any start is a mix of eigenvectors, x_0 = \sum_i c_i v_i, so

with eigenvalues z_i and eigenvectors v_i (the columns of V): each mode shrinks by |z_i| and turns by \angle z_i per step, independently. In numpy the sum is V @ (c * z ** k). A complex pair rings at \angle z / (2\pi\,\Delta t) hertz.

import numpy as np
dt = 0.005
A = np.array([[1.2, -0.5], [1.0, 0.0]])       # the rig
z, V = np.linalg.eig(A)                       # eigenvalues; eigenvectors as columns
for p in z[z.imag > 0]:
    print(f"pole {p:.4f}: radius {abs(p):.4f}, rings at {np.angle(p) / (2 * np.pi * dt):.1f} Hz")
x0 = np.array([1.0, 0.0])
c = np.linalg.solve(V, x0)                    # x0 as a mix of the modes
print(np.round((V @ (c * z ** 7)).real, 6).tolist(), np.round(np.linalg.matrix_power(A, 7) @ x0, 6).tolist())

pole 0.6000+0.3742j: radius 0.7071, rings at 17.7 Hz
[-0.161779, -0.163016] [-0.161779, -0.163016]

An eigenvalue of exactly 1 is something that can drift without limit, such as a position.

Practice: How a Train of Wagons Rings

Controllability and stabilisability

Can the input reach every mode? From rest, one step reaches multiples of B, two add A B, and after n steps nothing new appears. So the controllability matrix

(those vectors as columns) must have rank n. Chlorine dosed into a swimming pool that overflows into a paddling pool:

import numpy as np
B = np.array([1.0, 0.0])                      # dosed into the main pool only
for pipe in (0.04, 0.0):                      # the overflow open or shut
    A = np.array([[0.95 - pipe, 0.0], [pipe, 0.97]])
    W = np.column_stack([B, A @ B])
    print(f"pipe {pipe}: W = {np.round(W, 4).tolist()}, rank {np.linalg.matrix_rank(W)},"
          f" singular values {np.round(np.linalg.svd(W)[1], 4).tolist()}")

pipe 0.04: W = [[1.0, 0.91], [0.0, 0.04]], rank 2, singular values [1.3523, 0.0296]
pipe 0.0: W = [[1.0, 0.95], [0.0, 0.0]], rank 1, singular values [1.3793, 0.0]

The rank counts the singular values clearly above zero, relative to the largest; the small 0.0296 warns that steering through a thin pipe takes large doses. With the pipe shut, the paddling pool's chlorine decays by 0.97 a step whatever the dosing: an uncontrollable mode. The columns U2 of U (from np.linalg.svd(W)) beyond the rank span the unreachable directions, and the eigenvalues of U2.T @ A @ U2 are the uncontrollable modes. If they lie inside the unit circle they fade alone and the system is stabilisable; otherwise no controller can save it.

Practice: Two Rooms, One Radiator

State feedback: A - B K

State feedback sets u_k = -K x_k, one gain per state. Then

one closed-loop matrix, whose eigenvalues are the loop's poles. BK is the outer product, entry (i, j) equal to B_i K_j. A robot arm's joint:

import numpy as np
A = np.array([[1.0, 0.05], [0.0, 0.9]])       # [angle (rad), rate (rad/s)], dt = 0.05 s
B = np.array([0.001, 0.04])                   # a torque command
for K in ([0, 0], [20, 2], [100, 8], [600, 20]):
    z = np.linalg.eigvals(A - np.outer(B, K))
    r, deg = max(abs(z)), max(abs(np.degrees(np.angle(z))))
    print(f"K = {str(K):9}: largest radius {r:.3f}, angle {deg:4.1f} degrees")

K = [0, 0]   : largest radius 1.000, angle  0.0 degrees
K = [20, 2]  : largest radius 0.918, angle 11.2 degrees
K = [100, 8] : largest radius 0.831, angle 27.0 degrees
K = [600, 20]: largest radius 0.872, angle 73.3 degrees

Without feedback the angle has a pole at 1: it stays wherever it is pushed. Stiffer gains are faster, then ring; the largest radius, the spectral radius, must stay below 1. K[0] is a spring and K[1] a brake, as in Level 4's PD.

Common mistakes: A + B K. np.outer(K, B), the transpose. B * K, element by element.

Practice: The Drone's Height Loop as One Matrix

Placing two poles

Now choose the poles and find the gains. A 2 by 2 matrix M has the characteristic polynomial z^2 - \operatorname{tr}(M)\, z + \det(M); poles p_1, p_2 want z^2 - (p_1 + p_2)\, z + p_1 p_2. trace(A - B K) and det(A - B K) are each a constant plus multiples of k1 and k2 (the k1 k2 terms cancel), so matching them gives two linear equations. numpy can find the multiples by trying each gain alone:

import numpy as np
A = np.array([[1.0, 0.05], [0.0, 0.9]])
B = np.array([0.001, 0.04])
coef = lambda K: np.poly(A - np.outer(B, K))[1:]              # [-trace, det]
c0 = coef([0, 0])
M = np.column_stack([coef([1, 0]) - c0, coef([0, 1]) - c0])   # what each gain adds
want = np.real(np.poly([0.8 + 0.1j, 0.8 - 0.1j]))[1:]         # z^2 - 1.6 z + 0.65
K = np.linalg.solve(M, want - c0)
print(np.round(K, 4).tolist(), np.round(np.linalg.eigvals(A - np.outer(B, K)), 6).tolist())

[23.8095, 6.9048] [(0.8+0.1j), (0.8-0.1j)]

By hand, write A - B K out entry by entry and solve the pair with Cramer's rule; a zero determinant means a mode is out of reach. Poles at 0 give deadbeat control: on target after n steps, with large commands.

Practice: Placing the Lift's Two Poles by Hand

Ackermann's formula

For n states, Ackermann's formula matches every coefficient at once:

with W the controllability matrix and \varphi the polynomial with the wanted poles. \varphi(A) is the polynomial evaluated at the matrix, powers of A for powers of z and the constant times the identity; Horner's rule starts from zeros and sets P = P @ A + c * I for each coefficient c. A check: by the Cayley-Hamilton theorem every matrix satisfies its own characteristic polynomial, so phi = np.poly(A) gives all zeros. Find the last row of W^-1 by solving W.T q = [0, ..., 0, 1], after testing the rank of W.

Practice: Ackermann's Formula for the Drone and the Pendulum

Tracking a reference

To hold the output at r instead of 0, add feedforward: u_k = -K x_k + N r. A stable loop settles where x = (A - BK)\,x + BNr, so

and N = 1 / g lands exactly on r. The arm, now with a return spring:

import numpy as np
A = np.array([[0.995, 0.05], [-0.2, 0.9]])    # a spring pulls the arm back to 0
B, C = np.array([0.001, 0.04]), np.array([1.0, 0.0])
K, r = np.array([100.0, 8.0]), 0.5            # go to 0.5 rad
g = C @ np.linalg.solve(np.eye(2) - A + np.outer(B, K), B)
for N in (0.0, K[0], 1 / g):
    x = np.zeros(2)
    for k in range(200):                      # 10 seconds
        x = A @ x + B * (N * r - K @ x)
    print(f"N = {N:5.1f}: angle after 10 s {x[0]:.4f} rad")

N =   0.0: angle after 10 s 0.0000 rad
N = 100.0: angle after 10 s 0.4762 rad
N = 105.0: angle after 10 s 0.5000 rad

N = K[0] cancels the feedback's spring but not the real one; the extra 5 is the torque that holds the arm against it. If g is 0, no N helps. A wrong model makes N wrong, which is why real loops add Level 3's integral.

Practice: Sending the Lift to the Third Floor

LQR: the Riccati iteration

The linear-quadratic regulator asks not where the poles go but what you care about, and minimises

Q prices error in each state, R prices effort; only their ratio matters. A stable loop's cost from x_0 is x_0^\top P x_0, and the best P is the fixed point of the discrete Riccati equation, iterated from P = Q:

The first is the best gain if the future costs x^\top P x; the second is one step's cost plus that future.

Each round looks one step further ahead, until nothing changes. With one state it is arithmetic. A maglev's air gap error, which grows 10 % a step if left alone:

a, b, q = 1.1, 0.5, 1.0
for r in (0.01, 1.0, 100.0):
    P = q
    for _ in range(500):
        K = a * b * P / (r + b * b * P)
        P = q + a * P * (a - b * K)
    K = a * b * P / (r + b * b * P)
    print(f"r = {r:6}: K = {K:.4f}, pole {a - b * K:.4f}, cost from x0 = 1: {P:.3f}")

r =   0.01: K = 2.1190, pole 0.0405, cost from x0 = 1: 1.047
r =    1.0: K = 0.9643, pole 0.6179, cost from x0 = 1: 3.121
r =  100.0: K = 0.4021, pole 0.8989, cost from x0 = 1: 89.471

Cheap effort puts the pole near 0; dear effort gives the gentlest gain that still works, its pole near 1 / a, the unstable pole reflected inside the circle. Any other gain costs more: price it by iterating P \leftarrow S + A_{\text{cl}}^\top P A_{\text{cl}}, with A_{\text{cl}} = A - BK and S = Q + K^\top R K.

Practice: The Best Gain for the Lift, by Riccati, Hold the Drone Still in a Gusty Hall

Looking ahead: a tiny MPC

LQR assumes any command is possible. A model-predictive controller tries plans u_0, \dots, u_{H-1} of H steps within the limits, predicts and prices each with the model, \sum_{i=1}^{H} \left( (h_i - h_{\text{want}})^2 + \rho\, u_{i-1}^2 \right) here, and keeps the cheapest. A water tower with zero, one or two pumps:

import itertools
def cost(h, plan, rho=0.01):                  # the town draws 0.25 m a step; each pump adds 0.2 m
    total = 0.0
    for u in plan:
        h = h + 0.2 * u - 0.25
        total += (h - 3.0) ** 2 + rho * u * u # 3 m is wanted
    return total
for h in (2.0, 2.9, 3.5):
    best = min(itertools.product((0, 1, 2), repeat=4), key=lambda plan: cost(h, plan))
    print(f"level {h} m: plan {best}, predicted cost {cost(h, best):.3f}")

level 2.0 m: plan (2, 2, 2, 2), predicted cost 1.835
level 2.9 m: plan (2, 1, 1, 1), predicted cost 0.085
level 3.5 m: plan (0, 0, 1, 1), predicted cost 0.095

Only the plan's first move is applied; then the controller measures and plans afresh. This receding horizon corrects disturbances as feedback does. Too short a horizon cannot see that braking must start early; too long costs levels ** H predictions a step, and real MPC optimises instead.

Practice: A Camera Dolly That Looks Ahead

The Kalman filter in one dimension

The estimate x comes with its variance p, the square of how unsure it is. Each step has two halves. Predict: x becomes the model's next value and p \leftarrow p + q, where q is how much the truth can change in a step. Update with a reading z whose variance is r:

The gain K lies between 0 and 1: the estimate moves part of the way to the reading, and ends more certain than either source.

A quantity that stays put predicts itself. The update is the Statistics rule for combining two independent estimates, weighted by the inverses of their variances, \hat{x} = \frac{x/p + z/r}{1/p + 1/r} with variance \frac{1}{1/p + 1/r}:

x, p = 14.0, 0.5                              # a buoy's sea temperature (°C) and its variance
z, r = 14.6, 0.3                              # a reading and the sensor's variance
K = p / (p + r)
print(f"gain {K:.3f}: x = {x + K * (z - x):.4f}, p = {(1 - K) * p:.4f}")
w = (x / p + z / r) / (1 / p + 1 / r)
print(f"inverse-variance weights: x = {w:.4f}, p = {1 / (1 / p + 1 / r):.4f}")

gain 0.625: x = 14.3750, p = 0.1875
inverse-variance weights: x = 14.3750, p = 0.1875

For a constant (q = 0) and a start that knows nothing (p huge), the filter is the running mean, with p = r / n after n readings: the squared standard error.

Practice: A Kitchen Scale That Grows More Sure

The steady-state gain

The lines for p never use the readings, so the gains can be computed in advance, and they settle. At steady state the predicted variance m returns after one update and one prediction: m = \frac{m r}{m + r} + q, so m^2 - q m - q r = 0 and

From then on the filter is x \leftarrow x + K\,(z - x), Level 2's exponential average with its weight chosen by the noise. The gain depends only on the ratio q / r: 0.031 at 0.001, 0.095 at 0.01, 0.270 at 0.1, 0.618 at 1 and 0.916 at 10. Iterating the three lines from any start converges to the same value:

q, r = 0.01, 1.0
for p0 in (0.0, 5.0):                         # two different starting variances
    p, gains = p0, []
    for k in range(40):
        p = p + q                             # predict
        K = p / (p + r)
        gains.append(K)
        p = (1 - K) * p                       # update
    plot(gains, label=f"p0 = {p0}", xlabel="reading", ylabel="gain K")

Press Run: one start knows the value, the other knows almost nothing, and after about 30 readings both gains are close to 0.095, the formula's value for q / r = 0.01.

Practice: How Far to Trust the Cellar Thermometer

Model against sensor: q and r

q says how far the model may be wrong in a step, r how far the sensor may be. A large q ("the truth wanders") keeps the gain high: the filter follows the readings, noise and all. A large r ("the sensor is poor") keeps it low: the filter smooths, and lags. With q = 0 the gain falls towards 0 for ever: told that nothing changes, the filter stops listening. r can be measured on something known to be still; q is the tuning knob, and too small a q, which looks smooth, is the commoner mistake.

Missing readings

When a reading does not come, predict and skip the update: p grows by q, so the next reading gets a larger gain. A river gauge with q = 0.02, r = 0.5 settles at K = 0.181; after three lost readings the predicted variance is 0.170 instead of 0.111, and the next gain is 0.254. Never feed a missing reading in as 0.

Practice: Following a Greenhouse as It Warms

Complementary filtering

A gyroscope gives a smooth turn rate, but its small bias integrates into drift; a compass never drifts, but jumps about. The complementary filter takes the gyro's short-term change and the compass's long-term level:

with heading \psi_k, gyro rate \omega_k, compass reading c_k and a weight \alpha just below 1. It low-passes the compass and high-passes the summed gyro, with time constant \alpha\,\Delta t / (1 - \alpha); a gyro bias b leaves a steady error of \alpha b\,\Delta t / (1 - \alpha). A dinghy tacking:

import math, random
rng = random.Random(3)
dt, n = 0.1, 1200                             # two minutes at 10 Hz
true = [40 + 30 * math.sin(2 * math.pi * k * dt / 40) for k in range(n)]
gyro = [(true[k] - true[k - 1]) / dt + 0.5 + rng.gauss(0, 0.2) if k else 0.0 for k in range(n)]   # bias 0.5 °/s
compass = [v + rng.gauss(0, 4) for v in true]
for alpha in (0.0, 0.9, 0.98, 0.995, 1.0):    # 0: the compass alone; 1: the gyro alone
    h, sq = compass[0], 0.0
    for k in range(1, n):
        h = alpha * (h + gyro[k] * dt) + (1 - alpha) * compass[k]
        sq += (h - true[k]) ** 2
    print(f"alpha {alpha:5}: RMS error {math.sqrt(sq / (n - 1)):5.2f} degrees")

alpha   0.0: RMS error  4.01 degrees
alpha   0.9: RMS error  1.21 degrees
alpha  0.98: RMS error  2.66 degrees
alpha 0.995: RMS error  8.25 degrees
alpha   1.0: RMS error 27.65 degrees

The bias costs 0.45° at alpha = 0.9 but 9.95° at 0.995; with a better gyro a larger alpha would win.

Common mistakes: blending headings across 359° and 0° as plain numbers. Mixing degrees and radians.

Practice: Tilt of a Balancing Robot From Two Cheap Sensors

Gating outliers by the innovation

The innovation z - x is what a reading says that the prediction did not. If the model is right its variance is S = p + r, so the normalised innovation d = (z - x) / \sqrt{S} rarely leaves -3 to 3. A reading beyond the gate is rejected: the prediction stands, with no update.

import math
x, p, r = 142.0, 4.0, 9.0                     # a heart-rate strap: predicted 142 bpm
S = p + r
for z in (145, 151, 284):                     # 284: every beat counted twice
    d = (z - x) / math.sqrt(S)
    print(f"reading {z}: innovation {z - x:+6.1f}, d = {d:+6.2f} ->", "rejected" if abs(d) > 3 else "used")

reading 145: innovation   +3.0, d =  +0.83 -> used
reading 151: innovation   +9.0, d =  +2.50 -> used
reading 284: innovation +142.0, d = +39.38 -> rejected

Test with the predicted p, before updating. With bell-curve noise a gate of 3 rejects about 3 good readings in 1000. While readings are rejected p keeps growing, so a genuine jump is eventually let through.

Practice: The Tank Gauge That Sometimes Hears the Wrong Echo

Two states: position and velocity

A Kalman filter can estimate a state that nothing measures. In a constant-velocity model the state is [position, velocity] and everything is a matrix:

with H = \begin{bmatrix} 1 & 0 \end{bmatrix}: only the position is read.

P is the covariance matrix: variances on the diagonal, and off it how the two errors move together.

import numpy as np
dt, q, r = 0.2, 0.05, 0.0025                  # a curling stone filmed from above
F = np.array([[1, dt], [0, 1]])
Q = q * np.array([[dt ** 3 / 3, dt ** 2 / 2], [dt ** 2 / 2, dt]])
P = np.diag([0.0004, 0.25])                   # position known to 2 cm, speed to 0.5 m/s
for k in range(3):                            # the camera blocked for three frames: predict only
    P = F @ P @ F.T + Q
    print(f"frame {k + 1}: P = {np.round(P, 4).tolist()}, correlation {P[0, 1] / np.sqrt(P[0, 0] * P[1, 1]):.3f}")
K = P[:, 0] / (P[0, 0] + r)                   # the gain for the next reading
print(f"gain {np.round(K, 3).tolist()}: a reading 0.1 m beyond the prediction adds {0.1 * K[1]:.3f} m/s")

frame 1: P = [[0.0105, 0.051], [0.051, 0.26]], correlation 0.975
frame 2: P = [[0.0415, 0.104], [0.104, 0.27]], correlation 0.983
frame 3: P = [[0.094, 0.159], [0.159, 0.28]], correlation 0.980
gain [0.974, 1.648]: a reading 0.1 m beyond the prediction adds 0.165 m/s

Prediction correlates the errors: a stone further on than expected is probably faster too. That covariance gives the velocity a gain, so a position reading corrects the speed.

Practice: Reading a Lift's Speed From a Rangefinder

Dead reckoning and fusion

Dead reckoning adds up measured motion, such as wheel turns: smooth and always available, but every error accumulates. A position fix is noisy but does not drift. Fusion predicts with the measured motion, x \leftarrow x + v\,\Delta t and p \leftarrow p + q, and updates with each fix:

q, r = 0.02, 0.09                             # a robot mower: odometry adds 0.02 m² a second; a beacon fix has 0.09 m²
p, before, trace = 0.0, [], []
for t in range(1, 61):
    p += q
    if t % 15 == 0:                           # a fix every 15 s
        before.append(round(p, 3))
        p = p * r / (p + r)                   # (1 - K) p
    trace.append(p)
print("variance at each fix", before, "after it", round(p, 3))
print("dead reckoning with a 3 % scale error at 0.5 m/s is off by", [round(0.015 * t, 2) for t in (15, 30, 45, 60)], "m")
plot(range(1, 61), trace, kind="step", xlabel="seconds", ylabel="variance (m²)")

variance at each fix [0.3, 0.369, 0.372, 0.372] after it 0.072
dead reckoning with a 3 % scale error at 0.5 m/s is off by [0.22, 0.45, 0.67, 0.9] m

Press Run for the sawtooth: the fused uncertainty is cut at each fix and stays bounded, while dead reckoning's error grows without limit.

Practice: A Cyclist in a Tunnel: Wheel Count or GPS?

Identifying a model by least squares

Level 2 fitted a model from two pairs of samples; with noise, use them all. Each k says "row [y[k-1], u[k-1]] times [a, b] equals y[k]", and np.linalg.lstsq finds the a and b that minimise the squared misses,

the Machine Learning path's linear regression. Judge the model on a different log, one step ahead from measured outputs and in simulation from its own:

import numpy as np, random
def log(n, seed):                             # an e-bike on a test roller: assist level u, speed y (m/s)
    rng, v, u, y = random.Random(seed), 0.0, [], []
    for k in range(n):
        u.append(rng.choice((0, 1, 2, 3)) if k % 10 == 0 else u[-1])
        y.append(round(v + rng.gauss(0, 0.05), 2))
        v = 0.85 * v + 0.6 * u[-1]            # the truth, hidden from the fit
    return np.array(u), np.array(y)
u, y = log(200, 1)
(a, b), *_ = np.linalg.lstsq(np.column_stack([y[:-1], u[:-1]]), y[1:], rcond=None)
print(f"a = {a:.4f}, b = {b:.4f}")
u, y = log(100, 2)                            # a fresh day
one = a * y[:-1] + b * u[:-1]
sim = [y[0]]
for k in range(1, len(y)):
    sim.append(a * sim[-1] + b * u[k - 1])
rms = lambda e: float(np.sqrt(np.mean(e ** 2)))
print(f"one-step RMS {rms(one - y[1:]):.3f} m/s, simulation RMS {rms(np.array(sim[1:]) - y[1:]):.3f} m/s")

a = 0.8493, b = 0.6028
one-step RMS 0.071 m/s, simulation RMS 0.053 m/s

The fit recovers 0.85 and 0.6. With the right model the simulation is off only by the sensor noise, while one step ahead also inherits the noise in y[k-1]; a wrong model looks fine one step ahead and piles up errors in simulation. The input must excite the system: a log in which it hardly moves fits numbers to noise.

Practice: A Model of the Hot-Water Cylinder, Tested on a New Day

Moving-horizon estimation

When a parameter changes, one fit over the whole log averages old and new into a value wrong for both. A moving-horizon estimate refits over only the latest w samples at every step. A scooter battery's voltage is V = E - R\,i for a current i, and the cold raises R:

import numpy as np, random
rng = random.Random(4)
amps = np.array([rng.uniform(0, 5) for _ in range(200)])           # once a second
R_true = np.where(np.arange(200) < 100, 0.050, 0.080)              # cold from 100 s on
volts = np.array([3.7 - R * i + rng.gauss(0, 0.005) for R, i in zip(R_true, amps)])
rows = np.column_stack([np.ones(200), -amps])                      # volts = E * 1 + R * (-amps)
def fit(lo, hi):
    return np.linalg.lstsq(rows[lo:hi], volts[lo:hi], rcond=None)[0]
for end in (90, 105, 110, 120, 200):
    print(f"window ending at {end:3} s: R = {fit(end - 20, end)[1] * 1000:5.1f} mΩ")
print(f"one fit over everything:   R = {fit(0, 200)[1] * 1000:5.1f} mΩ")

window ending at  90 s: R =  50.1 mΩ
window ending at 105 s: R =  55.8 mΩ
window ending at 110 s: R =  62.9 mΩ
window ending at 120 s: R =  79.9 mΩ
window ending at 200 s: R =  80.5 mΩ
one fit over everything:   R =  67.8 mΩ

After 20 s the window has moved over entirely; a threshold on the estimate makes an alarm. A short window follows quickly but is noisy, Kalman's trade again, and a Kalman filter whose state holds the parameters, with q > 0, is the same idea without the hard edge.

Practice: Spotting the Open Window From the Heating Log

Where to go next

The path ends here; the subject does not. For nonlinear systems, the extended and unscented Kalman filters linearise around the estimate, and a particle filter carries a cloud of samples, the Statistics path's Monte Carlo. Robust control asks how wrong a model may be before a loop fails, and reinforcement learning, in the Machine Learning path, is optimal control when nobody gives you A and B. Packages such as scipy.signal and python-control do each idea of this level in a line; you now know what that line does.

Practice

These problems are required on this path at this level; the path adds one problem from each of the level's topics, picked for you and kept.

• A Lift's State, One Step at a Time
• The Drone's Height Loop as One Matrix
• Placing the Lift's Two Poles by Hand
• A Kitchen Scale That Grows More Sure
• The Tank Gauge That Sometimes Hears the Wrong Echo
• Reading a Lift's Speed From a Rangefinder

Checklist

You can now:

• Simulate x[k+1] = A x + B u, y = C x, and convert to and from a difference equation.
• Read poles and modes from eigenvalues, and test controllability and stabilisability.
• Close a loop with u = -K x, place two poles by matching coefficients, and use Ackermann's formula.
• Add a feedforward gain, find an LQR gain by the Riccati iteration, and plan with a receding horizon.
• Run a 1D Kalman filter, find its steady-state gain, weigh q against r, and survive missing readings.
• Blend two sensors with a complementary filter, and gate outliers by the innovation.
• Track position and velocity with a 2-state filter, and compare dead reckoning with fusion.
• Identify a model by least squares, test it on fresh data, and follow a changing parameter with a moving window.