Square-Root Unscented Kalman Filter Implementation. More...

#include <assert.h>
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "srukf.h"

Macros
#define	DEFAULT_ALPHA 1e-3
	Default sigma point spread parameter.

#define	DEFAULT_BETA 2.0
	Default prior distribution parameter (optimal for Gaussian)

#define	DEFAULT_KAPPA 1.0
	Default secondary scaling parameter.

#define	SRUKF_EPS 1e-12
	Numerical tolerance for near-zero checks.

Functions
srukf_mat *	srukf_mat_alloc (srukf_index rows, srukf_index cols, int alloc_data)
	Allocate a matrix.

void	srukf_mat_free (srukf_mat *mat)
	Free a matrix and its data.

void	srukf_set_diag_callback (srukf_diag_fn fn)
	Set the global diagnostic callback.

static void	diag_report (const char *msg)
	Report a diagnostic message.

static srukf_return	propagate_sigma_points (const srukf_mat Xsig, srukf_mat Ysig, void(func)(const srukf_mat , srukf_mat , void ), void *user)
	Propagate sigma points through a function.

static srukf_return	compute_cross_covariance (const srukf_mat Xsig, const srukf_mat Ysig, const srukf_mat x_mean, const srukf_mat y_mean, const srukf_value weights, srukf_mat Pxz)
	Compute cross-covariance between state and measurement.

static srukf_return	chol_downdate_rank1 (srukf_mat S, const srukf_value v, srukf_value *work)
	Cholesky rank-1 downdate.

static srukf_return	compute_weighted_deviations (const srukf_mat Ysig, const srukf_mat mean, const srukf_value wc, srukf_mat Dev)
	Compute weighted deviations from sigma points.

void	srukf_free_workspace (srukf *ukf)
	Free workspace to reclaim memory.

srukf_return	srukf_alloc_workspace (srukf *ukf)
	Pre-allocate workspace.

static srukf_return	ensure_workspace (srukf *ukf)
	Ensure workspace is allocated (lazy allocation)

static bool	is_numeric_valid (const srukf_mat *M)
	Check if matrix contains any NaN or Inf values.

static srukf_return	alloc_vector (srukf_value **vec, srukf_index len)
	Allocate a vector of doubles.

static srukf_return	srukf_compute_weights (srukf *ukf, const srukf_index n)
	Compute sigma point weights.

srukf_return	srukf_set_scale (srukf *ukf, srukf_value alpha, srukf_value beta, srukf_value kappa)
	Set the UKF scaling parameters.

void	srukf_free (srukf *ukf)
	Free all memory allocated for the filter.

srukf_index	srukf_state_dim (const srukf *ukf)
	Get the state dimension N.

srukf_index	srukf_meas_dim (const srukf *ukf)
	Get the measurement dimension M.

srukf_return	srukf_get_state (const srukf ukf, srukf_mat x_out)
	Get the current state estimate.

srukf_return	srukf_set_state (srukf ukf, const srukf_mat x_in)
	Set the state estimate.

srukf_return	srukf_get_sqrt_cov (const srukf ukf, srukf_mat S_out)
	Get the sqrt-covariance matrix.

srukf_return	srukf_set_sqrt_cov (srukf ukf, const srukf_mat S_in)
	Set the sqrt-covariance matrix.

srukf_return	srukf_reset (srukf *ukf, srukf_value init_std)
	Reset filter to initial conditions.

static srukf_return	srukf_init (srukf ukf, int N, int M, const srukf_mat Qsqrt_src, const srukf_mat *Rsqrt_src)

srukf *	srukf_create (int N, int M)
	Create a filter with uninitialized noise matrices.

srukf *	srukf_create_from_noise (const srukf_mat Qsqrt, const srukf_mat Rsqrt)
	Create a filter from noise covariance matrices.

srukf_return	srukf_set_noise (srukf ukf, const srukf_mat Qsqrt, const srukf_mat *Rsqrt)
	Set the noise sqrt-covariance matrices.

static srukf_return	generate_sigma_points_from (const srukf_mat x, const srukf_mat S, srukf_value lambda, srukf_mat *Xsig)
	Generate sigma points from state and sqrt-covariance.

static void	srukf_mat_column_view (srukf_mat V, const srukf_mat M, srukf_index k)
	Create a column view into a matrix (zero-copy)

static srukf_return	srukf_sqrt_from_deviations_ex (const srukf_mat Dev, const srukf_mat Noise_sqrt, srukf_mat S, srukf_mat work, srukf_value tau, srukf_value downdate_work, bool wc0_negative, const srukf_value *dev0)
	Compute sqrt-covariance from weighted deviations via QR.

static srukf_return	compute_weighted_mean (const srukf_mat Ysig, const srukf_value wm, srukf_mat *mean)
	Compute weighted mean from sigma points.

static srukf_return	srukf_predict_core (const srukf ukf, const srukf_mat x_in, const srukf_mat S_in, void(f)(const srukf_mat , srukf_mat , void ), void user, srukf_mat x_out, srukf_mat S_out)
	Core predict implementation.

srukf_return	srukf_predict_to (srukf ukf, srukf_mat x, srukf_mat S, void(f)(const srukf_mat , srukf_mat , void ), void user)
	Transactional predict: operate on external buffers.

srukf_return	srukf_predict (srukf ukf, void(f)(const srukf_mat , srukf_mat , void ), void user)
	Process model callback type.

static srukf_return	srukf_correct_core (const srukf ukf, const srukf_mat x_in, const srukf_mat S_in, srukf_mat z, void(h)(const srukf_mat , srukf_mat , void ), void user, srukf_mat x_out, srukf_mat *S_out)
	Core correct implementation.

srukf_return	srukf_correct_to (srukf ukf, srukf_mat x, srukf_mat S, srukf_mat z, void(h)(const srukf_mat , srukf_mat , void ), void *user)
	Transactional correct: operate on external buffers.

srukf_return	srukf_correct (srukf ukf, srukf_mat z, void(h)(const srukf_mat , srukf_mat , void ), void *user)
	Correct step: incorporate measurement.

