A Kalman Tracker on Hardware

A two-state constant-velocity filter on STM32F4 and ESP32-S3, no matrix library

The Kalman filter has a reputation for being heavy: matrix inverses, covariance propagation, linear algebra in the inner loop. That reputation comes from the general filter. The two-state constant-velocity tracker, the truck on rails from the main page, is the opposite. Its state is two floats, its matrices are mostly ones and zeros, and the one “matrix inverse” it needs is the reciprocal of a scalar. It runs per sample on any microcontroller with a few hundred spare cycles, and once it reaches steady state it collapses to a handful of floating-point operations per measurement.

This page builds it twice: first the full recursive filter with its covariance, written out as plain scalar algebra so you can see there is no library hiding anything, then the steady-state simplification that a real product usually ships.

The two default platforms follow ADR-005: NUCLEO-F446RE (Cortex-M4F) and ESP32-S3, single-precision float. Both have an FPU, so the float code runs directly.

The model, in two floats

The state is position and velocity, $\mathbf{x} = [p,\ v]^{\mathsf T}$. With a fixed sample interval $\Delta t$, a constant-velocity prediction, position-only measurement, process-acceleration noise $\sigma_a$, and measurement noise $\sigma_z$, the matrices are the ones from the main page:

\[ \mathbf{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix},\quad \mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix},\quad \mathbf{Q} = \sigma_a^2\!\begin{bmatrix} \Delta t^4/4 & \Delta t^3/2 \\ \Delta t^3/2 & \Delta t^2 \end{bmatrix},\quad \mathbf{R} = [\sigma_z^2]. \]

Because $\mathbf{H}$ picks out only position and $\mathbf{R}$ is a single number, the innovation covariance $\mathbf{S} = \mathbf{H}\mathbf{P}\mathbf{H}^{\mathsf T} + \mathbf{R}$ is the scalar $P_{11} + \sigma_z^2$, and “inverting” it is one division. The covariance $\mathbf{P}$ is symmetric, so it has three distinct entries, $P_{11}, P_{12}, P_{22}$, not four. That is the whole reason this fits on a small core.

The full recursive filter

Carrying the covariance lets the gain adapt: during the uncertain startup the filter leans on measurements, then it tightens as it learns the velocity. Written as scalar algebra, with p11, p12, p22 the three unique covariance entries:

#include <math.h>

typedef struct {
    float p, v;             // state: position, velocity
    float p11, p12, p22;    // error covariance (symmetric, 3 unique entries)
    float dt;
    float q11, q12, q22;    // process-noise covariance Q (precomputed from sigma_a)
    float r;                // measurement-noise variance sigma_z^2
} kalman2_t;

void kalman2_init(kalman2_t *k, float dt, float sigma_a, float sigma_z,
                  float p0_pos, float v0) {
    k->p = p0_pos;
    k->v = v0;
    // Start uncertain so the filter trusts the first measurements. "Uncertain"
    // is relative to R = sigma_z^2: with a noisy sensor (sigma_z > 1) raise
    // these so P0 still dominates R, otherwise the filter starts over-confident.
    k->p11 = 1.0f; k->p12 = 0.0f; k->p22 = 1.0f;
    k->dt = dt;
    float dt2 = dt * dt, dt3 = dt2 * dt, dt4 = dt2 * dt2;
    float sa2 = sigma_a * sigma_a;
    k->q11 = 0.25f * dt4 * sa2;
    k->q12 = 0.5f  * dt3 * sa2;
    k->q22 =         dt2 * sa2;
    k->r   = sigma_z * sigma_z;
}

