#include <unscented_kalman_filter.h>

Public Member Functions
	UnscentedKalmanFilter (const std::function< StateVector(const StateVector &, const InputVector &)> &f, const std::function< OutputVector(const StateVector &, const InputVector &)> &h, const WithInputIntegrator &integrator, const StateVector &state_stddevs, const OutputVector &measurement_stddevs)

	UnscentedKalmanFilter (const std::function< StateVector(const StateVector &, const InputVector &)> &f, const std::function< OutputVector(const StateVector &, const InputVector &)> &h, const WithInputIntegrator &integrator, const StateVector &state_stddevs, const OutputVector &measurement_stddevs, const std::function< StateVector(const EMat< STATES, NUM_SIGMAS > &, const EVec< NUM_SIGMAS > &)> &mean_func_X, const std::function< OutputVector(const EMat< OUTPUTS, NUM_SIGMAS > &, const EVec< NUM_SIGMAS > &)> &mean_func_Y, const std::function< StateVector(const StateVector &, const StateVector &)> &residual_func_X, const std::function< OutputVector(const OutputVector &, const OutputVector &)> &residual_func_Y, const std::function< StateVector(const StateVector &, const StateVector &)> &add_func_X)

StateMatrix	S () const

double	S (int i, int j) const

void	set_S (const StateMatrix &S)

StateMatrix	P () const

void	set_P (const StateMatrix &P)

StateVector	xhat () const

double	xhat (int i) const

void	set_xhat (const StateVector &xhat)

void	set_xhat (int i, double value)

void	reset ()

void	predict (const InputVector &u, double dt)

void	correct (const InputVector &u, const OutputVector &y)

void	correct (const InputVector &u, const OutputVector &y, const EVec< OUTPUTS > &measurement_stddevs)

template<int ROWS>
void	correct (const InputVector &u, const EVec< ROWS > &y, const std::function< EVec< ROWS >(const StateVector &, const InputVector &)> &h, const EVec< ROWS > &measurement_stddevs)

template<int ROWS>
void	correct (const InputVector &u, const EVec< ROWS > &y, const std::function< EVec< ROWS >(const StateVector &, const InputVector &)> &h, const EVec< ROWS > measurement_stddevs, const std::function< EVec< ROWS >(const EMat< ROWS, NUM_SIGMAS > &, const EVec< NUM_SIGMAS > &)> &mean_func_Y, const std::function< EVec< ROWS >(const EVec< ROWS > &, const EVec< ROWS > &)> &residual_func_Y, const std::function< StateVector(const StateVector &, const StateVector &)> &residual_func_X, const std::function< StateVector(const StateVector &, const StateVector &)> &add_func_X)

Detailed Description

template<int STATES, int INPUTS, int OUTPUTS>
class UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >

Kalman filters combine predictions from a model and measurements to estimate a system's true state.

The Unscented Kalman Filter is a nonlinear estimator, meaning that the model used to predict how the state changes over time can be nonlinear. The model that determines the expected measurement given the current state can also be nonlinear.

At each timestep, sigma points are generated close to the mean, they are all propagated forward in time according to the nonlinear model. The Unscented Transform uses the propagated sigma points to compute the prior state and covariance.

When correcting the state and covariance with a measurement, sigma points are again generated, but are transformed into the measurement space using the measurement function. A Kalman gain matrix K is then computed, and used to update the state and covariance.

To read more about Kalman filters and the standard UKF read: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

This implementation is somewhat non-standard. The square-root form of the UKF (SR-UKF) is used, and the way the sigma points are generated is different than most implementations. The square-root form is used to ensure that the covariance matrix remains positive definite.

To learn more about the SR-UKF, and see the exact formulation that most of this implementation follows, read: https://www.researchgate.net/publication/3908304

The sigma points are not generated symmetrically around the mean, instead they are generated as vertices of a simplex. Using N = # of states, this method uses N + 2 sigma points instead of the standard 2N + 1 sigma points. This reduces computation up to 50%. To learn more about this method, read: https://www.sciencedirect.com/science/article/pii/S0888327020308190

This filter uses a method of "recalibrating" by essentially applying a measurement twice instead of once, and only using it if it is more accurate than before the measurement was applied. To learn more about this framework for nonlinear filters, read: https://arxiv.org/pdf/2407.05717

Template Parameters

STATES	Dimension of the state vector.
INPUTS	Dimension of the control input vector.
OUTPUTS	Dimension of the measurement vector.

Constructor & Destructor Documentation

◆ UnscentedKalmanFilter() [1/2]

