Lab 6.6 — Real-Time Kalman Filter & State Estimation

← Course 2 syllabus · Module 6 · Prev: « Lab 6.5 · Next: Lab 7.1 »

Goal

Implement the Kalman filter — the recursive, minimum-mean-square-error state estimator — in real time on the STM32, first as a scalar estimator that pulls a clean signal out of a noisy ADC stream, then as a 2-state constant-velocity tracker. State estimation and sensor fusion are everywhere in embedded/DSP firmware (IMUs, motor control, navigation, battery SoC, any noisy sensor that feeds a control loop), and the Kalman filter is the optimal linear tool for it. This lab also closes a loop with the rest of Module 6: you’ll reuse the measured noise variance from Lab 6.4 as the filter’s measurement-noise parameter, and you’ll see that a steady-state Kalman filter is a recursively-implemented Wiener filter. Getting the tuning (\(Q\) vs. \(R\)) and the numerical implementation right is the difference between a filter that tracks and one that diverges.

Recommended reading

• Grewal Ch. 1–4 — linear dynamic systems, random processes, and the derivation of the linear optimal (Kalman) filter as the recursive MMSE estimator. Ch. 4 is the core.
• Grewal Ch. 5 (skim) — the extended/unscented filters for nonlinear problems (e.g. IMU orientation) — the natural next step, revisited in the sensor-fusion extension below.
• Grewal Ch. 7 (skim) — numerically stable (square-root / factorized) implementations and why the naive covariance update can lose positive-definiteness in fixed/short float.
• Hayes Ch. 7 (Statistical DSP) — the Wiener filter; the steady-state Kalman gain is the recursive form of the same optimal estimator.
• Kuo (Real-Time DSP) — real-time implementation and the CMSIS-DSP matrix routines.
• Course 1 Weeks 5–6 (probability) — Gaussian random variables and conditional expectation: the Kalman update is the conditional mean of a jointly-Gaussian pair, i.e. the MMSE estimator. Course 1 Weeks 1–3 (linear algebra) — the matrix form is weighted least squares; conditioning (Week 3) is exactly why the square-root form exists.

Equipment & parts

• STM32 NUCLEO-L476RG (Cortex-M4F with FPU — floating-point Kalman runs comfortably).
• MCP4725 DAC (Lab 3.3) as the truth signal source (a slow ramp or a step), plus a deliberately noisy path into the ADC — e.g. a resistive divider with a long unshielded jumper, or add the DAC signal to a small noise source. The ADS1115 (Lab 3.4) or the STM32 ADC reads it.
• Siglent scope (to see truth vs. noisy vs. estimate simultaneously if you output the estimate on the second DAC/PWM channel) and the Saleae for timing.
• Host (Python) for prototyping the filter and tuning \(Q\)/\(R\) before it goes on-target.

Safety & don’t-break-it

• Keep every analog voltage into the STM32 ADC within 0–3.3 V (the ADC pins are not 5 V tolerant); if you inject noise, make sure the peaks still stay in range or you’ll clip and bias the estimate.
• This is mostly a firmware lab — the physical risk is low, but a diverging filter is the real hazard: if the estimated covariance \(P\) goes negative (from bad tuning or numerical loss of symmetry) the estimate can blow up. Guard against it in code (below), don’t let a runaway estimate drive an actuator.
• Discharge/observe the usual bench rules from the global primer when wiring the analog front end.

Background

The model. The Kalman filter assumes a linear state-space model with Gaussian noise:

\[ \mathbf{x}_k = F\,\mathbf{x}_{k-1} + \mathbf{w}_k,\qquad \mathbf{z}_k = H\,\mathbf{x}_k + \mathbf{v}_k, \]

with process noise \(\mathbf{w}_k \sim \mathcal{N}(0, Q)\) and measurement noise \(\mathbf{v}_k \sim \mathcal{N}(0, R)\), independent. \(\mathbf{x}\) is the hidden state you want; \(\mathbf{z}\) is what the ADC actually measures.

The recursion. Each sample runs a predict then an update:

\[ \begin{aligned} &\textbf{Predict:} && \hat{\mathbf{x}}_k^- = F\,\hat{\mathbf{x}}_{k-1}, && P_k^- = F P_{k-1} F^\top + Q,\\ &\textbf{Update:} && K_k = P_k^- H^\top\!\left(H P_k^- H^\top + R\right)^{-1}, &&\\ & && \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + K_k\left(\mathbf{z}_k - H\hat{\mathbf{x}}_k^-\right), && P_k = (I - K_k H)\,P_k^-. \end{aligned} \]

The Kalman gain \(K_k\) is the whole story: it interpolates between trusting the model (\(K\to 0\) when measurements are noisy, \(R\) large) and trusting the measurement (\(K\to 1\) when the model is uncertain, \(P^-\) large). The innovation \(\mathbf{z}_k - H\hat{\mathbf{x}}_k^-\) is the new information; \(P\) is the estimate’s error covariance.

Scalar case (constant / random-walk model). Take \(F=H=1\): the state is a slowly-drifting scalar signal, \(x_k = x_{k-1} + w_k\), measured as \(z_k = x_k + v_k\). Then everything is scalar:

\[ \hat{x}_k^- = \hat{x}_{k-1},\quad P^- = P + Q,\quad K = \frac{P^-}{P^- + R},\quad \hat{x}_k = \hat{x}_k^- + K(z_k - \hat{x}_k^-),\quad P = (1-K)P^-. \]