// One measurement z of position. Returns the updated position estimate.
float kalman2_step(kalman2_t *k, float z) {
    float dt = k->dt;

    // Predict: x = F x,  P = F P F^T + Q
    k->p += dt * k->v;                      // v unchanged under constant velocity
    float p11 = k->p11 + dt * (2.0f * k->p12 + dt * k->p22) + k->q11;
    float p12 = k->p12 + dt * k->p22        + k->q12;
    float p22 = k->p22                      + k->q22;

    // Update: scalar innovation, position-only measurement
    float s   = p11 + k->r;                 // innovation covariance (a scalar)
    float k0  = p11 / s;                    // Kalman gain, position
    float k1  = p12 / s;                    // Kalman gain, velocity
    float y   = z - k->p;                   // innovation
    k->p += k0 * y;
    k->v += k1 * y;

    // P = (I - K H) P, written out for the 2x2 symmetric case
    k->p11 = (1.0f - k0) * p11;
    k->p12 = (1.0f - k0) * p12;
    k->p22 = p22 - k1 * p12;

    return k->p;
}

The predict step’s P = F P F^T + Q is the only part worth checking by hand: with $\mathbf{F} = \big[\begin{smallmatrix}1 & \Delta t\\ 0 & 1\end{smallmatrix}\big]$ it works out to $P_{11} \leftarrow P_{11} + 2\Delta t\,P_{12} + \Delta t^2 P_{22}$, $P_{12} \leftarrow P_{12} + \Delta t\,P_{22}$, $P_{22} \leftarrow P_{22}$, each plus the corresponding $\mathbf{Q}$ entry. Everything else is scalar. There is no arm_mat_inverse, no loop, no allocation.

The steady-state filter: precompute the gain

For a fixed $\Delta t$, $\sigma_a$, $\sigma_z$, the covariance $\mathbf{P}$ and therefore the gain $(k_0, k_1)$ converge to constants within a few dozen steps (the convergence plot is on the main page). Once there, nothing in the covariance block changes, so you can compute the steady-state gain once, offline, on the host, hard-code the two numbers, and drop the entire covariance from the firmware. This is the alpha-beta filter.

Compute the gain offline with the Python from the main page:

from model_based import cv_model, steady_state_gain
F, H, Q, R = cv_model(dt=0.01, sigma_a=2.0, sigma_z=0.05)   # 100 Hz position sensor
K, P = steady_state_gain(F, H, Q, R)
print(K.ravel())          # [0.08555, 0.38251]  ->  k0, k1

then the per-sample filter is just predict and a fixed-gain correction:

typedef struct { float p, v; } ab_t;   // position, velocity only

#define AB_DT  0.01f       // 100 Hz
#define AB_K0  0.08555f    // steady-state position gain (precomputed offline)
#define AB_K1  0.38251f    // steady-state velocity gain

static inline float ab_step(ab_t *s, float z) {
    s->p += AB_DT * s->v;          // predict
    float y = z - s->p;            // innovation
    s->p += AB_K0 * y;             // update position
    s->v += AB_K1 * y;             // update velocity
    return s->p;
}

Four operations (three multiply-adds and a subtraction), two state variables, no covariance, no division. For the 100 Hz position sensor above, this estimate has a steady-state position standard deviation of about 0.0146 against a raw sensor noise of 0.05, roughly a 3.4x reduction, and it produces a velocity estimate the sensor never measured. The cost is that the gain no longer adapts: it is tuned for one operating point, and if $\sigma_z$ or the dynamics change you have recomputed for the wrong conditions. For a sensor with stable noise and a fixed sample rate, which is most embedded cases, that is exactly the trade you want.

STM32F4: filtering a sensor stream

A typical use is smoothing a position or angle from a noisy sensor (a potentiometer through the ADC, an encoder, a time-of-flight rangefinder) sampled by a hardware timer. Run the filter from the same periodic interrupt that reads the sensor, so $\Delta t$ is exactly the timer period.

#include "stm32f4xx_hal.h"

extern ADC_HandleTypeDef hadc1;   // the CubeMX-generated ADC handle (in adc.c)

static ab_t tracker;
// Written in the ISR, read by the main loop: must be volatile, or the compiler
// may cache the main-loop read in a register and never see the ISR's update.
static volatile float latest_position;

void tracker_init(void) {
    tracker.p = 0.0f;
    tracker.v = 0.0f;
}

