explainer
LQG control: connect a Kalman estimate to LQR feedback
Control a cart from noisy position readings. Connect Kalman filtering to LQR, compare matched disturbances, and test what changes when the model is wrong.
What you will learn
- Connect a current Kalman state estimate to an LQR control input.
- Separate quadratic cost weights from process and measurement noise covariances.
- Follow the measurement, control, and prediction order in a sampled system.
- Compare controllers under matched disturbances and interpret finite-run cost.
- Explain why nominal LQG optimality does not guarantee robustness to model error.
Before you start
LQG control feeds a Kalman state estimate into an LQR feedback law. The estimator reconstructs motion from noisy measurements. The controller uses that estimate to trade position error against control effort.
Here a cart has a position sensor but no velocity sensor. You can compare estimated-state feedback with a controller that differences raw readings, then change the cart's mass while leaving the design unchanged. The Python example reproduces the same experiment.
Control from an estimated state
An LQR controller needs the state its feedback law uses. For a moving cart, that state contains position and velocity. A noisy position reading supplies only part of that information.
Linear quadratic Gaussian, or LQG, names a problem with linear dynamics, quadratic cost, and Gaussian uncertainty. Under its assumptions, a Kalman filter supplies the current conditional state estimate, and the corresponding LQR gain converts it into an input. Stephen Boyd's finite-horizon LQG notes derive this separation of estimation and control.
This lesson uses 80 discrete control intervals. The earlier LQR lesson uses continuous time and an infinite horizon, so its gain numbers do not carry over directly.
Sample the same cart
The nominal cart has mass m = 2 kg and viscous damping b = 1 N·s/m. Its state order stays [position, velocity]. Applied force u acts in the positive position direction:
ṗ = v
m v̇ = u − b v
Normalize position by 1 m, velocity by 1 m/s, force by 1 N, and time by 1 s. Call the normalized state x = [p/(1 m), v/(1 m/s)]ᵀ and normalized force a = u/(1 N). Physical and normalized values then have the same numbers, while the cost below remains dimensionless.
Hold each input for 0.1 s. Integrating this damped cart exactly over that interval gives the nominal sampled dynamics:
xₖ₊₁ = A xₖ + B aₖ + wₖ
A ≈ [[1, 0.097541], [0, 0.951229]]
B ≈ [[0.002459], [0.048771]]
yₖ = H xₖ + νₖ, H = [1, 0]
The code retains full precision. For a general positive mass and damping, write ρ = exp(−bΔt/m): A₁₂ = (m/b)(1 − ρ), A₂₂ = ρ, B₁ = Δt/b − (m/b²)(1 − ρ), and B₂ = (1 − ρ)/b, using the stated unit scales. This is a zero-order-hold model of the deterministic motion.
The added wₖ acts at the sample boundary. Lines joining plotted state samples show the recorded sequence; they do not reconstruct the motion of each disturbance within an interval.
Declare the uncertainty
The baseline uses these normalized distributions:
- Initial state: x₀ ∼ N([1, 0]ᵀ, diag(0.04, 0.04)). Initial position and velocity standard deviations correspond to 0.2 m and 0.2 m/s.
- Process disturbance: wₖ ∼ N(0, W), with W = diag(0.0004, 0.0025). Each interval adds independent position and velocity errors, with physical standard deviations 0.02 m and 0.05 m/s.
- Measurement error: νₖ ∼ N(0, V), with V = 0.01. The position sensor's standard deviation is 0.1 m.
Initial error, process disturbances, and sensor errors are mutually independent; draws at different steps are independent too. These are synthetic teaching assumptions. W describes discrete state errors and is not a derived covariance for continuous force noise.
The filter knows the initial mean and covariance. It does not know the drawn starting state or disturbances. Review covariance if the distinction between variance and standard deviation is unfamiliar.
Define one cost for every controller
All three controllers aim at the origin and use the same eight-second objective. Let h = 0.1 denote the normalized time step and N = 80:
C = Σₖ₌₀⁷⁹ h (xₖᵀ Q xₖ + R aₖ²) + x₈₀ᵀ Qf x₈₀
J = E[C]
Q = Qf = diag(4, 1), R = 1
Q charges for sampled position and velocity. R charges for force. Qf adds a terminal state penalty at eight seconds, without the h multiplier.
C is the cost of one realized run; J is its expectation over the declared uncertainty. The lab reports C and a 64-run estimate of J. A single favorable disturbance sequence cannot establish expected optimality.
Keep the letters separate: Q, R, and Qf are design costs; W and V are noise covariances. The Kalman lesson uses Q and R for noise instead, so compare meanings before copying symbols between references.
This is a declared discrete cost. Exact sampling of the dynamics does not make this sum an exact discretization of the earlier continuous-time integral. No force clipping, actuator lag, or hard position bound enters this problem.
Calculate feedback backward from the ending time
Call the controller's value matrix Sₖ, reserving P for estimation covariance. Start with S₈₀ = Qf, then calculate the gain and preceding value matrix for k = 79 down to 0:
Dₖ = h R + Bᵀ Sₖ₊₁ B
Kₖ = Dₖ⁻¹ Bᵀ Sₖ₊₁ A
Sₖ = h Q + Aᵀ Sₖ₊₁ A − Aᵀ Sₖ₊₁ B Kₖ
aₖ = −Kₖ x̂ₖ⁺
Each Kₖ has a position and a velocity coefficient. The minus sign belongs to the feedback law above. The current estimate x̂ₖ⁺ includes the measurement at step k.
For this cart, K₀ ≈ [1.894797, 2.092006], while K₇₉ ≈ [0.096046, 0.462403]. The remaining horizon changes the gain. A constant gain copied from a continuous-time regulator solves a different design problem.
The recursion uses A, B, and the cost weights. Changing the sensor variance changes the estimate and its resulting inputs, but leaves this K sequence unchanged.
Correct, control, then predict
At step k, begin with prior mean x̂ₖ⁻ and covariance Pₖ⁻. Read position yₖ, compute the innovation, and update the estimate:
rₖ = yₖ − H x̂ₖ⁻
Lₖ = Pₖ⁻ Hᵀ / (H Pₖ⁻ Hᵀ + V)
x̂ₖ⁺ = x̂ₖ⁻ + Lₖ rₖ
Pₖ⁺ = (I − LₖH) Pₖ⁻ (I − LₖH)ᵀ + Lₖ V Lₖᵀ
The last line is the Joseph covariance update. Lₖ is the Kalman gain; Kₖ is the control gain. The Stanford Kalman notes distinguish the current estimate from a prediction using earlier readings.
Apply aₖ = −Kₖx̂ₖ⁺, then predict the next prior using that known input:
x̂ₖ₊₁⁻ = A x̂ₖ⁺ + B aₖ
Pₖ₊₁⁻ = A Pₖ⁺ Aᵀ + W
The simulator advances the true state separately and adds wₖ. The estimator never receives that hidden draw. Omitting B aₖ would make the filter treat some commanded motion as an unexplained innovation.
At step 80, the experiment records the terminal state and predicted estimate. It takes no new reading and applies no new input. The terminal state contributes to cost even though the control sequence has ended.
Work through the first measurement
Use the baseline prior x̂₀⁻ = [1, 0]ᵀ and P₀⁻ = diag(0.04, 0.04). For this hand calculation, suppose the first position reading is 1.2 m. This chosen reading is separate from the seeded lab record.
The innovation is 0.2, and the measurement variance is 0.01. Therefore:
L₀ = [0.04 / (0.04 + 0.01), 0]ᵀ = [0.8, 0]ᵀ
x̂₀⁺ = [1.16, 0]ᵀ
P₀⁺ = diag(0.008, 0.04)
u₀ ≈ −(1.894797 × 1.16 + 2.092006 × 0) N ≈ −2.198 N
This first reading changes only the position estimate because the initial cross-covariance is zero. Prediction couples uncertain velocity into position, so later position readings can also correct velocity. The controller uses the updated position immediately.
Compare feedback under matched disturbances
The three runs receive the same drawn initial state, process disturbances, and sensor-error sequence for the selected seed. Their actual measurement values differ as their motions diverge. Their shared gain sequence and cost make the comparison interpretable:
- LQG: use the Kalman posterior for position and velocity.
- Raw readings: use yₖ as position and (yₖ − yₖ₋₁)/0.1 as velocity. At the first reading, use the prior mean velocity of zero.
- True-state feedback: use the actual position and velocity. This supplies information the position-sensor controller cannot access; it is a reference comparison.
Each baseline's feedback uses its own resulting trajectory. The display's true-state values support simulation and scoring; they do not feed the LQG estimator.
Try the baseline with several seeds, then select the noisier sensor. That preset increases the physical sensor standard deviation from 0.1 m to 0.3 m and gives the filter the matching V = 0.09. Differencing sensor errors divides their difference by a short interval, which can produce large apparent velocities and force commands.
With the matched model and seed 7, the realized costs are 8.617 for LQG, 95.791 for raw feedback, and 8.195 for true-state feedback, rounded to three decimals. Across seeds 1 through 64, their means are 9.111, 80.300, and 8.328, respectively. The noisy finite-difference baseline spends much more of its cost on force in this setup.
Select seed 63 to see a different ordering: LQG costs about 9.321927, just below true-state feedback at 9.326673. That small reversal is consistent with a statement about expected cost rather than a guarantee for each disturbance sequence.
The 64-run table always uses seeds 1 through 64; changing the inspected seed changes only the single-run display. Its paired raw-minus-LQG difference subtracts costs within each seed before averaging. The displayed Monte Carlo standard error measures uncertainty in that estimated mean under independent-run sampling; it is neither run-to-run spread nor a guarantee for another system.
What the separation principle permits
For the declared linear Gaussian model and unconstrained quadratic objective, the optimal causal controller combines the current Kalman estimate with the corresponding finite-horizon LQR law. The estimator covariance can be calculated forward while the control gains are calculated backward. This is the separation principle.
The model uses positive R and V, and positive-semidefinite state costs and covariances. The noises do not depend on the chosen input. The filter must also use the known input and the correct timing.
Expected optimality compares controllers with the same available information and objective. True-state feedback has more information. Neither that reference nor LQG must have the lowest realized cost on every random run.
Change the plant without redesigning the controller
Select the wrong-mass preset. The simulated cart now weighs 4 kg, but the estimator and gain calculation still use 2 kg. Damping, noise distributions, the objective, and the selected random draws stay fixed.
The actual plant uses its own exact sampled A and B. All three controllers retain the nominal gain sequence, including true-state feedback. That reference therefore has access to truth but has not been redesigned for the heavier cart.
Compare the estimate with the actual motion and inspect costs. The nominal estimator covariance still describes its assumed model; it does not automatically account for the mass error. The nominal LQG optimality statement no longer applies to this changed plant.
John Doyle's 1978 robustness result shows that LQG regulators have no universal guaranteed gain or phase margin. It does not say every LQG controller has zero margin. This finite cart experiment illustrates model error; it neither reproduces Doyle's counterexample nor certifies stability margins.
Eight seconds of bounded motion cannot certify asymptotic stability or physical-robot performance. For a persistent external load, continue with integral action in state space; a constant disturbance violates the zero-mean model used here.
Run the same experiment in Python
Save lqg.py. The script requires Python 3.10 or later, uses the standard library, and needs no packages. Run:
python3 lqg.py --check
python3 lqg.py --preset baseline --seed 7
python3 lqg.py --preset noisy-sensor --seed 7
python3 lqg.py --preset wrong-mass --seed 7
The README records conventions and commands; expected output gives a comparison for the checked run. Keep the original conditions when reproducing those results. On Windows, use py -3 if that is your Python launcher.
The checks verify the hand measurement update, initial feedback gain, Bellman identity, and cost accounting for all three presets. The code fixes its random generator so the browser and Python use matching sequences. These synthetic runs demonstrate the stated model; they are not recordings from a robot.
Check the timing and noise yourself
Exercise 1: Repeat the first measurement calculation with V = 0.09, keeping the prior and y₀ = 1.2 unchanged. What happens to the position estimate and first force?
Check the noisier-reading update
The position Kalman gain is 0.04 / 0.13 = 4/13. The updated position is 1 + (4/13)(0.2) ≈ 1.061538 m, and estimated velocity stays zero. The control gain sequence is unchanged, so the first force is about −2.011 N. The less precise reading moves the estimate less far from its prior.
Exercise 2: Two successive sensor errors are independent, each with variance V = 0.01 m². With Δt = 0.1 s, what is the variance contributed by those errors to the raw finite-difference velocity?
Check the differenced-noise variance
Var((νₖ − νₖ₋₁)/Δt) = (V + V)/Δt² = 2 m²/s². Its standard deviation is √2 ≈ 1.414 m/s. This calculation isolates the sensor-noise contribution; the backward difference also describes average motion over the interval rather than exact instantaneous velocity.
Sources and further study
- Stephen Boyd, EE363: Linear Quadratic Stochastic Control with Partial State Observation, especially slides 10–2 through 10–8: finite-horizon assumptions, separation, backward control gains, and current-state estimation. This lesson writes the minus sign outside K.
- Stanford EE363, The Kalman Filter: Gaussian conditional estimates, covariance updates, and prediction timing.
- John C. Doyle, Guaranteed Margins for LQG Regulators, IEEE Transactions on Automatic Control, 1978: limits of general robustness guarantees.
Return to model uncertainty and choose one assumption to challenge. Keep the random draws and objective fixed, then explain which part of the estimator or controller no longer matches the plant.