Classes
struct	FilterState
	Filter state container holding estimate and covariance. More...
struct	ProcessNoise
	Process noise specification. More...
struct	MeasurementNoise
	Measurement noise specification. More...
struct	UKFParams
	UKF tuning parameters. More...

Functions
template<typename Scalar, int M>
Scalar	normalized_innovation_squared (const Eigen::Matrix< Scalar, M, 1 > &innovation, const Eigen::Matrix< Scalar, M, M > &S)
	Compute normalized innovation squared (NIS).
template<typename Scalar, int M>
bool	passes_gate (const Eigen::Matrix< Scalar, M, 1 > &innovation, const Eigen::Matrix< Scalar, M, M > &S, double threshold)
	Chi-squared gating for outlier rejection.
template<typename Scalar, int M>
bool	measurement_passes_gate (const Eigen::Matrix< Scalar, M, 1 > &z, const Eigen::Matrix< Scalar, M, 1 > &z_pred, const Eigen::Matrix< Scalar, M, M > &S, double threshold)
	Compute innovation and check gate in one call.
template<int N, int M>
Eigen::Matrix< double, N, N >	joseph_update (const Eigen::Matrix< double, N, N > &P_prior, const Eigen::Matrix< double, N, M > &K, const Eigen::Matrix< double, M, N > &H, const Eigen::Matrix< double, M, M > &R)
	Joseph form covariance update.
double	chi_squared_threshold (int dof, double confidence=0.95)
	Chi-squared threshold lookup.
template<int N>
Eigen::Matrix< double, N, N >	symmetrize (const Eigen::Matrix< double, N, N > &P)
	Enforce covariance matrix symmetry.
template<typename Scalar, int N, int U, typename DynamicsFunc>
FilterState< Scalar, N >	ekf_predict (const FilterState< Scalar, N > &state, const Eigen::Matrix< Scalar, U, 1 > &u, DynamicsFunc f, const Eigen::Matrix< double, N, N > &F, const ProcessNoise< N > &noise)
	Extended Kalman filter predict step.
template<typename Scalar, int N, typename DynamicsFunc>
FilterState< Scalar, N >	ekf_predict (const FilterState< Scalar, N > &state, DynamicsFunc f, const Eigen::Matrix< double, N, N > &F, const ProcessNoise< N > &noise)
	Extended Kalman filter predict (no control input).
template<typename Scalar, int N, int M, typename MeasurementFunc>
FilterState< Scalar, N >	ekf_update (const FilterState< Scalar, N > &state, const Eigen::Matrix< Scalar, M, 1 > &z, MeasurementFunc h, const Eigen::Matrix< double, M, N > &H, const MeasurementNoise< M > &noise)
	Extended Kalman filter update step.
template<typename Scalar, int N, int U, int M, typename DynamicsFunc, typename MeasurementFunc>
FilterState< Scalar, N >	ekf_step (const FilterState< Scalar, N > &state, const Eigen::Matrix< Scalar, U, 1 > &u, const Eigen::Matrix< Scalar, M, 1 > &z, DynamicsFunc f, MeasurementFunc h, const Eigen::Matrix< double, N, N > &F, const Eigen::Matrix< double, M, N > &H, const ProcessNoise< N > &Q, const MeasurementNoise< M > &R)
	Combined EKF step (predict + update).
template<typename Scalar, int N, int U>
FilterState< Scalar, N >	kf_predict (const FilterState< Scalar, N > &state, const Eigen::Matrix< double, N, N > &F, const Eigen::Matrix< double, N, U > &B, const Eigen::Matrix< Scalar, U, 1 > &u, const ProcessNoise< N > &noise)
	Linear Kalman filter predict step.
template<typename Scalar, int N>
FilterState< Scalar, N >	kf_predict (const FilterState< Scalar, N > &state, const Eigen::Matrix< double, N, N > &F, const ProcessNoise< N > &noise)
	Linear Kalman filter predict (no control input).
template<typename Scalar, int N, int M>
FilterState< Scalar, N >	kf_update (const FilterState< Scalar, N > &state, const Eigen::Matrix< Scalar, M, 1 > &z, const Eigen::Matrix< double, M, N > &H, const MeasurementNoise< M > &noise)
	Linear Kalman filter update step.
template<typename Scalar, int N, int U, int M>
FilterState< Scalar, N >	kf_step (const FilterState< Scalar, N > &state, const Eigen::Matrix< Scalar, U, 1 > &u, const Eigen::Matrix< Scalar, M, 1 > &z, const Eigen::Matrix< double, N, N > &F, const Eigen::Matrix< double, N, U > &B, const Eigen::Matrix< double, M, N > &H, const ProcessNoise< N > &Q, const MeasurementNoise< M > &R)
	Combined Kalman filter step (predict + update).
