File syn_kalman.h¶

FileList > dsp > syn_kalman.h

General-purpose fixed-point Kalman filter. More...

#include "../common/syn_defs.h"
#include "../util/syn_matrix.h"

Classes¶

Type	Name
struct	SYN_Kalman Kalman filter instance.
struct	SYN_Kalman_Config Kalman filter configuration — all matrices are caller-owned.

Public Functions¶

Type	Name
SYN_Status	syn_kalman_init (SYN_Kalman * kf, const SYN_Kalman_Config * cfg) Initialize the Kalman filter.
void	syn_kalman_predict (SYN_Kalman * kf) Predict step: propagate state and covariance forward.
SYN_Status	syn_kalman_update (SYN_Kalman * kf, const SYN_Matrix * z) Update step: incorporate a measurement.

Macros¶

Type	Name
define	SYN_KALMAN_MAX_MEAS `4` Maximum measurement dimension (default 4, override in syn_config.h).
define	SYN_KALMAN_MAX_STATE `6` Maximum state dimension (default 6, override in syn_config.h).
define	SYN_KALMAN_SCRATCH_ASSIGN (kf, prefix) `/* multi line expression /` Assign scratch matrices to the Kalman filter instance.*
define	SYN_KALMAN_SCRATCH_DECL (prefix, NS, NM) `/* multi line expression /` Convenience macro to declare all Kalman scratch matrices.*

Detailed Description¶

Implements a discrete-time linear Kalman filter using Q16.16 matrix operations. Supports arbitrary state and measurement dimensions (compile-time determined, no heap allocation).

** **

Predict: * x̂⁻ = F · x̂ * P⁻ = F · P · Fᵀ + Q

Update: * y = z − H · x̂⁻ (innovation) * S = H · P⁻ · Hᵀ + R (innovation covariance) * K = P⁻ · Hᵀ · S⁻¹ (Kalman gain) * x̂ = x̂⁻ + K · y (state update) * P = (I − K · H) · P⁻ (covariance update)

** **

// State: [position, velocity] → 2×1
// Measurement: [position]    → 1×1
#define N_STATE 2
#define N_MEAS  1

SYN_Kalman kf;
SYN_Kalman_Config cfg;

// Allocate all matrices on the stack
SYN_MAT_DECL(x,  N_STATE, 1);       // State vector
SYN_MAT_DECL(P,  N_STATE, N_STATE); // Error covariance
SYN_MAT_DECL(F,  N_STATE, N_STATE); // State transition
SYN_MAT_DECL(Q,  N_STATE, N_STATE); // Process noise
SYN_MAT_DECL(H,  N_MEAS,  N_STATE); // Measurement model
SYN_MAT_DECL(R,  N_MEAS,  N_MEAS);  // Measurement noise
SYN_MAT_DECL(z,  N_MEAS,  1);       // Measurement vector

cfg.x = &x;  cfg.P = &P;  cfg.F = &F;
cfg.Q = &Q;  cfg.H = &H;  cfg.R = &R;
cfg.n_state = N_STATE;
cfg.n_meas  = N_MEAS;

// Set up F = [[1, dt], [0, 1]] for constant-velocity model
syn_matrix_identity(&F);
SYN_MAT_AT(&F, 0, 1) = Q16_FROM_FRAC(1, 100); // dt = 10ms

// H = [[1, 0]] — we only measure position
syn_matrix_zero(&H);
SYN_MAT_AT(&H, 0, 0) = Q16_ONE;

syn_kalman_init(&kf, &cfg);

// In control loop:
z.data[0] = sensor_reading;
syn_kalman_predict(&kf);
syn_kalman_update(&kf, &z);
q16_t filtered_pos = kf.cfg->x->data[0];

Public Functions Documentation¶

function syn_kalman_init¶

Initialize the Kalman filter.

SYN_Status syn_kalman_init (
    SYN_Kalman * kf,
    const SYN_Kalman_Config * cfg
)

The caller must have populated cfg->F, cfg->H, cfg->Q, cfg->R, and initial cfg->x and cfg->P before calling this. Scratch matrices must be assigned via SYN_KALMAN_SCRATCH_ASSIGN.

Parameters:

kf Kalman filter instance.
cfg Configuration (caller-owned, must outlive kf).

Returns:

SYN_OK on success, SYN_INVALID_PARAM on dimension mismatch.

function syn_kalman_predict¶

Predict step: propagate state and covariance forward.

void syn_kalman_predict (
    SYN_Kalman * kf
)

After this call: * x̂⁻ = F · x̂ (state predicted forward) * P⁻ = F · P · Fᵀ + Q (covariance grows)

Parameters:

kf Kalman filter instance.

function syn_kalman_update¶

Update step: incorporate a measurement.

SYN_Status syn_kalman_update (
    SYN_Kalman * kf,
    const SYN_Matrix * z
)

After this call: * x̂ is corrected toward the measurement * P is reduced (uncertainty decreased)

Parameters:

kf Kalman filter instance.
z Measurement vector (n_meas × 1).

Returns:

SYN_OK on success, SYN_ERROR if innovation covariance is singular.

Macro Definition Documentation¶

define SYN_KALMAN_MAX_MEAS¶

Maximum measurement dimension (default 4, override in syn_config.h).

#define SYN_KALMAN_MAX_MEAS `4`

define SYN_KALMAN_MAX_STATE¶

Maximum state dimension (default 6, override in syn_config.h).

#define SYN_KALMAN_MAX_STATE `6`

Controls the stack size of internal Kalman gain buffer during update.

define SYN_KALMAN_SCRATCH_ASSIGN¶

Assign scratch matrices to the Kalman filter instance.

#define SYN_KALMAN_SCRATCH_ASSIGN (
    kf,
    prefix
) `/* multi line expression */`

Parameters:

kf Kalman filter instance.
prefix Name prefix used in SYN_KALMAN_SCRATCH_DECL.

define SYN_KALMAN_SCRATCH_DECL¶

Convenience macro to declare all Kalman scratch matrices.

#define SYN_KALMAN_SCRATCH_DECL (
    prefix,
    NS,
    NM
) `SYN_MAT_DECL (prefix##_nn1, NS, NS);         \ SYN_MAT_DECL (prefix##_nn2, NS, NS);         \ SYN_MAT_DECL (prefix##_nm, NS, NM);          \ SYN_MAT_DECL (prefix##_mn, NM, NS);          \ SYN_MAT_DECL (prefix##_mm, NM, NM);          \ SYN_MAT_DECL (prefix##_mm2, NM, NM);         \ SYN_MAT_DECL (prefix##_n1, NS, 1);           \ SYN_MAT_DECL (prefix##_m1, NM, 1)`

Declares the 8 scratch matrices needed by SYN_Kalman on the stack.

Parameters:

prefix Name prefix for the scratch variables.
NS State dimension.
NM Measurement dimension.

The documentation for this class was generated from the following file src/syntropic/dsp/syn_kalman.h