This is a first-order recursive low-pass whose “cutoff” adapts to the confidence in the estimate. As \(k\to\infty\) the gain settles to a constant steady-state \(K_\infty\) set by the ratio \(Q/R\) — and at that point it is exactly a fixed IIR smoother, the recursive Wiener filter. Larger \(Q/R\) → higher \(K_\infty\) → faster tracking but noisier; smaller \(Q/R\) → smoother but laggier. You measured \(R\) already: it is the ADC noise variance \(\sigma_v^2\) from Lab 6.4. \(Q\) is the tuning knob.

2-state constant-velocity model. To track a ramp (the DAC sweeping), let the state be position and velocity \(\mathbf{x}=[p,\ \dot p]^\top\) with sample period \(T_s\):

\[ F = \begin{bmatrix} 1 & T_s\\ 0 & 1\end{bmatrix},\qquad H = \begin{bmatrix} 1 & 0\end{bmatrix}, \]

and \(Q\) a \(2\times2\) process-noise matrix. Now the filter estimates a velocity it never directly measures — the payoff of state estimation over a plain low-pass.

Procedure

Part A — Prototype and tune on the host (do this first).

1. In Python, generate a truth signal (step, then ramp), add Gaussian noise with the variance you measured in Lab 6.4, and implement the scalar recursion above.
2. Sweep \(Q\) over a few decades with \(R\) fixed at the measured value. Plot truth, measurement, and estimate. Watch the lag-vs-noise tradeoff. Note the \(Q\) that gives the response you want; record the steady-state gain \(K_\infty\).

Part B — Scalar filter on the STM32.

3. Reuse the timer-triggered ADC + DMA front end from Lab 5.3 at a fixed \(f_s\). Per sample (or per block), run the scalar predict/update in float.

// Scalar Kalman step (illustrative — you write the real module).
// R = measured ADC noise variance (Lab 6.4); Q = tuned process noise.
static float xhat = 0.0f, P = 1.0f;   // state estimate + error covariance
float kalman_step(float z, float Q, float R) {
    float P_pred = P + Q;                 // predict (F=H=1)
    float K = P_pred / (P_pred + R);      // gain
    xhat = xhat + K * (z - xhat);         // update estimate with innovation
    P = (1.0f - K) * P_pred;              // update covariance
    return xhat;
}

4. Stream truth, raw measurement, and estimate to the host (or drive the estimate out of the DAC / a PWM channel) and compare on the scope. Toggle a GPIO around kalman_step and measure its execution time on the Saleae — confirm it fits inside one sample period.

Part C — 2-state tracker.

5. Switch to the constant-velocity model. Use CMSIS-DSP matrix ops (arm_mat_mult_f32, arm_mat_add_f32, arm_mat_inverse_f32) for the \(F\), \(P\), \(K\) arithmetic. Feed a DAC ramp; verify the filter recovers both position and a sensible velocity estimate.

Part D — Break it, then make it robust.

6. Mis-tune deliberately: set \(R\) far too small (filter over-trusts a noisy measurement) and far too large (filter ignores measurements and lags badly). Observe both failure modes.
7. Force numerical trouble (very small \(Q\), short float, many iterations) until \(P\) loses symmetry/positivity. Then apply a robustness fix — the Joseph-form covariance update \(P = (I-KH)P^-(I-KH)^\top + KRK^\top\) (always symmetric PSD), or symmetrize \(P\leftarrow\tfrac12(P+P^\top)\) each step — and show it stops diverging (Grewal Ch. 7).

Deliverable & expected results

A bench note (docs/lab-6-6.md) with: the host tuning plot; on-target scope/serial capture of truth vs. measurement vs. estimate for the scalar and 2-state filters; the measured kalman_step execution time and % of the sample period; and a short before/after on the Joseph-form fix.

Quantity	Predicted	Measured
Measurement noise \(R=\sigma_v^2\) (from Lab 6.4)	your measured value	…
Steady-state gain \(K_\infty\) (from \(Q\), \(R\))	compute from the scalar recursion fixed point	…
Estimate noise reduction vs. raw (dB)	≈ \(10\log_{10}(1/K_\infty)\) ballpark	…
kalman_step time (scalar, float)	a few µs on M4F @ 80 MHz	…

The steady-state covariance \(P_\infty\) solves the scalar Riccati fixed point \(P_\infty = (1-K_\infty)(P_\infty+Q)\) with \(K_\infty = (P_\infty+Q)/(P_\infty+Q+R)\); solve it by hand and compare to the value \(P\) converges to on-target.

Analysis & reconciliation

Confirm the on-target steady-state gain matches the \(K_\infty\) you predicted from \(Q\) and the measured \(R\). Compare the Kalman estimate’s residual noise against a plain moving-average or the FIR low-pass from Lab 6.1 tuned to the same bandwidth — the Kalman filter should match or beat it and give you the velocity state for free. Explain, in one paragraph, why the steady-state scalar Kalman filter is a Wiener filter (Hayes Ch. 7): both minimize mean-square error; the Kalman form just computes the optimal gain recursively instead of in the frequency domain.

Going further

• Sensor fusion: fuse two noisy measurements of the same quantity (two ADS1115 channels, or two DAC-derived paths with different noise) by stacking them in \(H\) — the filter weights each by its inverse variance automatically. This is the toy version of IMU accel/gyro fusion.
• Nonlinear (EKF/UKF): add the optional BNO055/ICM-20948 IMU noted in Lab 8.4 and estimate tilt from accelerometer + gyro with an extended Kalman filter (Grewal Ch. 5) — the canonical embedded state-estimation project.
• Fixed-point / square-root: re-implement the scalar filter in Q15 and observe the numerical fragility, then a square-root (Potter/Bierman) update (Grewal Ch. 7) — the version that ships on memory-constrained parts.
• Classical vs. learned: contrast this optimal model-based estimator with the learned denoiser of Lab 8.3 — same goal (clean signal from noise), opposite philosophy.