template<typename Scalar, int N, int M>
FilterState< Scalar, N >	kf_step (const FilterState< Scalar, N > &state, const Eigen::Matrix< Scalar, M, 1 > &z, const Eigen::Matrix< double, N, N > &F, const Eigen::Matrix< double, M, N > &H, const ProcessNoise< N > &Q, const MeasurementNoise< M > &R)
	Combined Kalman filter step (no control input).
template<int N>
Eigen::Matrix< double, N, 2 *N+1 >	sigma_points (const Eigen::Matrix< double, N, 1 > &x, const Eigen::Matrix< double, N, N > &P, const UKFParams &params)
	Generate sigma points for UKF.
template<int N, int U, typename DynamicsFunc>
FilterState< double, N >	ukf_predict (const FilterState< double, N > &state, const Eigen::Matrix< double, U, 1 > &u, DynamicsFunc f, const ProcessNoise< N > &noise, const UKFParams &params={})
	Unscented Kalman filter predict step.
template<int N, typename DynamicsFunc>
FilterState< double, N >	ukf_predict (const FilterState< double, N > &state, DynamicsFunc f, const ProcessNoise< N > &noise, const UKFParams &params={})
	UKF predict (no control input).
template<int N, int M, typename MeasurementFunc>
FilterState< double, N >	ukf_update (const FilterState< double, N > &state, const Eigen::Matrix< double, M, 1 > &z, MeasurementFunc h, const MeasurementNoise< M > &noise, const UKFParams &params={})
	Unscented Kalman filter update step.
template<int N, int U, int M, typename DynamicsFunc, typename MeasurementFunc>
FilterState< double, N >	ukf_step (const FilterState< double, N > &state, const Eigen::Matrix< double, U, 1 > &u, const Eigen::Matrix< double, M, 1 > &z, DynamicsFunc f, MeasurementFunc h, const ProcessNoise< N > &Q, const MeasurementNoise< M > &R, const UKFParams &params={})
	Combined UKF step (predict + update).

Function Documentation

◆ chi_squared_threshold()

double vulcan::estimation::chi_squared_threshold	(	int	dof,
		double	confidence = 0.95 )

inline

Chi-squared threshold lookup.

Returns the chi-squared critical value for the given degrees of freedom and confidence level.

Parameters

dof	Degrees of freedom (measurement dimension)
confidence	Confidence level (0.95 or 0.99)

Returns: Chi-squared threshold

◆ ekf_predict() [1/2]

template<typename Scalar, int N, int U, typename DynamicsFunc>

FilterState< Scalar, N > vulcan::estimation::ekf_predict	(	const FilterState< Scalar, N > &	state,
		const Eigen::Matrix< Scalar, U, 1 > &	u,
		DynamicsFunc	f,
		const Eigen::Matrix< double, N, N > &	F,
		const ProcessNoise< N > &	noise )

Extended Kalman filter predict step.

Propagates state through nonlinear dynamics with linear covariance update: x̂⁻ = f(x̂, u) (nonlinear state propagation) P⁻ = F*P*Fᵀ + Q (linearized covariance update)

The user provides both the nonlinear dynamics function and its Jacobian.

Template Parameters

Scalar	Numeric type (double or casadi::MX)
N	State dimension
U	Control input dimension
DynamicsFunc	Callable: x_next = f(x, u)

Parameters

state	Current filter state (x, P)
u	Control input vector [U x 1]
f	Nonlinear dynamics function
F	Jacobian of dynamics ∂f/∂x evaluated at current state [N x N]
noise	Process noise specification (Q)

Returns: Predicted filter state (x⁻, P⁻)

◆ ekf_predict() [2/2]

template<typename Scalar, int N, typename DynamicsFunc>

FilterState< Scalar, N > vulcan::estimation::ekf_predict	(	const FilterState< Scalar, N > &	state,
		DynamicsFunc	f,
		const Eigen::Matrix< double, N, N > &	F,
		const ProcessNoise< N > &	noise )

Extended Kalman filter predict (no control input).

For autonomous nonlinear systems.

Template Parameters

DynamicsFunc Callable: x_next = f(x)

◆ ekf_step()

template<typename Scalar, int N, int U, int M, typename DynamicsFunc, typename MeasurementFunc>

