Square-Root Unscented Kalman Filter (SR-UKF) Library. More...

#include <assert.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <cblas.h>
#include <lapacke.h>

Classes
struct	srukf_mat
	Matrix structure. More...

struct	srukf
	Square-Root Unscented Kalman Filter. More...

Macros
#define	SRUKF_ENTRY(A, i, j) ((A)->data[(i) * (A)->inc_row + (j) * (A)->inc_col])
	Element access macro (column-major)

#define	SRUKF_SET_TYPE(A, t) ((A)->type \|= (t))
	Set a type flag on a matrix.

#define	SRUKF_UNSET_TYPE(A, t) ((A)->type &= ~(t))
	Clear a type flag on a matrix.

#define	SRUKF_IS_TYPE(A, t) ((A)->type & (t))
	Test if a type flag is set.

#define	SRUKF_LEADING_DIM(A) ((A)->n_rows)
	Get leading dimension for BLAS/LAPACK (always n_rows for col-major)

#define	SRUKF_CBLAS_LAYOUT CblasColMajor
	BLAS layout constant.

#define	SRUKF_LAPACK_LAYOUT LAPACK_COL_MAJOR
	LAPACK layout constant.

#define	SRUKF_GEMM cblas_dgemm

#define	SRUKF_TRSM cblas_dtrsm

#define	SRUKF_TRSV cblas_dtrsv

#define	SRUKF_GEQRF LAPACKE_dgeqrf

#define	SRUKF_POTRF LAPACKE_dpotrf

#define	SRUKF_MAT_ALLOC(rows, cols) srukf_mat_alloc((rows), (cols), 1)
	Allocate a matrix with data buffer.

#define	SRUKF_MAT_ALLOC_NO_DATA(rows, cols) srukf_mat_alloc((rows), (cols), 0)
	Allocate a matrix struct only (data pointer will be NULL)

Typedefs
typedef size_t	srukf_index
	Scalar index type.

typedef double	srukf_value
	Scalar value type.

typedef struct srukf_workspace	srukf_workspace
	Opaque workspace structure.

typedef void(*	srukf_diag_fn) (const char *msg)
	Diagnostic callback function type.

Enumerations
enum	srukf_return { SRUKF_RETURN_OK = 0 , SRUKF_RETURN_PARAMETER_ERROR , SRUKF_RETURN_MATH_ERROR }
	Function return codes. More...

enum	srukf_mat_type { SRUKF_TYPE_COL_MAJOR = 0x01 , SRUKF_TYPE_NO_DATA = 0x02 , SRUKF_TYPE_VECTOR = 0x04 , SRUKF_TYPE_SQUARE = 0x08 }
	Matrix type flags. More...

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.

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

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

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

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

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

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.

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

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

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_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_alloc_workspace (srukf *ukf)
	Pre-allocate workspace.

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

Detailed Description

Square-Root Unscented Kalman Filter (SR-UKF) Library.

Definition in file srukf.h.

Macro Definition Documentation

◆ SRUKF_CBLAS_LAYOUT

#define SRUKF_CBLAS_LAYOUT CblasColMajor

BLAS layout constant.

Definition at line 313 of file srukf.h.

◆ SRUKF_ENTRY

#define SRUKF_ENTRY	(	A,
		i,
		j
	)	((A)->data[(i) * (A)->inc_row + (j) * (A)->inc_col])

Element access macro (column-major)

Parameters

A	Pointer to srukf_mat
i	Row index (0-based)
j	Column index (0-based)

Returns: Reference to element A(i,j)

Example:

SRUKF_ENTRY(mat, 0, 0) = 1.0; // Set top-left element

double val = SRUKF_ENTRY(mat, 1, 2); // Read element at row 1, col 2

SRUKF_ENTRY

#define SRUKF_ENTRY(A, i, j)

Element access macro (column-major)

Definition srukf.h:298

Definition at line 298 of file srukf.h.

◆ SRUKF_GEMM

#define SRUKF_GEMM cblas_dgemm

Matrix multiply (double)

Definition at line 325 of file srukf.h.

◆ SRUKF_GEQRF

#define SRUKF_GEQRF LAPACKE_dgeqrf

QR factorization (double)

Definition at line 328 of file srukf.h.

◆ SRUKF_IS_TYPE

#define SRUKF_IS_TYPE	(	A,
		t
	)	((A)->type & (t))

Test if a type flag is set.

Definition at line 306 of file srukf.h.

◆ SRUKF_LAPACK_LAYOUT

#define SRUKF_LAPACK_LAYOUT LAPACK_COL_MAJOR

LAPACK layout constant.

Definition at line 315 of file srukf.h.

◆ SRUKF_LEADING_DIM

#define SRUKF_LEADING_DIM ( A ) ((A)->n_rows)

Get leading dimension for BLAS/LAPACK (always n_rows for col-major)

Definition at line 310 of file srukf.h.

◆ SRUKF_MAT_ALLOC

#define SRUKF_MAT_ALLOC	(	rows,
		cols
	)	srukf_mat_alloc((rows), (cols), 1)

Allocate a matrix with data buffer.