Variables
static srukf_diag_fn	g_diag_callback = NULL

Detailed Description

Square-Root Unscented Kalman Filter Implementation.

This file contains the complete implementation of the SR-UKF algorithm. The code is organized into several sections:

Matrix utilities - Basic allocation and helper functions
Workspace management - Pre-allocated temporaries for efficiency
Weight computation - UKF sigma point weights
Sigma point generation - Creating the 2N+1 sample points
SR-UKF core operations - QR-based covariance updates, Cholesky downdates
Predict/Correct - The main filter operations

Numerical Considerations

The SR-UKF differs from the standard UKF in how covariance is maintained:

Standard UKF:

P = sum(wc[i] * (X[i] - x_mean) * (X[i] - x_mean)') + Q

This requires ensuring P remains positive-definite, which can fail due to numerical errors.

Square-Root UKF:

S = qr([sqrt(wc) * (X - x_mean)' ; Qsqrt'])'

We never form P explicitly. Instead, we maintain S where P = S*S'. The QR decomposition guarantees S is a valid Cholesky factor.

Handling Negative Weights

For small alpha (< 1), the zeroth covariance weight wc[0] can be negative:

wc[0] = lambda/(N+lambda) + (1 - alpha^2 + beta)

If lambda ≈ -N (which happens for small alpha), wc[0] becomes negative. We handle this by:

Excluding the zeroth deviation from the QR (which computes S² = sum of squares)
Applying a Cholesky rank-1 downdate: S² → S² - dev0 * dev0'

The downdate uses Givens rotations for numerical stability.

Definition in file srukf.c.

Macro Definition Documentation

◆ DEFAULT_ALPHA

#define DEFAULT_ALPHA 1e-3

Default sigma point spread parameter.

Definition at line 60 of file srukf.c.

◆ DEFAULT_BETA

#define DEFAULT_BETA 2.0

Default prior distribution parameter (optimal for Gaussian)

Definition at line 62 of file srukf.c.

◆ DEFAULT_KAPPA

#define DEFAULT_KAPPA 1.0

Default secondary scaling parameter.

Definition at line 64 of file srukf.c.

◆ SRUKF_EPS

#define SRUKF_EPS 1e-12

Numerical tolerance for near-zero checks.

Definition at line 66 of file srukf.c.

Function Documentation

◆ alloc_vector()

static srukf_return alloc_vector	(	srukf_value **	vec,
		srukf_index	len
	)

static

Allocate a vector of doubles.

Parameters

vec	Output pointer
len	Number of elements

Returns: SRUKF_RETURN_OK on success

Definition at line 408 of file srukf.c.

References SRUKF_RETURN_OK, and SRUKF_RETURN_PARAMETER_ERROR.

Referenced by srukf_compute_weights().

◆ chol_downdate_rank1()

static srukf_return chol_downdate_rank1	(	srukf_mat *	S,
		const srukf_value *	v,
		srukf_value *	work
	)

static

Cholesky rank-1 downdate.

Updates a Cholesky factor to subtract a rank-1 term:

S_new * S_new' = S * S' - v * v'

This is the inverse operation of a Cholesky update. While updates are always stable (adding positive-definite term), downdates can fail if the result would be non-positive-definite.

Algorithm: Uses Givens rotations applied column-by-column. For each column j:

Compute rotation to zero out work[j] against S[j,j]
Apply rotation to update S[j,j] and remaining elements
Propagate effect to work[j+1..n]

Numerical stability: Givens rotations are orthogonal transformations, which are maximally stable. We detect failure (non-SPD result) by checking if r^2 = S[j,j]^2 - work[j]^2 becomes negative.

Parameters

S	Lower triangular Cholesky factor (N x N), modified in place
v	Vector to downdate by (length N)
work	Scratch buffer (length N), contents destroyed

Returns: SRUKF_RETURN_OK on success, SRUKF_RETURN_MATH_ERROR if non-SPD

Definition at line 1014 of file srukf.c.

References srukf_mat::n_cols, srukf_mat::n_rows, SRUKF_ENTRY, SRUKF_EPS, SRUKF_RETURN_MATH_ERROR, SRUKF_RETURN_OK, and SRUKF_RETURN_PARAMETER_ERROR.

Referenced by srukf_correct_core(), and srukf_sqrt_from_deviations_ex().

◆ compute_cross_covariance()

static srukf_return compute_cross_covariance	(	const srukf_mat *	Xsig,
		const srukf_mat *	Ysig,
		const srukf_mat *	x_mean,
		const srukf_mat *	y_mean,
		const srukf_value *	weights,
		srukf_mat *	Pxz
	)

static

Compute cross-covariance between state and measurement.

Computes:

Pxz = sum_k wc[k] * (Xsig(:,k) - x_mean) * (Ysig(:,k) - y_mean)'

This cross-covariance measures how state uncertainty correlates with measurement uncertainty. It's a key ingredient in the Kalman gain:

K = Pxz * inv(Pyy)

Intuition: If a state variable and a measurement are highly correlated (large Pxz entry), then observing that measurement tells us a lot about that state variable.

Note: Unlike auto-covariance (Pxx or Pyy), cross-covariance is not symmetric and can have any shape (N x M here).

Parameters

Xsig	State sigma points (N x (2N+1))
Ysig	Measurement sigma points (M x (2N+1))
x_mean	State mean (N x 1)
y_mean	Measurement mean (M x 1)
weights	Covariance weights wc ((2N+1) elements)
Pxz	Output cross-covariance (N x M)

Returns: SRUKF_RETURN_OK on success

Definition at line 1324 of file srukf.c.

References srukf_mat::n_cols, srukf_mat::n_rows, SRUKF_ENTRY, SRUKF_RETURN_OK, and SRUKF_RETURN_PARAMETER_ERROR.

Referenced by srukf_correct_core().

◆ compute_weighted_deviations()

static srukf_return compute_weighted_deviations	(	const srukf_mat *	Ysig,
		const srukf_mat *	mean,
		const srukf_value *	wc,
		srukf_mat *	Dev
	)

static

Compute weighted deviations from sigma points.

Computes:

Dev(:,k) = sqrt(|wc[k]|) * (Ysig(:,k) - mean)

These deviations are the building blocks for the QR-based covariance computation. The covariance (if we computed it) would be:

P = sum_k wc[k] * (Ysig(:,k) - mean) * (Ysig(:,k) - mean)'

= sum_k sign(wc[k]) * Dev(:,k) * Dev(:,k)'

For positive weights, this is Dev * Dev'. For negative wc[0], we compute QR without column 0, then downdate with Dev(:,0).

Note on negative weights: wc[0] can be negative for small alpha. We use |wc| for the sqrt and track the sign separately. The caller must handle the negative case via Cholesky downdate.

Parameters

Ysig	Propagated sigma points (dim x (2N+1))
mean	Weighted mean (dim x 1)
wc	Covariance weights (2N+1 elements)
Dev	Output deviations (dim x (2N+1))

Returns: SRUKF_RETURN_OK on success

Definition at line 1097 of file srukf.c.

References srukf_mat::n_cols, srukf_mat::n_rows, SRUKF_ENTRY, SRUKF_RETURN_OK, and SRUKF_RETURN_PARAMETER_ERROR.

Referenced by srukf_correct_core(), and srukf_predict_core().

◆ compute_weighted_mean()

static srukf_return compute_weighted_mean	(	const srukf_mat *	Ysig,
		const srukf_value *	wm,
		srukf_mat *	mean
	)

static

Compute weighted mean from sigma points.

Computes:

mean = sum_k wm[k] * Ysig(:,k)

The mean weights sum to 1, so this is a proper weighted average. For the standard UKF parameters, the central sigma point (k=0) gets the most weight when alpha is small.

Parameters

Ysig	Sigma points (dim x (2N+1))
wm	Mean weights ((2N+1) elements, sum to 1)
mean	Output mean (dim x 1)

Returns: SRUKF_RETURN_OK on success

Definition at line 1270 of file srukf.c.

References srukf_mat::n_cols, srukf_mat::n_rows, SRUKF_ENTRY, SRUKF_RETURN_OK, and SRUKF_RETURN_PARAMETER_ERROR.

Referenced by srukf_correct_core(), and srukf_predict_core().

◆ diag_report()

static void diag_report ( const char * msg )

static

Report a diagnostic message.

If a diagnostic callback is registered, invokes it with the message. Used throughout the implementation to report errors and warnings.

Parameters

msg	Message to report

Definition at line 137 of file srukf.c.

References g_diag_callback.

Referenced by generate_sigma_points_from(), srukf_compute_weights(), srukf_correct_core(), srukf_predict_core(), and srukf_sqrt_from_deviations_ex().

◆ ensure_workspace()

static srukf_return ensure_workspace ( srukf * ukf )

static

Ensure workspace is allocated (lazy allocation)

Checks if workspace exists and matches current dimensions. Allocates or reallocates as needed.

Parameters

ukf	Filter instance

Returns: SRUKF_RETURN_OK if workspace is ready

Definition at line 360 of file srukf.c.

References srukf_alloc_workspace(), srukf_meas_dim(), SRUKF_RETURN_OK, srukf_state_dim(), and srukf::ws.

Referenced by srukf_correct(), srukf_correct_to(), srukf_predict(), and srukf_predict_to().

◆ generate_sigma_points_from()

static srukf_return generate_sigma_points_from	(	const srukf_mat *	x,
		const srukf_mat *	S,
		srukf_value	lambda,
		srukf_mat *	Xsig
	)

static

Generate sigma points from state and sqrt-covariance.

Creates 2N+1 sigma points arranged symmetrically around the mean:

chi[0]   = x                        (the mean)
chi[i]   = x + gamma * S(:,i)       for i = 1..N
chi[i+N] = x - gamma * S(:,i)       for i = 1..N

where gamma = sqrt(N + lambda) is the spread factor.

Geometric interpretation: If P = S*S' is the covariance, then the sigma points lie on an ellipsoid centered at x, scaled by gamma. The columns of S define the principal axes of this ellipsoid.

Why use S instead of P? We need sqrt(P) to generate sigma points. In standard UKF, we'd compute chol(P) every time. In SR-UKF, we already have S, saving a Cholesky decomposition per step.

Parameters

x	State vector (N x 1)
S	Sqrt-covariance (N x N, lower triangular)
lambda	Scaling parameter from UKF tuning
Xsig	Output: sigma points (N x (2N+1))

Returns: SRUKF_RETURN_OK on success

Definition at line 862 of file srukf.c.

References diag_report(), srukf_mat::n_cols, srukf_mat::n_rows, SRUKF_ENTRY, SRUKF_RETURN_MATH_ERROR, SRUKF_RETURN_OK, and SRUKF_RETURN_PARAMETER_ERROR.

Referenced by srukf_correct_core(), and srukf_predict_core().

◆ is_numeric_valid()

static bool is_numeric_valid ( const srukf_mat * M )

static

Check if matrix contains any NaN or Inf values.

Used to validate callback outputs. If a process or measurement model produces non-finite values, we detect it here and return an error rather than letting garbage propagate through the filter.

Parameters

M	Matrix to check

Returns: true if all values are finite, false if any NaN/Inf found

Definition at line 390 of file srukf.c.

References srukf_mat::data, srukf_mat::n_cols, srukf_mat::n_rows, and SRUKF_ENTRY.

Referenced by srukf_correct_core(), and srukf_predict_core().

◆ propagate_sigma_points()

static srukf_return propagate_sigma_points	(	const srukf_mat *	Xsig,
		srukf_mat *	Ysig,
		void()(const srukf_mat , srukf_mat , void )	func,
		void *	user
	)

static

Propagate sigma points through a function.

Applies a function (process model or measurement model) to each sigma point individually. This is where the "unscented" magic happens: we evaluate the nonlinear function exactly, rather than linearizing it.

Parameters

Xsig	Input sigma points (N x (2N+1) for state, M x (2N+1) for meas)
Ysig	Output propagated points (same size as Xsig or different for h)
func	Function to apply: func(x_in, x_out, user)
user	User data passed to func

Returns: SRUKF_RETURN_OK on success

Definition at line 937 of file srukf.c.

References srukf_mat::data, srukf_mat::n_cols, srukf_mat_column_view(), SRUKF_RETURN_OK, and SRUKF_RETURN_PARAMETER_ERROR.

Referenced by srukf_correct_core(), and srukf_predict_core().

◆ srukf_alloc_workspace()

srukf_return srukf_alloc_workspace ( srukf * ukf )

Pre-allocate workspace.

The workspace holds temporary matrices used during predict/correct. By default, it's allocated on first use. Call this to allocate it explicitly (e.g., during initialization to avoid allocation during real-time operation).

Parameters

ukf	Filter instance

Returns: SRUKF_RETURN_OK on success

Note: Workspace size depends on N and M. If these change (via srukf_set_noise with different dimensions), workspace is reallocated automatically.

Definition at line 278 of file srukf.c.

References srukf_free_workspace(), SRUKF_MAT_ALLOC, srukf_meas_dim(), SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, srukf_state_dim(), and srukf::ws.

Referenced by ensure_workspace().

◆ srukf_compute_weights()

static srukf_return srukf_compute_weights	(	srukf *	ukf,
		const srukf_index	n
	)

static

Compute sigma point weights.

Given the current scaling parameters (alpha, beta, kappa, lambda), computes the 2N+1 weights for mean and covariance calculations.

Weight formulas:

wm[0] = lambda / (N + lambda)
wm[i] = 1 / (2 * (N + lambda))  for i = 1..2N
 
wc[0] = wm[0] + (1 - alpha^2 + beta)
wc[i] = wm[i]  for i = 1..2N

Note on negative wc[0]: For small alpha, lambda approaches -N, making wm[0] large and negative. Adding (1 - alpha^2 + beta) may not compensate enough, leaving wc[0] < 0. This is handled correctly in the QR-based covariance computation.

Parameters

ukf	Filter with alpha/beta/kappa/lambda set
n	State dimension N

Returns: SRUKF_RETURN_OK on success

Definition at line 456 of file srukf.c.

References alloc_vector(), srukf::alpha, srukf::beta, diag_report(), srukf::lambda, SRUKF_EPS, SRUKF_RETURN_MATH_ERROR, SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, srukf::wc, and srukf::wm.

Referenced by srukf_set_scale().

◆ srukf_correct()

srukf_return srukf_correct	(	srukf *	ukf,
		srukf_mat *	z,
		void()(const srukf_mat , srukf_mat , void )	h,
		void *	user
	)

Correct step: incorporate measurement.

Updates the state estimate using a new measurement. This decreases uncertainty (measurement reduces our ignorance about the state).

Parameters

ukf	Filter instance
z	Measurement vector (M x 1)
h	Measurement model function \(z = h(x)\)
user	User data passed to h

Returns: SRUKF_RETURN_OK on success. On error, filter state is unchanged.

Algorithm:

Generate sigma points from predicted state
Propagate through measurement model h
Compute predicted measurement mean and sqrt-covariance Syy
Compute cross-covariance Pxz between state and measurement
Compute Kalman gain K = Pxz * inv(Syy' * Syy)
Update state: x = x + K * (z - z_predicted)
Update S via Cholesky downdates: S^2 -= (K*Syy)(K*Syy)'

Intuition: The Kalman gain K determines how much to trust the measurement vs the prediction. If Rsqrt is large (noisy sensor), K is small and we mostly keep our prediction. If Syy is large (uncertain prediction), K is large and we trust the measurement more.

See also: srukf_predict

Definition at line 1817 of file srukf.c.

References srukf_mat::data, ensure_workspace(), srukf_mat::n_rows, srukf::S, srukf_correct_core(), SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, srukf::ws, and srukf::x.

◆ srukf_correct_core()

static srukf_return srukf_correct_core	(	const srukf *	ukf,
		const srukf_mat *	x_in,
		const srukf_mat *	S_in,
		srukf_mat *	z,
		void()(const srukf_mat , srukf_mat , void )	h,
		void *	user,
		srukf_mat *	x_out,
		srukf_mat *	S_out
	)

static

Core correct implementation.

Performs the SR-UKF correction step using QR for measurement covariance and Cholesky downdates for state covariance update.

Algorithm:

Xsig = generate_sigma_points(x_in, S_in)
Zsig = h(Xsig)                              // propagate to measurement
space
z_pred = weighted_mean(Zsig, wm)            // predicted measurement
Syy = qr([Dev_z'; Rsqrt'])'                 // measurement sqrt-covariance
Pxz = cross_covariance(Xsig, Zsig)          // state-measurement
correlation
K = Pxz * inv(Syy' * Syy)                   // Kalman gain via triangular
solves
innovation = z - z_pred
x_out = x_in + K * innovation               // state update
U = K * Syy
for each column u of U:                    // M Cholesky downdates
      S_out = choldowndate(S_out, u)

Why Cholesky downdates for covariance update? The covariance update formula is:

P_new = P - K * Pyy * K'

= P - (K * Syy) * (K * Syy)'

If U = K * Syy (N x M), then P_new = P - U * U'. This is exactly M rank-1 downdates using the columns of U.

Parameters

ukf	Filter (provides parameters and workspace)
x_in	Predicted state (N x 1)
S_in	Predicted sqrt-covariance (N x N)
z	Measurement (M x 1)
h	Measurement model function
user	User data for h
x_out	Output corrected state (N x 1, may alias x_in)
S_out	Output corrected sqrt-covariance (N x N, may alias S_in)

Returns: SRUKF_RETURN_OK on success

< Triangular solve (double)

< Matrix multiply (double)

Definition at line 1618 of file srukf.c.

References chol_downdate_rank1(), compute_cross_covariance(), compute_weighted_deviations(), compute_weighted_mean(), srukf_mat::data, diag_report(), generate_sigma_points_from(), is_numeric_valid(), srukf::lambda, srukf_mat::n_cols, srukf_mat::n_rows, propagate_sigma_points(), srukf::Rsqrt, SRUKF_CBLAS_LAYOUT, SRUKF_ENTRY, SRUKF_EPS, SRUKF_GEMM, SRUKF_LEADING_DIM, SRUKF_RETURN_MATH_ERROR, SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, srukf_sqrt_from_deviations_ex(), SRUKF_TRSM, srukf::wc, srukf::wm, and srukf::ws.

Referenced by srukf_correct(), and srukf_correct_to().

◆ srukf_correct_to()

srukf_return srukf_correct_to	(	srukf *	ukf,
		srukf_mat *	x,
		srukf_mat *	S,
		srukf_mat *	z,
		void()(const srukf_mat , srukf_mat , void )	h,
		void *	user
	)

Transactional correct: operate on external buffers.

Like srukf_correct(), but reads/writes state from user-provided buffers.

Parameters

ukf	Filter instance (provides parameters and workspace)
x	State vector (N x 1), modified in-place
S	Sqrt-covariance (N x N), modified in-place
z	Measurement vector (M x 1)
h	Measurement model function
user	User data passed to h

Returns: SRUKF_RETURN_OK on success

Definition at line 1790 of file srukf.c.

References ensure_workspace(), srukf_mat::n_cols, srukf_mat::n_rows, srukf_correct_core(), srukf_meas_dim(), SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, and srukf_state_dim().

◆ srukf_create()

srukf * srukf_create	(	int	N,
		int	M
	)

Create a filter with uninitialized noise matrices.

Creates a filter with the specified dimensions but without setting noise covariances. You must call srukf_set_noise() before using predict/correct.

Parameters

N	State dimension (must be > 0)
M	Measurement dimension (must be > 0)

Returns: Allocated filter, or NULL on failure

This two-step initialization is useful when noise parameters are computed or loaded separately from filter creation.

See also: srukf_set_noise, srukf_free

Definition at line 714 of file srukf.c.

References srukf_free(), and SRUKF_RETURN_OK.

◆ srukf_create_from_noise()

srukf * srukf_create_from_noise	(	const srukf_mat *	Qsqrt,
		const srukf_mat *	Rsqrt
	)

Create a filter from noise covariance matrices.

Creates and initializes a filter with the given noise characteristics. The state dimension N is inferred from Qsqrt, measurement dimension M from Rsqrt.

Parameters

Qsqrt	Process noise sqrt-covariance (N x N, lower triangular)
Rsqrt	Measurement noise sqrt-covariance (M x M, lower triangular)

Returns: Allocated filter, or NULL on failure

Note: Both matrices must be square. They are copied, so the originals can be freed after this call.

See also: srukf_create, srukf_free

Definition at line 734 of file srukf.c.

References srukf_mat::n_cols, srukf_mat::n_rows, srukf_free(), and SRUKF_RETURN_OK.

◆ srukf_free()

void srukf_free ( srukf * ukf )

Free all memory allocated for the filter.

Releases the filter struct, all matrices, weight vectors, and workspace. Safe to call with NULL.

Parameters

ukf	Filter to free (may be NULL)

Definition at line 530 of file srukf.c.

References srukf::Qsqrt, srukf::Rsqrt, srukf::S, srukf_free_workspace(), srukf_mat_free(), srukf::wc, srukf::wm, and srukf::x.

Referenced by srukf_create(), and srukf_create_from_noise().

◆ srukf_free_workspace()

void srukf_free_workspace ( srukf * ukf )

Free workspace to reclaim memory.

Releases the workspace memory. The next predict/correct call will reallocate it. Useful for long-running applications where the filter is dormant for extended periods.

Parameters

ukf	Filter instance (may be NULL)

Note: Called automatically by srukf_free().

Definition at line 240 of file srukf.c.

References srukf_mat_free(), and srukf::ws.

Referenced by srukf_alloc_workspace(), and srukf_free().

◆ srukf_get_sqrt_cov()

srukf_return srukf_get_sqrt_cov	(	const srukf *	ukf,
		srukf_mat *	S_out
	)

Get the sqrt-covariance matrix.

Copies S where \(P = SS^T\) is the state covariance.

Parameters

ukf	Filter instance
S_out	Output buffer (N x N), must be pre-allocated

Returns: SRUKF_RETURN_OK on success

To get the full covariance P, compute \(P = S \cdot S^T\).

Definition at line 600 of file srukf.c.

References srukf_mat::data, srukf_mat::n_cols, srukf_mat::n_rows, srukf::S, SRUKF_RETURN_OK, and SRUKF_RETURN_PARAMETER_ERROR.

◆ srukf_get_state()

srukf_return srukf_get_state	(	const srukf *	ukf,
		srukf_mat *	x_out
	)

Get the current state estimate.

Copies the state vector to a user-provided buffer.

Parameters

ukf	Filter instance
x_out	Output buffer (N x 1), must be pre-allocated

Returns: SRUKF_RETURN_OK on success

Definition at line 576 of file srukf.c.

References srukf_mat::data, srukf_mat::n_cols, srukf_mat::n_rows, SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, and srukf::x.

◆ srukf_init()

static srukf_return srukf_init	(	srukf *	ukf,
		int	N,
		int	M,
		const srukf_mat *	Qsqrt_src,
		const srukf_mat *	Rsqrt_src
	)

static

Definition at line 648 of file srukf.c.

◆ srukf_mat_alloc()

srukf_mat * srukf_mat_alloc	(	srukf_index	rows,
		srukf_index	cols,
		int	alloc_data
	)

Allocate a matrix.

Creates a new matrix with the specified dimensions. Memory is zero-initialized.

Parameters

rows	Number of rows
cols	Number of columns
alloc_data	If non-zero, allocate data buffer; otherwise data is NULL

Returns: Allocated matrix, or NULL on failure

Note: Use srukf_mat_free() to release memory when done.

Definition at line 75 of file srukf.c.

References srukf_mat::data, srukf_mat::inc_col, srukf_mat::inc_row, srukf_mat::n_cols, srukf_mat::n_rows, SRUKF_SET_TYPE, SRUKF_TYPE_COL_MAJOR, SRUKF_TYPE_NO_DATA, SRUKF_TYPE_SQUARE, SRUKF_TYPE_VECTOR, and srukf_mat::type.

◆ srukf_mat_column_view()

static void srukf_mat_column_view	(	srukf_mat *	V,
		const srukf_mat *	M,
		srukf_index	k
	)

inlinestatic

Create a column view into a matrix (zero-copy)

Sets up V to reference column k of M without copying data. This is used extensively when propagating sigma points, where we need to pass individual columns to the process/measurement model.

Parameters

V	Output view (caller provides storage for the srukf_mat struct)
M	Source matrix
k	Column index

Definition at line 912 of file srukf.c.

References srukf_mat::data, srukf_mat::inc_col, srukf_mat::inc_row, srukf_mat::n_cols, srukf_mat::n_rows, SRUKF_SET_TYPE, SRUKF_TYPE_COL_MAJOR, and srukf_mat::type.

Referenced by propagate_sigma_points().

◆ srukf_mat_free()

void srukf_mat_free ( srukf_mat * mat )

Free a matrix and its data.

Releases all memory associated with the matrix. Safe to call with NULL.

Parameters

mat	Matrix to free (may be NULL)

Definition at line 105 of file srukf.c.

References srukf_mat::data, SRUKF_IS_TYPE, and SRUKF_TYPE_NO_DATA.

Referenced by srukf_free(), srukf_free_workspace(), and srukf_set_noise().

◆ srukf_meas_dim()

srukf_index srukf_meas_dim ( const srukf * ukf )

Get the measurement dimension M.

Parameters

ukf	Filter instance

Returns: Measurement dimension, or 0 if ukf is NULL/invalid

Definition at line 568 of file srukf.c.

References srukf_mat::n_rows, and srukf::Rsqrt.

Referenced by ensure_workspace(), srukf_alloc_workspace(), and srukf_correct_to().

◆ srukf_predict()

srukf_return srukf_predict	(	srukf *	ukf,
		void()(const srukf_mat , srukf_mat , void )	f,
		void *	user
	)

Process model callback type.

User-provided function implementing the state transition model: \(x_{k+1} = f(x_k)\)

Parameters

x_in	Current state (N x 1), read-only
x_out	Next state (N x 1), write output here
user	User data pointer (passed through from predict call)

Example (constant velocity model):

void process_model(const srukf_mat *x_in, srukf_mat *x_out, void *user) {
    double dt = *(double*)user;
    // State: [position, velocity]
    SRUKF_ENTRY(x_out, 0, 0) = SRUKF_ENTRY(x_in, 0, 0)
                             + dt * SRUKF_ENTRY(x_in, 1, 0);
    SRUKF_ENTRY(x_out, 1, 0) = SRUKF_ENTRY(x_in, 1, 0);  // velocity
unchanged
}

Measurement model callback type

User-provided function implementing the measurement model: \(z = h(x)\)

Parameters

x_in	Current state (N x 1), read-only
z_out	Predicted measurement (M x 1), write output here
user	User data pointer (passed through from correct call)

Example (observe position only):

void meas_model(const srukf_mat *x_in, srukf_mat *z_out, void *user) {
    (void)user;
    // We only measure position (first state variable)
    SRUKF_ENTRY(z_out, 0, 0) = SRUKF_ENTRY(x_in, 0, 0);
}

Predict step: propagate state through process model

Advances the state estimate forward in time using the process model. This increases uncertainty (covariance grows due to process noise).

Parameters

ukf	Filter instance
f	Process model function \(x_{k+1} = f(x_k)\)
user	User data passed to f

Returns: SRUKF_RETURN_OK on success. On error, filter state is unchanged.

Algorithm:

Generate 2N+1 sigma points from current (x, S)
Propagate each sigma point through f
Compute new mean as weighted average of propagated points
Compute new S via QR decomposition of weighted deviations + Qsqrt

See also: srukf_correct

Definition at line 1515 of file srukf.c.

References srukf_mat::data, ensure_workspace(), srukf_mat::n_rows, srukf::S, srukf_predict_core(), SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, srukf::ws, and srukf::x.

◆ srukf_predict_core()

static srukf_return srukf_predict_core	(	const srukf *	ukf,
		const srukf_mat *	x_in,
		const srukf_mat *	S_in,
		void()(const srukf_mat , srukf_mat , void )	f,
		void *	user,
		srukf_mat *	x_out,
		srukf_mat *	S_out
	)

static

Core predict implementation.

Performs the SR-UKF predict step using QR-based covariance update. Can operate in-place (x_out == x_in, S_out == S_in) or to separate buffers.

Algorithm:

Xsig = generate_sigma_points(x_in, S_in)      // 2N+1 points
Ysig = f(Xsig)                                // propagate each
x_out = weighted_mean(Ysig, wm)               // predicted mean
Dev = sqrt(|wc|) * (Ysig - x_out)             // weighted deviations
S_out = qr([Dev'; Qsqrt'])'                   // sqrt-covariance via QR
if wc[0] < 0: choldowndate(S_out, Dev(:,0))  // handle negative weight

Parameters

ukf	Filter (provides parameters and workspace)
x_in	Current state (N x 1)
S_in	Current sqrt-covariance (N x N)
f	Process model function
user	User data for f
x_out	Output predicted state (N x 1, may alias x_in)
S_out	Output predicted sqrt-covariance (N x N, may alias S_in)

Returns: SRUKF_RETURN_OK on success

Definition at line 1405 of file srukf.c.

References compute_weighted_deviations(), compute_weighted_mean(), srukf_mat::data, diag_report(), generate_sigma_points_from(), is_numeric_valid(), srukf::lambda, srukf_mat::n_cols, srukf_mat::n_rows, propagate_sigma_points(), srukf::Qsqrt, SRUKF_ENTRY, SRUKF_RETURN_MATH_ERROR, SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, srukf_sqrt_from_deviations_ex(), srukf::wc, srukf::wm, and srukf::ws.

Referenced by srukf_predict(), and srukf_predict_to().

◆ srukf_predict_to()

srukf_return srukf_predict_to	(	srukf *	ukf,
		srukf_mat *	x,
		srukf_mat *	S,
		void()(const srukf_mat , srukf_mat , void )	f,
		void *	user
	)

Transactional predict: operate on external buffers.

Like srukf_predict(), but reads/writes state from user-provided buffers instead of the filter's internal state. Useful for:

Implementing particle filters (multiple hypotheses)
What-if analysis without modifying the filter
Custom rollback/checkpoint schemes

Parameters

ukf	Filter instance (provides parameters and workspace)
x	State vector (N x 1), modified in-place
S	Sqrt-covariance (N x N), modified in-place
f	Process model function
user	User data passed to f

Returns: SRUKF_RETURN_OK on success

Definition at line 1492 of file srukf.c.

References ensure_workspace(), srukf_mat::n_cols, srukf_mat::n_rows, srukf_predict_core(), SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, and srukf_state_dim().

◆ srukf_reset()

srukf_return srukf_reset	(	srukf *	ukf,
		srukf_value	init_std
	)

Reset filter to initial conditions.

Sets state to zero and sqrt-covariance to a scaled identity matrix. Useful for reinitializing a filter without reallocating.

Parameters

ukf	Filter instance
init_std	Initial standard deviation (> 0). The sqrt-covariance is set to `init_std * I`.

Returns: SRUKF_RETURN_OK on success

Interpretation: After reset, each state variable has zero mean and variance init_std^2, with no correlation between variables.

Definition at line 624 of file srukf.c.

References srukf_mat::data, srukf_mat::n_rows, srukf::S, SRUKF_ENTRY, SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, and srukf::x.

◆ srukf_set_diag_callback()

void srukf_set_diag_callback ( srukf_diag_fn fn )

Set the global diagnostic callback.

When set, the library will call this function with diagnostic messages about errors and warnings (e.g., "QR factorization failed").

Parameters

fn	Callback function, or NULL to disable diagnostics

Note: This is a global setting affecting all filter instances.

Example:

void my_diag(const char *msg) {
    fprintf(stderr, "SR-UKF: %s\n", msg);
}
srukf_set_diag_callback(my_diag);

Definition at line 125 of file srukf.c.

References g_diag_callback.

◆ srukf_set_noise()

srukf_return srukf_set_noise	(	srukf *	ukf,
		const srukf_mat *	Qsqrt,
		const srukf_mat *	Rsqrt
	)

Set the noise sqrt-covariance matrices.

Updates the process and measurement noise covariances. This affects how the filter trades off between trusting the model vs measurements.

Parameters

ukf	Filter instance
Qsqrt	Process noise sqrt-covariance (N x N)
Rsqrt	Measurement noise sqrt-covariance (M x M)

Returns: SRUKF_RETURN_OK on success

Intuition:

Larger Qsqrt = "my model is unreliable" = trust measurements more
Larger Rsqrt = "my sensors are noisy" = trust model more

Note: The matrices are copied. Originals can be freed after this call.; On failure, the existing noise matrices are preserved.

Definition at line 756 of file srukf.c.

References srukf_mat::n_cols, srukf_mat::n_rows, srukf::Qsqrt, srukf::Rsqrt, SRUKF_ENTRY, SRUKF_MAT_ALLOC, srukf_mat_free(), SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, SRUKF_SET_TYPE, SRUKF_TYPE_COL_MAJOR, SRUKF_TYPE_SQUARE, and srukf::x.

◆ srukf_set_scale()

srukf_return srukf_set_scale	(	srukf *	ukf,
		srukf_value	alpha,
		srukf_value	beta,
		srukf_value	kappa
	)

Set the UKF scaling parameters.

Configures the sigma point distribution parameters. These affect how the filter captures nonlinear effects.

Parameters

ukf	Filter instance
alpha	Spread of sigma points around mean (must be > 0, typically 1e-3 to 1)
beta	Prior knowledge of distribution (2.0 is optimal for Gaussian)
kappa	Secondary scaling parameter (typically 0 or 3-N)

Returns: SRUKF_RETURN_OK on success, SRUKF_RETURN_PARAMETER_ERROR if alpha <= 0

Derived parameters:

\(\lambda = \alpha^2 (N + \kappa) - N\)
\(\gamma = \sqrt{N + \lambda}\) (sigma point spread factor)

Weight formulas:

\(w^m_0 = \lambda / (N + \lambda)\)
\(w^m_i = 1 / (2(N + \lambda))\) for i > 0
\(w^c_0 = w^m_0 + (1 - \alpha^2 + \beta)\)
\(w^c_i = w^m_i\) for i > 0

Note: For very small alpha, \(w^c_0\) can become negative. This library handles this correctly via Cholesky downdates.

Definition at line 494 of file srukf.c.

References srukf::alpha, srukf::beta, srukf::kappa, srukf::lambda, srukf_mat::n_rows, srukf_compute_weights(), SRUKF_EPS, SRUKF_RETURN_PARAMETER_ERROR, srukf_set_scale(), and srukf::x.

Referenced by srukf_set_scale().

◆ srukf_set_sqrt_cov()

srukf_return srukf_set_sqrt_cov	(	srukf *	ukf,
		const srukf_mat *	S_in
	)

Set the sqrt-covariance matrix.

Parameters

ukf	Filter instance
S_in	New sqrt-covariance (N x N, should be lower triangular)

Returns: SRUKF_RETURN_OK on success

Warning: S_in should be a valid Cholesky factor (lower triangular, positive diagonal). Invalid values may cause filter divergence.

Definition at line 612 of file srukf.c.

References srukf_mat::data, srukf_mat::n_cols, srukf_mat::n_rows, srukf::S, SRUKF_RETURN_OK, and SRUKF_RETURN_PARAMETER_ERROR.

◆ srukf_set_state()

srukf_return srukf_set_state	(	srukf *	ukf,
		const srukf_mat *	x_in
	)

Set the state estimate.

Overwrites the current state with user-provided values.

Parameters

ukf	Filter instance
x_in	New state vector (N x 1)

Returns: SRUKF_RETURN_OK on success

Note: This does not update the covariance. Use srukf_set_sqrt_cov() or srukf_reset() to also set uncertainty.

Definition at line 588 of file srukf.c.

References srukf_mat::data, srukf_mat::n_cols, srukf_mat::n_rows, SRUKF_RETURN_OK, SRUKF_RETURN_PARAMETER_ERROR, and srukf::x.

◆ srukf_sqrt_from_deviations_ex()

static srukf_return srukf_sqrt_from_deviations_ex	(	const srukf_mat *	Dev,
		const srukf_mat *	Noise_sqrt,
		srukf_mat *	S,
		srukf_mat *	work,
		srukf_value *	tau,
		srukf_value *	downdate_work,
		bool	wc0_negative,
		const srukf_value *	dev0
	)

static

Compute sqrt-covariance from weighted deviations via QR.

This is the heart of the SR-UKF algorithm. Instead of computing:

P = sum_k wc[k] * dev_k * dev_k' + Q

S = chol(P)

We directly compute S via QR decomposition of a compound matrix:

A = [ Dev(:,1:2N)'  ]     <-- (2N) rows if wc[0] >= 0, else (2N) rows
    [ Noise_sqrt'   ]     <-- (dim) rows
 
[Q, R] = qr(A)
S = R'                    <-- lower triangular

Why this works: If A = [a1; a2; ...] (rows), then A'*A = sum(a_i' * a_i). The R from QR satisfies R'*R = A'*A (by orthogonality of Q). So R'*R = sum(dev_k * dev_k') + Noise_sqrt * Noise_sqrt' = P. Thus R' is the Cholesky factor S.

Handling negative wc[0]: If wc[0] < 0, we exclude Dev(:,0) from the QR (which computes the sum of positive terms), then apply a Cholesky downdate to subtract Dev(:,0) * Dev(:,0)'.

Ensuring positive diagonal: QR can produce negative diagonal in R. We flip signs of entire columns to ensure S has positive diagonal, which is the standard Cholesky convention.

Parameters

Dev	Weighted deviations (dim x (2N+1))
Noise_sqrt	Noise sqrt-covariance (dim x dim)
S	Output sqrt-covariance (dim x dim)
work	QR workspace matrix ((2N+1+dim) x dim)
tau	QR householder scalars (dim elements)
downdate_work	Scratch for downdate (dim elements)
wc0_negative	True if wc[0] < 0 (requires downdate)
dev0	First deviation column (for downdate, NULL if wc0 >= 0)

Returns: SRUKF_RETURN_OK on success

< QR factorization (double)

Definition at line 1167 of file srukf.c.

References chol_downdate_rank1(), srukf_mat::data, diag_report(), srukf_mat::n_cols, srukf_mat::n_rows, SRUKF_ENTRY, SRUKF_GEQRF, SRUKF_LAPACK_LAYOUT, SRUKF_LEADING_DIM, SRUKF_RETURN_MATH_ERROR, SRUKF_RETURN_OK, and SRUKF_RETURN_PARAMETER_ERROR.

Referenced by srukf_correct_core(), and srukf_predict_core().

◆ srukf_state_dim()

srukf_index srukf_state_dim ( const srukf * ukf )

Get the state dimension N.

Parameters

ukf	Filter instance

Returns: State dimension, or 0 if ukf is NULL/invalid

Definition at line 562 of file srukf.c.

References srukf_mat::n_rows, and srukf::x.

Referenced by ensure_workspace(), srukf_alloc_workspace(), srukf_correct_to(), and srukf_predict_to().

Variable Documentation

◆ g_diag_callback

srukf_diag_fn g_diag_callback = NULL

static

Global diagnostic callback (NULL = disabled)

Definition at line 123 of file srukf.c.

Referenced by diag_report(), and srukf_set_diag_callback().

Macros

Functions

Variables

Detailed Description

Numerical Considerations

Handling Negative Weights

Macro Definition Documentation

◆ DEFAULT_ALPHA

◆ DEFAULT_BETA

◆ DEFAULT_KAPPA

◆ SRUKF_EPS

Function Documentation

◆ alloc_vector()

◆ chol_downdate_rank1()

◆ compute_cross_covariance()

◆ compute_weighted_deviations()

◆ compute_weighted_mean()

◆ diag_report()

◆ ensure_workspace()

◆ generate_sigma_points_from()

◆ is_numeric_valid()

◆ propagate_sigma_points()

◆ srukf_alloc_workspace()

◆ srukf_compute_weights()

◆ srukf_correct()

◆ srukf_correct_core()

◆ srukf_correct_to()

◆ srukf_create()

◆ srukf_create_from_noise()

◆ srukf_free()

◆ srukf_free_workspace()

◆ srukf_get_sqrt_cov()

◆ srukf_get_state()

◆ srukf_init()

◆ srukf_mat_alloc()

◆ srukf_mat_column_view()

◆ srukf_mat_free()

◆ srukf_meas_dim()

◆ srukf_predict()

◆ srukf_predict_core()

◆ srukf_predict_to()

◆ srukf_reset()

◆ srukf_set_diag_callback()

◆ srukf_set_noise()

◆ srukf_set_scale()

◆ srukf_set_sqrt_cov()

◆ srukf_set_state()

◆ srukf_sqrt_from_deviations_ex()

◆ srukf_state_dim()

Variable Documentation

◆ g_diag_callback