// Timer ISR at 100 Hz: read the sensor, update the filter.
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
    // A bare HAL_ADC_GetValue only reads the data register; it does not trigger
    // a conversion. Start one and poll it to completion (the conversion takes a
    // few microseconds; the 1 is the poll timeout in milliseconds) so the
    // sample is fresh.
    HAL_ADC_Start(&hadc1);
    HAL_ADC_PollForConversion(&hadc1, 1);   // 1 ms timeout
    uint16_t raw = HAL_ADC_GetValue(&hadc1);          // 12-bit position sample
    HAL_ADC_Stop(&hadc1);

    float z = (float)raw * (1.0f / 4095.0f);          // scale to [0, 1]
    latest_position = ab_step(&tracker, z);           // hand to the control loop
}

The blocking poll keeps this snippet self-contained; the cleaner production wiring is to have the same timer trigger the ADC directly (TIMx TRGO as the ADC external trigger) with DMA, so a converted sample is always waiting and the ISR never polls. (Polling with a timeout inside an ISR only times out if SysTick is allowed to preempt this interrupt; the timer-triggered DMA path sidesteps the question entirely.) Either way the sample must be triggered, not merely read.

The four-operation ab_step is a few dozen cycles on the M4F’s FPU and disappears inside a 100 Hz budget (the NUCLEO-F446RE runs at 180 MHz, so 180e6 / 100 = 1.8 million cycles between samples). The full kalman2_step is still only a couple of dozen flops, also negligible here. Keep the ISR short: do the filter, store the result, and leave any heavier work to the main loop. The shared latest_position is volatile because it crosses the ISR-to-main boundary; on the M4F an aligned 32-bit float is read atomically, so there is no tearing, but on the smaller AVR or M0+ tier a multi-word read would need a guard.

Because the tracker holds velocity, it gives you a clean derivative for free. Differencing raw position samples amplifies the sensor noise badly; the filter’s tracker.v is a far quieter velocity estimate, which is often the real reason to reach for it.

ESP32-S3: the same filter from any sensor

Nothing in the filter is STM32-specific. On the ESP32-S3 the loop is identical; only the sensor read and the timing source change. Here a FreeRTOS task paces itself with vTaskDelayUntil for a steady sample interval.

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_timer.h"

void tracker_task(void *arg) {
    ab_t s = { .p = 0.0f, .v = 0.0f };
    const TickType_t period = pdMS_TO_TICKS(10);      // 100 Hz
    TickType_t last = xTaskGetTickCount();
    while (1) {
        float z = read_position_sensor();             // your sensor, scaled to units
        float p_est = ab_step(&s, z);
        publish_position(p_est, s.v);                 // position and a free velocity
        vTaskDelayUntil(&last, period);
    }
}

vTaskDelayUntil (not vTaskDelay) keeps the period fixed against jitter in the body, which matters because the filter’s gains were computed for one specific $\Delta t$. If the real interval drifts, the steady-state gain is slightly mistuned; for the full kalman2_step you would instead pass the measured $\Delta t$ each call and let the covariance adapt.

Fixed point: when there is no FPU

On a Cortex-M0+ or an AVR, the steady-state form is the one to use, because it has no covariance to keep conditioned and its only operations are multiply-add. The two gains are small fractions, so Q15 holds them comfortably ($k_0 = 0.0856$, $k_1 = 0.383$ both sit well inside $[-1, 1)$). The hazard is not the gain but the state: position and velocity can range far outside $[-1, 1)$, so the state cannot live in a Q15 int16_t. Keep the state in a wider fixed-point format (Q15 held in int32_t, or a plain integer-plus-fraction split) sized to the physical range of the quantity you track, and form the gain products in a 32-bit or 64-bit accumulator. This is the same wide-accumulator discipline that protects any fixed-point filter; the full covariance filter additionally needs care that $\mathbf{P}$ stays positive, which is exactly why the precomputed-gain form is preferred on small cores.

See the main page for the derivation, the truck-on-rails demo, and the covariance-consistency check that says the filter knows its own error.