Definition at line 357 of file srukf.h.

◆ SRUKF_MAT_ALLOC_NO_DATA

#define SRUKF_MAT_ALLOC_NO_DATA	(	rows,
		cols
	)	srukf_mat_alloc((rows), (cols), 0)

Allocate a matrix struct only (data pointer will be NULL)

Definition at line 359 of file srukf.h.

◆ SRUKF_POTRF

#define SRUKF_POTRF LAPACKE_dpotrf

Cholesky factorization (double)

Definition at line 329 of file srukf.h.

◆ SRUKF_SET_TYPE

#define SRUKF_SET_TYPE	(	A,
		t
	)	((A)->type \|= (t))

Set a type flag on a matrix.

Definition at line 302 of file srukf.h.

◆ SRUKF_TRSM

#define SRUKF_TRSM cblas_dtrsm

Triangular solve (double)

Definition at line 326 of file srukf.h.

◆ SRUKF_TRSV

#define SRUKF_TRSV cblas_dtrsv

Triangular vector solve (double)

Definition at line 327 of file srukf.h.

◆ SRUKF_UNSET_TYPE

#define SRUKF_UNSET_TYPE	(	A,
		t
	)	((A)->type &= ~(t))

Clear a type flag on a matrix.

Definition at line 304 of file srukf.h.

Typedef Documentation

◆ srukf_diag_fn

typedef void(* srukf_diag_fn) (const char *msg)

Diagnostic callback function type.

User-provided function to receive diagnostic messages from the library. Useful for debugging filter issues.

Parameters

msg	Null-terminated diagnostic message

Definition at line 457 of file srukf.h.

◆ srukf_index

typedef size_t srukf_index

Scalar index type.

Used for matrix dimensions and loop indices. Using size_t ensures we can handle matrices up to the platform's memory limits.

Definition at line 218 of file srukf.h.

◆ srukf_value

typedef double srukf_value

Scalar value type.

Double precision by default. Define SRUKF_SINGLE before including this header for single precision (float), which may be faster on some hardware but reduces numerical precision.

Definition at line 230 of file srukf.h.

◆ srukf_workspace

typedef struct srukf_workspace srukf_workspace

Opaque workspace structure.

Pre-allocated temporaries to avoid malloc during predict/correct. Allocated on demand, freed with filter or explicitly.

Definition at line 375 of file srukf.h.

Enumeration Type Documentation

◆ srukf_mat_type

enum srukf_mat_type

Matrix type flags.

Bit flags describing matrix properties. Used internally for validation and optimization.

Enumerator
SRUKF_TYPE_COL_MAJOR	Column-major storage (always set)
SRUKF_TYPE_NO_DATA	Matrix struct exists but data not allocated
SRUKF_TYPE_VECTOR	Single column (n_cols == 1)
SRUKF_TYPE_SQUARE	Square matrix (n_rows == n_cols)

Definition at line 252 of file srukf.h.

◆ srukf_return

enum srukf_return

Function return codes.

All functions that can fail return one of these codes. Check the return value and handle errors appropriately.

Enumerator
SRUKF_RETURN_OK	Success
SRUKF_RETURN_PARAMETER_ERROR	Invalid parameter (NULL, wrong dims, etc.)
SRUKF_RETURN_MATH_ERROR	Numerical failure (non-SPD matrix, etc.)

Definition at line 239 of file srukf.h.

Function Documentation

◆ 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_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_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_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_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_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_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().

Classes

Macros

Typedefs

Enumerations

Functions

Detailed Description

Macro Definition Documentation

◆ SRUKF_CBLAS_LAYOUT

◆ SRUKF_ENTRY

◆ SRUKF_GEMM

◆ SRUKF_GEQRF

◆ SRUKF_IS_TYPE

◆ SRUKF_LAPACK_LAYOUT

◆ SRUKF_LEADING_DIM

◆ SRUKF_MAT_ALLOC

◆ SRUKF_MAT_ALLOC_NO_DATA

◆ SRUKF_POTRF

◆ SRUKF_SET_TYPE

◆ SRUKF_TRSM

◆ SRUKF_TRSV

◆ SRUKF_UNSET_TYPE

Typedef Documentation

◆ srukf_diag_fn

◆ srukf_index

◆ srukf_value

◆ srukf_workspace

Enumeration Type Documentation

◆ srukf_mat_type

◆ srukf_return

Function Documentation

◆ srukf_alloc_workspace()

◆ srukf_correct()

◆ srukf_correct_to()

◆ srukf_create()

◆ srukf_create_from_noise()

◆ srukf_free()

◆ srukf_free_workspace()

◆ srukf_get_sqrt_cov()

◆ srukf_get_state()

◆ srukf_mat_alloc()

◆ srukf_mat_free()

◆ srukf_meas_dim()

◆ srukf_predict()

◆ srukf_predict_to()

◆ srukf_reset()

◆ srukf_set_diag_callback()

◆ srukf_set_noise()

◆ srukf_set_scale()

◆ srukf_set_sqrt_cov()

◆ srukf_set_state()

◆ srukf_state_dim()