template<int STATES, int INPUTS, int OUTPUTS>

UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::UnscentedKalmanFilter	(	const std::function< StateVector(const StateVector &, const InputVector &)> &	f,
		const std::function< OutputVector(const StateVector &, const InputVector &)> &	h,
		const WithInputIntegrator &	integrator,
		const StateVector &	state_stddevs,
		const OutputVector &	measurement_stddevs )

inline

Constructs an Unscented Kalman Filter.

Parameters

f	A vector valued function of x and u that returns the derivative of the state vector with respect to time.
h	A vector valued function of x and u that returns the expected measurement at the given state.
integrator	A function from "numerical_integration.h" that integrates a differential equation of the form f(x, u).
state_stddevs	Standard deviations of the states in the model.
measurement_stddevs	Standard deviations of the measurements.

◆ UnscentedKalmanFilter() [2/2]

template<int STATES, int INPUTS, int OUTPUTS>

UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::UnscentedKalmanFilter	(	const std::function< StateVector(const StateVector &, const InputVector &)> &	f,
		const std::function< OutputVector(const StateVector &, const InputVector &)> &	h,
		const WithInputIntegrator &	integrator,
		const StateVector &	state_stddevs,
		const OutputVector &	measurement_stddevs,
		const std::function< StateVector(const EMat< STATES, NUM_SIGMAS > &, const EVec< NUM_SIGMAS > &)> &	mean_func_X,
		const std::function< OutputVector(const EMat< OUTPUTS, NUM_SIGMAS > &, const EVec< NUM_SIGMAS > &)> &	mean_func_Y,
		const std::function< StateVector(const StateVector &, const StateVector &)> &	residual_func_X,
		const std::function< OutputVector(const OutputVector &, const OutputVector &)> &	residual_func_Y,
		const std::function< StateVector(const StateVector &, const StateVector &)> &	add_func_X )

inline

Constructs an Unscented Kalman Filter with custom mean, residual, and addition functions. The most common use for these functions is when you are estimating angles whose arithmetic operations need to be wrapped.

Parameters

f	A vector valued function of x and u that returns the derivative of the state vector with respect to time.
h	A vector valued function of x and u that returns the expected measurement at the given state.
integrator	A function from "numerical_integration.h" that integrates a differential equation of the form f(x, u).
state_stddevs	Standard deviations of the states in the model.
measurement_stddevs	Standard deviations of the measurements.
mean_func_X	A function that computes the mean of a matrix containing NUM_SIGMAS state sigma points with a set of weights for each.
mean_func_Y	A function that computes the mean of a matrix containing NUM_SIGMAS measurement sigma points with a set of weights for each.
residual_func_X	A function that computes the residual of two state vectors, usually by simple subtraction.
residual_func_Y	A function that computes the residual of two measurement vectors, usually by simple subtraction.
add_funx_X	A function that adds two state vectors.

Member Function Documentation

◆ correct() [1/4]

template<int STATES, int INPUTS, int OUTPUTS>

template<int ROWS>

void UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::correct	(	const InputVector &	u,
		const EVec< ROWS > &	y,
		const std::function< EVec< ROWS >(const StateVector &, const InputVector &)> &	h,
		const EVec< ROWS > &	measurement_stddevs )

inline

Correct the state estimate using the measurements in y, a custom measurement function, and custom standard deviations. This is useful for when a different set of measurements are being applied.

Parameters

u	The control input used in the last predict step.
y	The vector of measurements.
h	A vector valued function of x and u that returns the expected measurement at the given state.
measurement_stddevs	The vector of standard deviations for each measurement to be used for this correct step.

◆ correct() [2/4]

template<int STATES, int INPUTS, int OUTPUTS>

template<int ROWS>

void UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::correct	(	const InputVector &	u,
		const EVec< ROWS > &	y,
		const std::function< EVec< ROWS >(const StateVector &, const InputVector &)> &	h,
		const EVec< ROWS >	measurement_stddevs,
		const std::function< EVec< ROWS >(const EMat< ROWS, NUM_SIGMAS > &, const EVec< NUM_SIGMAS > &)> &	mean_func_Y,
		const std::function< EVec< ROWS >(const EVec< ROWS > &, const EVec< ROWS > &)> &	residual_func_Y,
		const std::function< StateVector(const StateVector &, const StateVector &)> &	residual_func_X,
		const std::function< StateVector(const StateVector &, const StateVector &)> &	add_func_X )

inline

