Lab 6.6 — Real-Time Kalman Filter & State Estimation

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

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

• G&A 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.
• G&A 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.
• G&A 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 Lessons 22–23 (conditional expectation and Gaussian vectors) — the Kalman update is the conditional mean of a jointly-Gaussian pair, i.e. the MMSE estimator. Course 1 Lesson 6 (least squares) — the matrix form is weighted least squares; conditioning (Lesson 51) 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.

Wiring & bench setup

The signal chain: the MCP4725 makes the clean truth signal, a deliberately bad path corrupts it on the way to the ADC, and the on-chip DAC plays the estimate back out so truth, measurement, and estimate all sit on the scope at once.

flowchart LR
  DAC["MCP4725<br/>truth: ramp / step<br/>addr 0x60/0x62"]
  NOISY["Divider + long<br/>unshielded jumper<br/>(deliberate noise pickup)"]
  MCU["NUCLEO-L476RG<br/>ADC A0 = PA0<br/>DAC A2 = PA4 estimate"]
  SCOPE["Siglent SDS1104X-E<br/>CH1 truth · CH2 noisy<br/>CH3 estimate"]
  SAL["Saleae Logic 8<br/>CH0 = timing toggle"]
  DAC --> NOISY --> MCU
  DAC -.-> SCOPE
  MCU -.-> SCOPE
  MCU -.-> SAL

flowchart LR
  DAC["MCP4725<br/>truth: ramp / step<br/>addr 0x60/0x62"]
  NOISY["Divider + long<br/>unshielded jumper<br/>(deliberate noise pickup)"]
  MCU["NUCLEO-L476RG<br/>ADC A0 = PA0<br/>DAC A2 = PA4 estimate"]
  SCOPE["Siglent SDS1104X-E<br/>CH1 truth · CH2 noisy<br/>CH3 estimate"]
  SAL["Saleae Logic 8<br/>CH0 = timing toggle"]
  DAC --> NOISY --> MCU
  DAC -.-> SCOPE
  MCU -.-> SCOPE
  MCU -.-> SAL

Pin map (every wire):

From	To	Pin/jack
MCP4725 VDD / GND	breadboard + / − rail ← Nucleo	3V3 / GND
MCP4725 SCL / SDA	I2C1, the bus from Lab 3.1	D15 (PB8) / D14 (PB9)
MCP4725 OUT	divider top resistor R1 (~10 kΩ)	breadboard
Divider midpoint (R1–R2 junction; R2 ~10 kΩ → − rail)	long unshielded jumper → ADC input	A0 (PA0)
Nucleo A2 (PA4, DAC1_OUT1)	estimate out → scope CH3	—
Scope CH1 (10×)	MCP4725 OUT (truth); ground clip → − rail (once)	CH1
Scope CH2 (10×)	the noisy ADC node at A0	CH2
Scope CH3 (10×)	PA4 (estimate)	CH3
Saleae CH0 + one GND lead	D7 (PA8) timing toggle · − rail	CH0

 MCP4725 OUT ──R1(~10k)──●──(long, unshielded jumper)──► A0/PA0   NUCLEO-L476RG
                         │                                estimate ◄─ A2/PA4 ─► scope CH3
                        R2(~10k)
                         │
                        − rail (shared GND: Nucleo, DAC, scope clips)

• The equal divider halves the truth signal (the ADC sees OUT/2 — account for it when comparing) and leaves a high-impedance node; that plus the long jumper is the point — it inverts Lab 6.4’s low-impedance rule on purpose so hum/pickup becomes your measurement noise \(v_k\). Keep the DAC ramp within 0–3.3 V so the divided node stays in 0–1.65 V.
• Most MCP4725 breakouts carry on-board I²C pull-ups (verified in Lab 3.1); the noise-variance number you plug in as \(R\) should be re-measured on this node if you changed anything since Lab 6.4.

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.

Project & environment setup

Firmware — reuse the Module 6 project (firmware/m6-dsp/, created in Lab 6.1; CMSIS-DSP already linked and the 80 MHz clock set per the setup essentials). Confirm the .ioc has:

CubeMX page	Setting
Analog → ADC1	IN5 single-ended (PA0), external trigger TIM2 TRGO, DMA circular — the Lab 5.3 front end
Timers → TIM2	TRGO = Update event; prescaler/ARR set from your chosen \(f_s\)
Analog → DAC1	OUT1 on PA4 (A2) — estimate playback for scope CH3
Connectivity → I2C1	I2C mode, 100 kHz, PB8/PB9 — drives the MCP4725 truth source
GPIO	PA8 (D7) output — toggle around kalman_step for the Saleae
Connectivity → USART2	Asynchronous, 115200 8-N-1 — stream truth/measurement/estimate to the host
Middleware → FREERTOS	RTOS variant only (see Same STM32: bare-metal vs RTOS below): CMSIS_V2, HAL timebase moved to a spare timer — per the FreeRTOS bullet in the setup essentials

Host — Part A’s prototyping/tuning runs on the Mac in the course venv (see Toolchain):

source venv/bin/activate      # numpy + matplotlib + pyserial
mkdir -p labs/lab-6-6/host labs/lab-6-6/captures

Put the Part A prototype in labs/lab-6-6/host/ (numpy for the scalar recursion and the \(Q\) sweep, matplotlib for the truth/measurement/estimate plots, pyserial for the on-target log capture — you write the scripts).

Keep this lab’s reconciliation in labs/lab-6-6/host/analysis.ipynb — the notebook convention — and export final figures next to it.

Where results go:

Artifact	Path
Bench note	labs/lab-6-6/notes.md
Host tuning-sweep plot (Part A)	labs/lab-6-6/host/tuning.png
Serial logs: truth, measurement, estimate	labs/lab-6-6/captures/scalar-run.csv, labs/lab-6-6/captures/cv-run.csv
Saleae kalman_step timing capture	labs/lab-6-6/captures/step-timing.sal
Joseph-form before/after log (Part D)	labs/lab-6-6/captures/joseph-fix.csv

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 (D7 = PA8, per Wiring & bench setup) 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 (labs/lab-6-6/notes.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.

Cross-platform ports & language variants

See the syllabus Implementation tracks for the framing; this is the Kalman-specific version. The filter is a strict sequential recursion — predict then update, each estimate depending on the last — so it lives in the latency-bound class where the STM32 is home and the GPU only earns its place at scale.

STM32 bare-metal (C, and Rust). In C the scalar predict/update runs in a few µs, deterministically — exactly what a hard-real-time control loop needs when the estimate drives an actuator on a fixed deadline (confirm the budget with the DWT cycle counter, setup essentials). This is also where the numerical-robustness content matters most: Joseph-form updates, square-root / Bierman factorization, and loss of covariance positive-definiteness are far more pressing in short-float or fixed-point than on float64. In Rust (#![no_std]) the recursion is identical; the \(2\times2\) matrix ops come from nalgebra (its no_std mode) or are hand-rolled, and checked arithmetic again makes any fixed-point overflow decision explicit.

Raspberry Pi 5 (Linux userspace, C or NumPy). A NumPy or C port is trivial and the float64 covariance rarely loses positive-definiteness — so the robustness curriculum that dominates the STM32 version quietly evaporates here.

Jetson Orin Nano. A poor fit for a single filter — there is no parallelism in one recursion. It only wins for many parallel, independent estimators: ensemble or particle filters, per-pixel or per-channel trackers, batched EKFs across thousands of targets. That is the regime where the GPU’s width finally pays.

Jetson Orin Nano — detailed procedure (embedded Linux)

Two runs: the honest single-loop port (CPU, where the GPU has no business), and the one reformulation that does fit the GPU — the same filter replicated across an ensemble. Reuse the Lab 6.1 Jetson harness conventions; board config in the Jetson setup essentials.

1. mkdir -p labs/lab-6-6/edge; copy the lab’s captured noisy measurement record (the serial log the STM32 filtered, as .npy) and your host-reference filtered output.
2. CPU single loop: compile the same portable-C predict/update kernel (shared/) in the edge/ project; replay the record; verify the state trajectory matches your host reference; time per-iteration p50/p99 under jetson_clocks + chrt and set it against the STM32’s DWT few-µs number. Note in notes.md which robustness machinery (Joseph form, symmetrization) float64 let you not need.
3. GPU at scale: vectorize the same recursion across an ensemble in CuPy — one (E × state) array stepping E independent filters per time step (elementwise ops for the scalar filter; the 2-state version is a handful of batched 2×2 products). Verify ensemble member 0 against the CPU run, then sweep E = 1, 100, 10⁴, 10⁶ and record time-per-step: nearly flat in E until memory bandwidth binds. The E where the GPU passes E× the CPU single-loop rate is the “wins at scale” claim, measured.
4. Save timing CSVs and the ensemble sweep figure to labs/lab-6-6/edge/; fill the table.

Raspberry Pi 5 differences: step 2 only (performance governor); a NumPy ensemble sweep on the Pi CPU makes a fair middle column if you want it.

Measure and compare (fill Measured on each platform):

Platform / build	Single-loop latency	Scales to many trackers?	Predicted	Measured
STM32 bare-metal, C (scalar, DWT)	few µs, deterministic	no (one loop)	home target	…
STM32 bare-metal, Rust (nalgebra no_std)	≈ C	no	≈ C	…
Pi 5, NumPy / C float64	fast, jittery tail	modest	robustness content vanishes	…
Jetson, batched EKF	launch-overhead-bound	yes — thousands in parallel	wins at scale	…

The contrast: a single low-latency loop (STM32 deterministic, drives an actuator) versus GPU-at-scale (thousands of independent trackers) — and the fixed-point robustness discipline exists only on the constrained target.

Same STM32: bare-metal vs RTOS

The runtime axis has a middle rung worth measuring on the MCU itself: run the estimator under FreeRTOS and compare against the bare-metal build. This is the deadline-sensitive case of Module 6 — the one where you weigh the scheduler most carefully — and it sets up Lab 7.2.

• Bare-metal (above): the DMA/sample callback runs kalman_step (predict then update) inline, in ISR context, and drives the estimate straight to the actuator/DAC — nothing between measurement and control output.
• FreeRTOS (C): turn the DMA callback into a fast signal and move the recursion into a prioritized estimator task. The HAL_ADC_Conv*CpltCallback only osSemaphoreRelease(sem)s (or osMessageQueuePuts the ready-buffer index) and returns; a high-priority task blocks on osSemaphoreAcquire(sem, osWaitForever), runs predict/update, and writes the control output. Setup: enable FREERTOS → CMSIS_V2 and move the HAL timebase to a spare timer (TIM17) per the setup essentials, then osSemaphoreNew / osThreadNew.
• Rust (RTIC / Embassy): in RTIC, the DMA-complete IRQ is a hardware task that spawns a lower-priority estimator software task; the software task runs predict/update. In Embassy, an async task awaits each measurement and steps the filter on wake. Same ISR→task deferral, statically scheduled.
• What you’ll see: the compute is only a few µs, but here the RTOS inserts scheduler latency directly between the measurement and the control output — the context-switch time and its jitter widen the measurement-to-actuation delay in a loop where that delay is a stability/performance parameter. So unlike the other Module 6 labs, the RTOS overhead is not free: it lands squarely on the control path. This is the case where you most carefully decide whether the task structure is worth it, or whether a bare-metal control loop (estimate computed and applied in the ISR) is the right call. Measure the added latency and its jitter with the DWT counter and judge it against the loop’s timing budget.

Build (same STM32)	Per-block latency/jitter added	Deadline margin	Structural benefit	Measured
Bare-metal, kalman_step + actuate in ISR	none (runs in ISR)	full — shortest measurement→control path	monolithic control loop	…
FreeRTOS, DMA→semaphore→estimator task	+ context switch on the control path (jitter widens loop delay)	reduced — scheduler latency between measure and actuate	estimator isolated as a task, but pays control-loop delay	…
Rust RTIC, DMA hw task→estimator sw task	≈ FreeRTOS	reduced (same control-path cost)	compile-time-checked task priorities	…

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.