FilterState< Scalar, N > vulcan::estimation::ekf_step	(	const FilterState< Scalar, N > &	state,
		const Eigen::Matrix< Scalar, U, 1 > &	u,
		const Eigen::Matrix< Scalar, M, 1 > &	z,
		DynamicsFunc	f,
		MeasurementFunc	h,
		const Eigen::Matrix< double, N, N > &	F,
		const Eigen::Matrix< double, M, N > &	H,
		const ProcessNoise< N > &	Q,
		const MeasurementNoise< M > &	R )

Combined EKF step (predict + update).

Template Parameters

DynamicsFunc	Callable: x_next = f(x, u)
MeasurementFunc	Callable: z_pred = h(x)

◆ ekf_update()

template<typename Scalar, int N, int M, typename MeasurementFunc>

FilterState< Scalar, N > vulcan::estimation::ekf_update	(	const FilterState< Scalar, N > &	state,
		const Eigen::Matrix< Scalar, M, 1 > &	z,
		MeasurementFunc	h,
		const Eigen::Matrix< double, M, N > &	H,
		const MeasurementNoise< M > &	noise )

Extended Kalman filter update step.

Incorporates measurement using nonlinear observation model: z_pred = h(x̂⁻) (predicted measurement) S = H*P⁻*Hᵀ + R (innovation covariance) K = P⁻*Hᵀ*S⁻¹ (Kalman gain) x̂ = x̂⁻ + K*(z - z_pred) (state update) P = (I - K*H)*P⁻ (covariance update)

Template Parameters

Scalar	Numeric type (double or casadi::MX)
N	State dimension
M	Measurement dimension
MeasurementFunc	Callable: z_pred = h(x)

Parameters

state	Predicted filter state (x⁻, P⁻)
z	Measurement vector [M x 1]
h	Nonlinear measurement function
H	Jacobian of measurement model ∂h/∂x [M x N]
noise	Measurement noise specification (R)

Returns: Updated filter state (x̂, P)

◆ joseph_update()

template<int N, int M>

Eigen::Matrix< double, N, N > vulcan::estimation::joseph_update	(	const Eigen::Matrix< double, N, N > &	P_prior,
		const Eigen::Matrix< double, N, M > &	K,
		const Eigen::Matrix< double, M, N > &	H,
		const Eigen::Matrix< double, M, M > &	R )

Joseph form covariance update.

Numerically stable covariance update that guarantees positive definiteness: P = (I - K*H)*P⁻*(I - K*H)ᵀ + K*R*Kᵀ

This is equivalent to the standard update P = (I - K*H)*P⁻ but is more robust to numerical errors and always produces a symmetric, positive semi-definite result.

Template Parameters

N	State dimension
M	Measurement dimension

Parameters

P_prior	Prior covariance P⁻ [N x N]
K	Kalman gain [N x M]
H	Measurement matrix [M x N]
R	Measurement noise covariance [M x M]

Returns: Updated covariance P [N x N]

◆ kf_predict() [1/2]

template<typename Scalar, int N, int U>

FilterState< Scalar, N > vulcan::estimation::kf_predict	(	const FilterState< Scalar, N > &	state,
		const Eigen::Matrix< double, N, N > &	F,
		const Eigen::Matrix< double, N, U > &	B,
		const Eigen::Matrix< Scalar, U, 1 > &	u,
		const ProcessNoise< N > &	noise )

Linear Kalman filter predict step.

Propagates state and covariance through linear dynamics: x̂⁻ = F*x̂ + B*u P⁻ = F*P*Fᵀ + Q

Template Parameters

Scalar	Numeric type (double or casadi::MX)
N	State dimension
U	Control input dimension

Parameters

state	Current filter state (x, P)
F	State transition matrix [N x N]
B	Control input matrix [N x U]
u	Control input vector [U x 1]
noise	Process noise specification (Q)

Returns: Predicted filter state (x⁻, P⁻)

◆ kf_predict() [2/2]

template<typename Scalar, int N>

FilterState< Scalar, N > vulcan::estimation::kf_predict	(	const FilterState< Scalar, N > &	state,
		const Eigen::Matrix< double, N, N > &	F,
		const ProcessNoise< N > &	noise )

Linear Kalman filter predict (no control input).

Simplified predict for autonomous systems: x̂⁻ = F*x̂ P⁻ = F*P*Fᵀ + Q

Template Parameters

Scalar	Numeric type
N	State dimension

◆ kf_step() [1/2]

template<typename Scalar, int N, int M>

FilterState< Scalar, N > vulcan::estimation::kf_step	(	const FilterState< Scalar, N > &	state,
		const Eigen::Matrix< Scalar, M, 1 > &	z,
		const Eigen::Matrix< double, N, N > &	F,
		const Eigen::Matrix< double, M, N > &	H,
		const ProcessNoise< N > &	Q,
		const MeasurementNoise< M > &	R )

