Bonus: Balancing with noisy sensors

Noisy readings

So far cartpole.state() has told the truth. Now every reading has noise on it, with standard deviations $\sigma$ of 2 mm on $x$, 5 cm/s on $\dot x$, 0.01 rad (0.6°) on $\theta$ and 0.5 rad/s on $\dot\theta$. The rate readings are the worst. Working the rate out from two noisy angles instead doesn't help: $(\theta_k - \theta_{k-1})/\Delta t$ has noise $\sqrt2\,\sigma_\theta/\Delta t \approx 0.7$ rad/s.

LQR multiplies every reading by its gain, noise and all. With Step 4's firm gains, half a radian per second of noise on $\dot\theta$ turns into about 3 N of force chattering back and forth.

Filter with the model

You know how the cart-pole moves: $A\hat{\mathbf{x}} + Bu$. A Kalman filter keeps an estimate $\hat{\mathbf{x}}$ with a covariance $P$ (how unsure it is), and every period it

1. predicts with the model and the force you applied:

2. corrects with the new reading $\mathbf{z}$, which measures every state:

• $Q_n$, the process noise: how far the model may be off in one period.
• $R_n = \mathrm{diag}(\sigma^2)$: the readings' noise.
• $K_f$, the Kalman gain (not the LQR gain $K$): roughly, for each state, the share of the surprise $\mathbf{z} - \hat{\mathbf{x}}$ to believe.

The model predicts the rates far better than the rate readings measure them, so the filter leans on the model there and on the readings for positions. Feeding $\hat{\mathbf{x}}$ to LQR is called LQG control. The update is the one from Foundations: Combining two sensors.

Your task

Implement kf_step(A, B, Qn, Rn, xhat, P, u, z) and return the new xhat, P.

The program balances for 3 s on raw readings, resets, and balances 3 s more on your estimates. The white pole is your estimate. To pass, after the first second your $\dot\theta$ estimate must be within 0.15 rad/s RMS (root mean square) of the truth, and the force must chatter less than 1 N RMS, never over the motor's 10 N, with the pole within 5° of upright.