--- title: "A Kalman Tracker on Hardware" subtitle: "A two-state constant-velocity filter on STM32F4 and ESP32-S3, no matrix library" --- The Kalman filter has a reputation for being heavy: matrix inverses, covariance propagation, linear algebra in the inner loop. That reputation comes from the *general* filter. The two-state constant-velocity tracker, the truck on rails from the [main page](index.qmd), is the opposite. Its state is two floats, its matrices are mostly ones and zeros, and the one "matrix inverse" it needs is the reciprocal of a scalar. It runs per sample on any microcontroller with a few hundred spare cycles, and once it reaches steady state it collapses to a handful of floating-point operations per measurement. This page builds it twice: first the full recursive filter with its covariance, written out as plain scalar algebra so you can see there is no library hiding anything, then the steady-state simplification that a real product usually ships. The two default platforms follow [ADR-005](../../docs/adr/005-embedded-platforms.md): NUCLEO-F446RE (Cortex-M4F) and ESP32-S3, single-precision `float`. Both have an FPU, so the float code runs directly. <hr> ## The model, in two floats The state is position and velocity, $\mathbf{x} = [p,\ v]^{\mathsf T}$. With a fixed sample interval $\Delta t$, a constant-velocity prediction, position-only measurement, process-acceleration noise $\sigma_a$, and measurement noise $\sigma_z$, the matrices are the ones from the main page: $$ \mathbf{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix},\quad \mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix},\quad \mathbf{Q} = \sigma_a^2\!\begin{bmatrix} \Delta t^4/4 & \Delta t^3/2 \\ \Delta t^3/2 & \Delta t^2 \end{bmatrix},\quad \mathbf{R} = [\sigma_z^2]. $$ Because $\mathbf{H}$ picks out only position and $\mathbf{R}$ is a single number, the innovation covariance $\mathbf{S} = \mathbf{H}\mathbf{P}\mathbf{H}^{\mathsf T} + \mathbf{R}$ is the scalar $P_{11} + \sigma_z^2$, and "inverting" it is one division. The covariance $\mathbf{P}$ is symmetric, so it has three distinct entries, $P_{11}, P_{12}, P_{22}$, not four. That is the whole reason this fits on a small core. <hr> ## The full recursive filter Carrying the covariance lets the gain adapt: during the uncertain startup the filter leans on measurements, then it tightens as it learns the velocity. Written as scalar algebra, with `p11, p12, p22` the three unique covariance entries: ```c #include <math.h> typedef struct { float p, v; // state: position, velocity float p11, p12, p22; // error covariance (symmetric, 3 unique entries) float dt; float q11, q12, q22; // process-noise covariance Q (precomputed from sigma_a) float r; // measurement-noise variance sigma_z^2 } kalman2_t; void kalman2_init(kalman2_t *k, float dt, float sigma_a, float sigma_z, float p0_pos, float v0) { k->p = p0_pos; k->v = v0; // Start uncertain so the filter trusts the first measurements. "Uncertain" // is relative to R = sigma_z^2: with a noisy sensor (sigma_z > 1) raise // these so P0 still dominates R, otherwise the filter starts over-confident. k->p11 = 1.0f; k->p12 = 0.0f; k->p22 = 1.0f; k->dt = dt; float dt2 = dt * dt, dt3 = dt2 * dt, dt4 = dt2 * dt2; float sa2 = sigma_a * sigma_a; k->q11 = 0.25f * dt4 * sa2; k->q12 = 0.5f * dt3 * sa2; k->q22 = dt2 * sa2; k->r = sigma_z * sigma_z; } // One measurement z of position. Returns the updated position estimate. float kalman2_step(kalman2_t *k, float z) { float dt = k->dt; // Predict: x = F x, P = F P F^T + Q k->p += dt * k->v; // v unchanged under constant velocity float p11 = k->p11 + dt * (2.0f * k->p12 + dt * k->p22) + k->q11; float p12 = k->p12 + dt * k->p22 + k->q12; float p22 = k->p22 + k->q22; // Update: scalar innovation, position-only measurement float s = p11 + k->r; // innovation covariance (a scalar) float k0 = p11 / s; // Kalman gain, position float k1 = p12 / s; // Kalman gain, velocity float y = z - k->p; // innovation k->p += k0 * y; k->v += k1 * y; // P = (I - K H) P, written out for the 2x2 symmetric case k->p11 = (1.0f - k0) * p11; k->p12 = (1.0f - k0) * p12; k->p22 = p22 - k1 * p12; return k->p; } ``` The predict step's `P = F P F^T + Q` is the only part worth checking by hand: with $\mathbf{F} = \big[\begin{smallmatrix}1 & \Delta t\\ 0 & 1\end{smallmatrix}\big]$ it works out to $P_{11} \leftarrow P_{11} + 2\Delta t\,P_{12} + \Delta t^2 P_{22}$, $P_{12} \leftarrow P_{12} + \Delta t\,P_{22}$, $P_{22} \leftarrow P_{22}$, each plus the corresponding $\mathbf{Q}$ entry. Everything else is scalar. There is no `arm_mat_inverse`, no loop, no allocation. <hr> ## The steady-state filter: precompute the gain For a fixed $\Delta t$, $\sigma_a$, $\sigma_z$, the covariance $\mathbf{P}$ and therefore the gain $(k_0, k_1)$ converge to constants within a few dozen steps (the convergence plot is on the [main page](index.qmd)). Once there, nothing in the covariance block changes, so you can compute the steady-state gain **once, offline, on the host**, hard-code the two numbers, and drop the entire covariance from the firmware. This is the **alpha-beta filter**. Compute the gain offline with the Python from the main page: ```python from model_based import cv_model, steady_state_gain F, H, Q, R = cv_model(dt=0.01, sigma_a=2.0, sigma_z=0.05) # 100 Hz position sensor K, P = steady_state_gain(F, H, Q, R) print(K.ravel()) # [0.08555, 0.38251] -> k0, k1 ``` then the per-sample filter is just predict and a fixed-gain correction: ```c typedef struct { float p, v; } ab_t; // position, velocity only #define AB_DT 0.01f // 100 Hz #define AB_K0 0.08555f // steady-state position gain (precomputed offline) #define AB_K1 0.38251f // steady-state velocity gain static inline float ab_step(ab_t *s, float z) { s->p += AB_DT * s->v; // predict float y = z - s->p; // innovation s->p += AB_K0 * y; // update position s->v += AB_K1 * y; // update velocity return s->p; } ``` Four operations (three multiply-adds and a subtraction), two state variables, no covariance, no division. For the 100 Hz position sensor above, this estimate has a steady-state position standard deviation of about 0.0146 against a raw sensor noise of 0.05, roughly a 3.4x reduction, and it produces a velocity estimate the sensor never measured. The cost is that the gain no longer adapts: it is tuned for one operating point, and if $\sigma_z$ or the dynamics change you have recomputed for the wrong conditions. For a sensor with stable noise and a fixed sample rate, which is most embedded cases, that is exactly the trade you want. <hr> ## STM32F4: filtering a sensor stream A typical use is smoothing a position or angle from a noisy sensor (a potentiometer through the ADC, an encoder, a time-of-flight rangefinder) sampled by a hardware timer. Run the filter from the same periodic interrupt that reads the sensor, so $\Delta t$ is exactly the timer period. ```c #include "stm32f4xx_hal.h" extern ADC_HandleTypeDef hadc1; // the CubeMX-generated ADC handle (in adc.c) static ab_t tracker; // Written in the ISR, read by the main loop: must be volatile, or the compiler // may cache the main-loop read in a register and never see the ISR's update. static volatile float latest_position; void tracker_init(void) { tracker.p = 0.0f; tracker.v = 0.0f; } // Timer ISR at 100 Hz: read the sensor, update the filter. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { // A bare HAL_ADC_GetValue only reads the data register; it does not trigger // a conversion. Start one and poll it to completion (the conversion takes a // few microseconds; the 1 is the poll timeout in milliseconds) so the // sample is fresh. HAL_ADC_Start(&hadc1); HAL_ADC_PollForConversion(&hadc1, 1); // 1 ms timeout uint16_t raw = HAL_ADC_GetValue(&hadc1); // 12-bit position sample HAL_ADC_Stop(&hadc1); float z = (float)raw * (1.0f / 4095.0f); // scale to [0, 1] latest_position = ab_step(&tracker, z); // hand to the control loop } ``` The blocking poll keeps this snippet self-contained; the cleaner production wiring is to have the same timer trigger the ADC directly (TIMx TRGO as the ADC external trigger) with DMA, so a converted sample is always waiting and the ISR never polls. (Polling with a timeout inside an ISR only times out if SysTick is allowed to preempt this interrupt; the timer-triggered DMA path sidesteps the question entirely.) Either way the sample must be triggered, not merely read. The four-operation `ab_step` is a few dozen cycles on the M4F's FPU and disappears inside a 100 Hz budget (the NUCLEO-F446RE runs at 180 MHz, so 180e6 / 100 = 1.8 million cycles between samples). The full `kalman2_step` is still only a couple of dozen flops, also negligible here. Keep the ISR short: do the filter, store the result, and leave any heavier work to the main loop. The shared `latest_position` is `volatile` because it crosses the ISR-to-main boundary; on the M4F an aligned 32-bit `float` is read atomically, so there is no tearing, but on the smaller AVR or M0+ tier a multi-word read would need a guard. Because the tracker holds velocity, it gives you a clean derivative for free. Differencing raw position samples amplifies the sensor noise badly; the filter's `tracker.v` is a far quieter velocity estimate, which is often the real reason to reach for it. <hr> ## ESP32-S3: the same filter from any sensor Nothing in the filter is STM32-specific. On the ESP32-S3 the loop is identical; only the sensor read and the timing source change. Here a FreeRTOS task paces itself with `vTaskDelayUntil` for a steady sample interval. ```c #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "esp_timer.h" void tracker_task(void *arg) { ab_t s = { .p = 0.0f, .v = 0.0f }; const TickType_t period = pdMS_TO_TICKS(10); // 100 Hz TickType_t last = xTaskGetTickCount(); while (1) { float z = read_position_sensor(); // your sensor, scaled to units float p_est = ab_step(&s, z); publish_position(p_est, s.v); // position and a free velocity vTaskDelayUntil(&last, period); } } ``` `vTaskDelayUntil` (not `vTaskDelay`) keeps the period fixed against jitter in the body, which matters because the filter's gains were computed for one specific $\Delta t$. If the real interval drifts, the steady-state gain is slightly mistuned; for the full `kalman2_step` you would instead pass the measured $\Delta t$ each call and let the covariance adapt. <hr> ## Fixed point: when there is no FPU On a Cortex-M0+ or an AVR, the steady-state form is the one to use, because it has no covariance to keep conditioned and its only operations are multiply-add. The two gains are small fractions, so Q15 holds them comfortably ($k_0 = 0.0856$, $k_1 = 0.383$ both sit well inside $[-1, 1)$). The hazard is not the gain but the **state**: position and velocity can range far outside $[-1, 1)$, so the state cannot live in a Q15 `int16_t`. Keep the state in a wider fixed-point format (Q15 held in `int32_t`, or a plain integer-plus-fraction split) sized to the physical range of the quantity you track, and form the gain products in a 32-bit or 64-bit accumulator. This is the same wide-accumulator discipline that protects [any fixed-point filter](../finite-wordlength/index.qmd); the full covariance filter additionally needs care that $\mathbf{P}$ stays positive, which is exactly why the precomputed-gain form is preferred on small cores. <hr> See the [main page](index.qmd) for the derivation, the truck-on-rails demo, and the covariance-consistency check that says the filter knows its own error.