Combined Kalman filter step (no control input).

◆ kf_step() [2/2]

template<typename Scalar, int N, int U, int M>

FilterState< Scalar, N > vulcan::estimation::kf_step	(	const FilterState< Scalar, N > &	state,
		const Eigen::Matrix< Scalar, U, 1 > &	u,
		const Eigen::Matrix< Scalar, M, 1 > &	z,
		const Eigen::Matrix< double, N, N > &	F,
		const Eigen::Matrix< double, N, U > &	B,
		const Eigen::Matrix< double, M, N > &	H,
		const ProcessNoise< N > &	Q,
		const MeasurementNoise< M > &	R )

Combined Kalman filter step (predict + update).

Convenience function that performs both predict and update in one call.

Template Parameters

Scalar	Numeric type
N	State dimension
U	Control input dimension
M	Measurement dimension

◆ kf_update()

template<typename Scalar, int N, int M>

FilterState< Scalar, N > vulcan::estimation::kf_update	(	const FilterState< Scalar, N > &	state,
		const Eigen::Matrix< Scalar, M, 1 > &	z,
		const Eigen::Matrix< double, M, N > &	H,
		const MeasurementNoise< M > &	noise )

Linear Kalman filter update step.

Incorporates measurement into state estimate: S = H*P⁻*Hᵀ + R (innovation covariance) K = P⁻*Hᵀ*S⁻¹ (Kalman gain) x̂ = x̂⁻ + K*(z - H*x̂⁻) (state update) P = (I - K*H)*P⁻ (covariance update)

Template Parameters

Scalar	Numeric type (double or casadi::MX)
N	State dimension
M	Measurement dimension

Parameters

state	Predicted filter state (x⁻, P⁻)
z	Measurement vector [M x 1]
H	Measurement matrix [M x N]
noise	Measurement noise specification (R)

Returns: Updated filter state (x̂, P)

◆ measurement_passes_gate()

template<typename Scalar, int M>

bool vulcan::estimation::measurement_passes_gate	(	const Eigen::Matrix< Scalar, M, 1 > &	z,
		const Eigen::Matrix< Scalar, M, 1 > &	z_pred,
		const Eigen::Matrix< Scalar, M, M > &	S,
		double	threshold )

Compute innovation and check gate in one call.

Convenience function that computes innovation from predicted and actual measurements, then checks the gate.

Template Parameters

Scalar	Numeric type
M	Measurement dimension

Parameters

z	Actual measurement [M x 1]
z_pred	Predicted measurement [M x 1]
S	Innovation covariance [M x M]
threshold	Chi-squared threshold

Returns: true if measurement passes gate

◆ normalized_innovation_squared()

template<typename Scalar, int M>

Scalar vulcan::estimation::normalized_innovation_squared	(	const Eigen::Matrix< Scalar, M, 1 > &	innovation,
		const Eigen::Matrix< Scalar, M, M > &	S )

Compute normalized innovation squared (NIS).

NIS = yᵀ * S⁻¹ * y

Used for filter consistency checking. For a properly tuned filter, NIS should follow a chi-squared distribution with M degrees of freedom.

Template Parameters

Scalar	Numeric type
M	Innovation (measurement residual) dimension

Parameters

innovation	Measurement residual y = z - z_pred [M x 1]
S	Innovation covariance [M x M]

Returns: Normalized innovation squared (scalar)

◆ passes_gate()

template<typename Scalar, int M>

bool vulcan::estimation::passes_gate	(	const Eigen::Matrix< Scalar, M, 1 > &	innovation,
		const Eigen::Matrix< Scalar, M, M > &	S,
		double	threshold )

Chi-squared gating for outlier rejection.

Returns true if the innovation passes the gate (is consistent with filter). A measurement fails the gate if NIS > threshold.

Common thresholds (from chi-squared distribution): M=1: 3.84 (95%), 6.63 (99%) M=2: 5.99 (95%), 9.21 (99%) M=3: 7.81 (95%), 11.34 (99%)

Template Parameters

Scalar	Numeric type
M	Innovation dimension

Parameters

innovation	Measurement residual [M x 1]
S	Innovation covariance [M x M]
threshold	Chi-squared threshold for gating

Returns: true if NIS <= threshold (measurement passes gate)

◆ sigma_points()

template<int N>

Eigen::Matrix< double, N, 2 *N+1 > vulcan::estimation::sigma_points	(	const Eigen::Matrix< double, N, 1 > &	x,
		const Eigen::Matrix< double, N, N > &	P,
		const UKFParams &	params )