Correct the state estimate using the measurements in y, a custom measurement function, custom standard deviations, and custom mean, residual, and addition functions. This is useful for when a different set of measurements are being applied, and they require custom arithmetic functions.

Parameters

u	The control input used in the last predict step.
y	The vector of measurements.
h	A vector valued function of x and u that returns the expected measurement at the given state.
measurement_stddevs	The vector of standard deviations for each measurement to be used for this correct step.
mean_func_Y	A function that computes the mean of a matrix containing NUM_SIGMAS measurement sigma points with a set of weights for each.
residual_func_X	A function that computes the residual of two state vectors, usually by simple subtraction.
residual_func_Y	A function that computes the residual of two measurement vectors, usually by simple subtraction.
add_funx_X	A function that adds two state vectors.

◆ correct() [3/4]

template<int STATES, int INPUTS, int OUTPUTS>

void UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::correct	(	const InputVector &	u,
		const OutputVector &	y )

inline

Correct the state estimate using the measurements in y.

Parameters

u	The control input used in the last predict step.
y	The vector of measurements.

◆ correct() [4/4]

template<int STATES, int INPUTS, int OUTPUTS>

void UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::correct	(	const InputVector &	u,
		const OutputVector &	y,
		const EVec< OUTPUTS > &	measurement_stddevs )

inline

Correct the state estimate using the measurements in y, and custom standard deviations. This is useful for when the noise in the measurements vary.

Parameters

u	The control input used in the last predict step.
y	The vector of measurements.
measurement_stddevs	The vector of standard deviations for each measurement to be used for this correct step.

◆ P()

template<int STATES, int INPUTS, int OUTPUTS>

StateMatrix UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::P ( ) const

inline

Returns the reconstructed covariance matrix P.

◆ predict()

template<int STATES, int INPUTS, int OUTPUTS>

void UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::predict	(	const InputVector &	u,
		double	dt )

inline

Projects the state into the future by dt seconds with control input u.

Parameters

u	The control input.
dt	The timestep in seconds.

◆ reset()

template<int STATES, int INPUTS, int OUTPUTS>

void UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::reset ( )

inline

Resets the filter.

◆ S() [1/2]

template<int STATES, int INPUTS, int OUTPUTS>

StateMatrix UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::S ( ) const

inline

Returns the square-root covariance matrix S.

◆ S() [2/2]

template<int STATES, int INPUTS, int OUTPUTS>

double UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::S	(	int	i,
		int	j ) const

inline

Returns one element of the square-root covariance matrix S.

Parameters

i	Row of S.
j	Column of S.

◆ set_P()

template<int STATES, int INPUTS, int OUTPUTS>

void UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::set_P ( const StateMatrix & P )

inline

Set the current square-root covariance matrix S to the square-root of P.

Parameters

P	The covariance matrix P.

◆ set_S()

template<int STATES, int INPUTS, int OUTPUTS>

void UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::set_S ( const StateMatrix & S )

inline

Set the current square-root covariance matrix S.

Parameters

S	The new square-root covariance matrix S.

◆ set_xhat() [1/2]

template<int STATES, int INPUTS, int OUTPUTS>

void UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::set_xhat ( const StateVector & xhat )

inline

Set the current state estimate x-hat.

◆ set_xhat() [2/2]

template<int STATES, int INPUTS, int OUTPUTS>

void UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::set_xhat	(	int	i,
		double	value )

inline

Set one element of the current state estimate x-hat.

Parameters

i	Row of x-hat.

◆ xhat() [1/2]

template<int STATES, int INPUTS, int OUTPUTS>

StateVector UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::xhat ( ) const

inline

Returns the current state estimate x-hat.

◆ xhat() [2/2]

template<int STATES, int INPUTS, int OUTPUTS>

double UnscentedKalmanFilter< STATES, INPUTS, OUTPUTS >::xhat ( int i ) const

inline

Returns one element of the current state estimate x-hat.

Parameters

i	Row of x-hat.

The documentation for this class was generated from the following file:

unscented_kalman_filter.h

Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ UnscentedKalmanFilter() [1/2]

◆ UnscentedKalmanFilter() [2/2]

Member Function Documentation

◆ correct() [1/4]

◆ correct() [2/4]

◆ correct() [3/4]

◆ correct() [4/4]

◆ P()

◆ predict()

◆ reset()

◆ S() [1/2]

◆ S() [2/2]

◆ set_P()

◆ set_S()

◆ set_xhat() [1/2]

◆ set_xhat() [2/2]

◆ xhat() [1/2]

◆ xhat() [2/2]