Generate sigma points for UKF.

Generates 2N+1 sigma points using the matrix square root (Cholesky). Points are arranged as columns: [x, x + √((n+λ)P), x - √((n+λ)P)]

Template Parameters

N	State dimension

Parameters

x	Mean state vector [N x 1]
P	State covariance [N x N]
params	UKF tuning parameters

Returns: Sigma points matrix [N x 2N+1]

Note: Numeric-only (Cholesky decomposition not available for symbolic)

◆ symmetrize()

template<int N>

Eigen::Matrix< double, N, N > vulcan::estimation::symmetrize ( const Eigen::Matrix< double, N, N > & P )

Enforce covariance matrix symmetry.

Forces a covariance matrix to be exactly symmetric by averaging with its transpose. Useful after numerical operations that may introduce small asymmetries.

Template Parameters

N	Matrix dimension

Parameters

P	Covariance matrix [N x N]

Returns: Symmetrized covariance (P + Pᵀ)/2

◆ ukf_predict() [1/2]

template<int N, int U, typename DynamicsFunc>

FilterState< double, N > vulcan::estimation::ukf_predict	(	const FilterState< double, N > &	state,
		const Eigen::Matrix< double, U, 1 > &	u,
		DynamicsFunc	f,
		const ProcessNoise< N > &	noise,
		const UKFParams &	params = {} )

Unscented Kalman filter predict step.

Propagates sigma points through nonlinear dynamics:

Generate sigma points from current state
Propagate each sigma point through f(x, u)
Compute weighted mean and covariance of propagated points

Template Parameters

N	State dimension
U	Control input dimension
DynamicsFunc	Callable: x_next = f(x, u)

Parameters

state	Current filter state (x, P) - must be double for Cholesky
u	Control input vector [U x 1]
f	Nonlinear dynamics function
noise	Process noise specification (Q)
params	UKF tuning parameters

Returns: Predicted filter state (x⁻, P⁻)

Note: Numeric-only due to Cholesky decomposition requirement

◆ ukf_predict() [2/2]

template<int N, typename DynamicsFunc>

FilterState< double, N > vulcan::estimation::ukf_predict	(	const FilterState< double, N > &	state,
		DynamicsFunc	f,
		const ProcessNoise< N > &	noise,
		const UKFParams &	params = {} )

UKF predict (no control input).

◆ ukf_step()

template<int N, int U, int M, typename DynamicsFunc, typename MeasurementFunc>

FilterState< double, N > vulcan::estimation::ukf_step	(	const FilterState< double, N > &	state,
		const Eigen::Matrix< double, U, 1 > &	u,
		const Eigen::Matrix< double, M, 1 > &	z,
		DynamicsFunc	f,
		MeasurementFunc	h,
		const ProcessNoise< N > &	Q,
		const MeasurementNoise< M > &	R,
		const UKFParams &	params = {} )

Combined UKF step (predict + update).

◆ ukf_update()

template<int N, int M, typename MeasurementFunc>

FilterState< double, N > vulcan::estimation::ukf_update	(	const FilterState< double, N > &	state,
		const Eigen::Matrix< double, M, 1 > &	z,
		MeasurementFunc	h,
		const MeasurementNoise< M > &	noise,
		const UKFParams &	params = {} )

Unscented Kalman filter update step.

Incorporates measurement using sigma points:

Generate sigma points from predicted state
Propagate each through measurement model h(x)
Compute innovation covariance and cross-covariance
Compute Kalman gain and update state

Template Parameters

N	State dimension
M	Measurement dimension
MeasurementFunc	Callable: z_pred = h(x)

Parameters

state	Predicted filter state (x⁻, P⁻)
z	Measurement vector [M x 1]
h	Nonlinear measurement function
noise	Measurement noise specification (R)
params	UKF tuning parameters

Returns: Updated filter state (x̂, P)

Classes

Functions

Function Documentation

◆ chi_squared_threshold()

◆ ekf_predict() [1/2]

◆ ekf_predict() [2/2]

◆ ekf_step()

◆ ekf_update()

◆ joseph_update()

◆ kf_predict() [1/2]

◆ kf_predict() [2/2]

◆ kf_step() [1/2]

◆ kf_step() [2/2]

◆ kf_update()

◆ measurement_passes_gate()

◆ normalized_innovation_squared()

◆ passes_gate()

◆ sigma_points()

◆ symmetrize()

◆ ukf_predict() [1/2]

◆ ukf_predict() [2/2]

◆ ukf_step()

◆ ukf_update()