A Kalman filter combines predictions from a model and measurements to give an estimate of the true system state. More...

#include <frc/estimator/UnscentedKalmanFilter.h>

Public Types
using	StateVector = Vectord< States >

using	InputVector = Vectord< Inputs >

using	OutputVector = Vectord< Outputs >

using	StateArray = wpi::array< double, States >

using	OutputArray = wpi::array< double, Outputs >

using	StateMatrix = Matrixd< States, States >

Public Member Functions
	UnscentedKalmanFilter (std::function< StateVector(const StateVector &, const InputVector &)> f, std::function< OutputVector(const StateVector &, const InputVector &)> h, const StateArray &stateStdDevs, const OutputArray &measurementStdDevs, units::second_t dt)
	Constructs an unscented Kalman filter. More...

	UnscentedKalmanFilter (std::function< StateVector(const StateVector &, const InputVector &)> f, std::function< OutputVector(const StateVector &, const InputVector &)> h, const StateArray &stateStdDevs, const OutputArray &measurementStdDevs, std::function< StateVector(const Matrixd< States, 2 States+1 > &, const Vectord< 2 States+1 > &)> meanFuncX, std::function< OutputVector(const Matrixd< Outputs, 2 States+1 > &, const Vectord< 2 States+1 > &)> meanFuncY, std::function< StateVector(const StateVector &, const StateVector &)> residualFuncX, std::function< OutputVector(const OutputVector &, const OutputVector &)> residualFuncY, std::function< StateVector(const StateVector &, const StateVector &)> addFuncX, units::second_t dt)
	Constructs an unscented Kalman filter with custom mean, residual, and addition functions. More...

const StateMatrix &	S () const
	Returns the square-root error covariance matrix S. More...

double	S (int i, int j) const
	Returns an element of the square-root error covariance matrix S. More...

void	SetS (const StateMatrix &S)
	Set the current square-root error covariance matrix S. More...

StateMatrix	P () const
	Returns the reconstructed error covariance matrix P. More...

void	SetP (const StateMatrix &P)
	Set the current square-root error covariance matrix S by taking the square root of P. More...

const StateVector &	Xhat () const
	Returns the state estimate x-hat. More...

double	Xhat (int i) const
	Returns an element of the state estimate x-hat. More...

void	SetXhat (const StateVector &xHat)
	Set initial state estimate x-hat. More...

void	SetXhat (int i, double value)
	Set an element of the initial state estimate x-hat. More...

void	Reset ()
	Resets the observer. More...

void	Predict (const InputVector &u, units::second_t dt)
	Project the model into the future with a new control input u. More...

void	Correct (const InputVector &u, const OutputVector &y)
	Correct the state estimate x-hat using the measurements in y. More...

void	Correct (const InputVector &u, const OutputVector &y, const Matrixd< Outputs, Outputs > &R)
	Correct the state estimate x-hat using the measurements in y. More...

template<int Rows>
void	Correct (const InputVector &u, const Vectord< Rows > &y, std::function< Vectord< Rows >(const StateVector &, const InputVector &)> h, const Matrixd< Rows, Rows > &R)
	Correct the state estimate x-hat using the measurements in y. More...

template<int Rows>
void	Correct (const InputVector &u, const Vectord< Rows > &y, std::function< Vectord< Rows >(const StateVector &, const InputVector &)> h, const Matrixd< Rows, Rows > &R, std::function< Vectord< Rows >(const Matrixd< Rows, 2 States+1 > &, const Vectord< 2 States+1 > &)> meanFuncY, std::function< Vectord< Rows >(const Vectord< Rows > &, const Vectord< Rows > &)> residualFuncY, std::function< StateVector(const StateVector &, const StateVector &)> residualFuncX, std::function< StateVector(const StateVector &, const StateVector &)> addFuncX)
	Correct the state estimate x-hat using the measurements in y. More...

Detailed Description

template<int States, int Inputs, int Outputs>
class frc::UnscentedKalmanFilter< States, Inputs, Outputs >

A Kalman filter combines predictions from a model and measurements to give an estimate of the true system state.

This is useful because many states cannot be measured directly as a result of sensor noise, or because the state is "hidden".

Kalman filters use a K gain matrix to determine whether to trust the model or measurements more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum of squares error in the state estimate. This K gain is used to correct the state estimate by some amount of the difference between the actual measurements and the measurements predicted by the model.

An unscented Kalman filter uses nonlinear state and measurement models. It propagates the error covariance using sigma points chosen to approximate the true probability distribution.

For more on the underlying math, read https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control theory".

This class implements a square-root-form unscented Kalman filter (SR-UKF). For more information about the SR-UKF, see https://www.researchgate.net/publication/3908304.

Template Parameters

States	Number of states.
Inputs	Number of inputs.
Outputs	Number of outputs.

Member Typedef Documentation

◆ InputVector

template<int States, int Inputs, int Outputs>

using frc::UnscentedKalmanFilter< States, Inputs, Outputs >::InputVector = Vectord<Inputs>

◆ OutputArray

template<int States, int Inputs, int Outputs>

using frc::UnscentedKalmanFilter< States, Inputs, Outputs >::OutputArray = wpi::array<double, Outputs>

◆ OutputVector

template<int States, int Inputs, int Outputs>

using frc::UnscentedKalmanFilter< States, Inputs, Outputs >::OutputVector = Vectord<Outputs>

◆ StateArray

template<int States, int Inputs, int Outputs>

using frc::UnscentedKalmanFilter< States, Inputs, Outputs >::StateArray = wpi::array<double, States>

◆ StateMatrix

template<int States, int Inputs, int Outputs>

using frc::UnscentedKalmanFilter< States, Inputs, Outputs >::StateMatrix = Matrixd<States, States>

◆ StateVector

template<int States, int Inputs, int Outputs>

using frc::UnscentedKalmanFilter< States, Inputs, Outputs >::StateVector = Vectord<States>

Constructor & Destructor Documentation

◆ UnscentedKalmanFilter() [1/2]

template<int States, int Inputs, int Outputs>

frc::UnscentedKalmanFilter< States, Inputs, Outputs >::UnscentedKalmanFilter	(	std::function< StateVector(const StateVector &, const InputVector &)>	f,
		std::function< OutputVector(const StateVector &, const InputVector &)>	h,
		const StateArray &	stateStdDevs,
		const OutputArray &	measurementStdDevs,
		units::second_t	dt
	)

Constructs an unscented Kalman filter.

See https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices for how to select the standard deviations.

Parameters

f	A vector-valued function of x and u that returns the derivative of the state vector.
h	A vector-valued function of x and u that returns the measurement vector.
stateStdDevs	Standard deviations of model states.
measurementStdDevs	Standard deviations of measurements.
dt	Nominal discretization timestep.

◆ UnscentedKalmanFilter() [2/2]

template<int States, int Inputs, int Outputs>

frc::UnscentedKalmanFilter< States, Inputs, Outputs >::UnscentedKalmanFilter	(	std::function< StateVector(const StateVector &, const InputVector &)>	f,
		std::function< OutputVector(const StateVector &, const InputVector &)>	h,
		const StateArray &	stateStdDevs,
		const OutputArray &	measurementStdDevs,
		std::function< StateVector(const Matrixd< States, 2 States+1 > &, const Vectord< 2 States+1 > &)>	meanFuncX,
		std::function< OutputVector(const Matrixd< Outputs, 2 States+1 > &, const Vectord< 2 States+1 > &)>	meanFuncY,
		std::function< StateVector(const StateVector &, const StateVector &)>	residualFuncX,
		std::function< OutputVector(const OutputVector &, const OutputVector &)>	residualFuncY,
		std::function< StateVector(const StateVector &, const StateVector &)>	addFuncX,
		units::second_t	dt
	)

Constructs an unscented Kalman filter with custom mean, residual, and addition functions.

Using custom functions for arithmetic can be useful if you have angles in the state or measurements, because they allow you to correctly account for the modular nature of angle arithmetic.

See https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-observers.html#process-and-measurement-noise-covariance-matrices for how to select the standard deviations.

Parameters

f	A vector-valued function of x and u that returns the derivative of the state vector.
h	A vector-valued function of x and u that returns the measurement vector.
stateStdDevs	Standard deviations of model states.
measurementStdDevs	Standard deviations of measurements.
meanFuncX	A function that computes the mean of 2 * States + 1 state vectors using a given set of weights.
meanFuncY	A function that computes the mean of 2 * States + 1 measurement vectors using a given set of weights.
residualFuncX	A function that computes the residual of two state vectors (i.e. it subtracts them.)
residualFuncY	A function that computes the residual of two measurement vectors (i.e. it subtracts them.)
addFuncX	A function that adds two state vectors.
dt	Nominal discretization timestep.

Member Function Documentation

◆ Correct() [1/4]

template<int States, int Inputs, int Outputs>

void frc::UnscentedKalmanFilter< States, Inputs, Outputs >::Correct	(	const InputVector &	u,
		const OutputVector &	y
	)

inline

Correct the state estimate x-hat using the measurements in y.

Parameters

u	Same control input used in the predict step.
y	Measurement vector.

◆ Correct() [2/4]

template<int States, int Inputs, int Outputs>

void frc::UnscentedKalmanFilter< States, Inputs, Outputs >::Correct	(	const InputVector &	u,
		const OutputVector &	y,
		const Matrixd< Outputs, Outputs > &	R
	)

inline

Correct the state estimate x-hat using the measurements in y.

This is useful for when the measurement noise covariances vary.

Parameters

u	Same control input used in the predict step.
y	Measurement vector.
R	Continuous measurement noise covariance matrix.

◆ Correct() [3/4]

template<int States, int Inputs, int Outputs>

template<int Rows>

void frc::UnscentedKalmanFilter< States, Inputs, Outputs >::Correct	(	const InputVector &	u,
		const Vectord< Rows > &	y,
		std::function< Vectord< Rows >(const StateVector &, const InputVector &)>	h,
		const Matrixd< Rows, Rows > &	R
	)

Correct the state estimate x-hat using the measurements in y.

This is useful for when the measurements available during a timestep's Correct() call vary. The h(x, u) passed to the constructor is used if one is not provided (the two-argument version of this function).

Parameters

u	Same control input used in the predict step.
y	Measurement vector.
h	A vector-valued function of x and u that returns the measurement vector.
R	Continuous measurement noise covariance matrix.

◆ Correct() [4/4]

template<int States, int Inputs, int Outputs>

template<int Rows>

void frc::UnscentedKalmanFilter< States, Inputs, Outputs >::Correct	(	const InputVector &	u,
		const Vectord< Rows > &	y,
		std::function< Vectord< Rows >(const StateVector &, const InputVector &)>	h,
		const Matrixd< Rows, Rows > &	R,
		std::function< Vectord< Rows >(const Matrixd< Rows, 2 States+1 > &, const Vectord< 2 States+1 > &)>	meanFuncY,
		std::function< Vectord< Rows >(const Vectord< Rows > &, const Vectord< Rows > &)>	residualFuncY,
		std::function< StateVector(const StateVector &, const StateVector &)>	residualFuncX,
		std::function< StateVector(const StateVector &, const StateVector &)>	addFuncX
	)

Correct the state estimate x-hat using the measurements in y.

This is useful for when the measurements available during a timestep's Correct() call vary. The h(x, u) passed to the constructor is used if one is not provided (the two-argument version of this function).

Parameters

u	Same control input used in the predict step.
y	Measurement vector.
h	A vector-valued function of x and u that returns the measurement vector.
R	Continuous measurement noise covariance matrix.
meanFuncY	A function that computes the mean of 2 * States + 1 measurement vectors using a given set of weights.
residualFuncY	A function that computes the residual of two measurement vectors (i.e. it subtracts them.)
residualFuncX	A function that computes the residual of two state vectors (i.e. it subtracts them.)
addFuncX	A function that adds two state vectors.

◆ P()

template<int States, int Inputs, int Outputs>

StateMatrix frc::UnscentedKalmanFilter< States, Inputs, Outputs >::P ( ) const

inline

Returns the reconstructed error covariance matrix P.

◆ Predict()

template<int States, int Inputs, int Outputs>

void frc::UnscentedKalmanFilter< States, Inputs, Outputs >::Predict	(	const InputVector &	u,
		units::second_t	dt
	)

Project the model into the future with a new control input u.

Parameters

u	New control input from controller.
dt	Timestep for prediction.

◆ Reset()

template<int States, int Inputs, int Outputs>

void frc::UnscentedKalmanFilter< States, Inputs, Outputs >::Reset ( )

inline

Resets the observer.

◆ S() [1/2]

template<int States, int Inputs, int Outputs>

const StateMatrix & frc::UnscentedKalmanFilter< States, Inputs, Outputs >::S ( ) const

inline

Returns the square-root error covariance matrix S.

◆ S() [2/2]

template<int States, int Inputs, int Outputs>

double frc::UnscentedKalmanFilter< States, Inputs, Outputs >::S	(	int	i,
		int	j
	)		const

inline

Returns an element of the square-root error covariance matrix S.

Parameters

i	Row of S.
j	Column of S.

◆ SetP()

template<int States, int Inputs, int Outputs>

void frc::UnscentedKalmanFilter< States, Inputs, Outputs >::SetP ( const StateMatrix & P )

inline

Set the current square-root error covariance matrix S by taking the square root of P.

Parameters

P	The error covariance matrix P.

◆ SetS()

template<int States, int Inputs, int Outputs>

void frc::UnscentedKalmanFilter< States, Inputs, Outputs >::SetS ( const StateMatrix & S )

inline

Set the current square-root error covariance matrix S.

Parameters

S	The square-root error covariance matrix S.

◆ SetXhat() [1/2]

template<int States, int Inputs, int Outputs>

void frc::UnscentedKalmanFilter< States, Inputs, Outputs >::SetXhat ( const StateVector & xHat )

inline

Set initial state estimate x-hat.

Parameters

xHat	The state estimate x-hat.

◆ SetXhat() [2/2]

template<int States, int Inputs, int Outputs>

void frc::UnscentedKalmanFilter< States, Inputs, Outputs >::SetXhat	(	int	i,
		double	value
	)

inline

Set an element of the initial state estimate x-hat.

Parameters

i	Row of x-hat.
value	Value for element of x-hat.

◆ Xhat() [1/2]

template<int States, int Inputs, int Outputs>

const StateVector & frc::UnscentedKalmanFilter< States, Inputs, Outputs >::Xhat ( ) const

inline

Returns the state estimate x-hat.

◆ Xhat() [2/2]

template<int States, int Inputs, int Outputs>

double frc::UnscentedKalmanFilter< States, Inputs, Outputs >::Xhat ( int i ) const

inline

Returns an element of the state estimate x-hat.

Parameters

i	Row of x-hat.

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

frc/estimator/UnscentedKalmanFilter.h
frc/estimator/UnscentedKalmanFilter.inc

Public Types

Public Member Functions

Detailed Description

Member Typedef Documentation

◆ InputVector

◆ OutputArray

◆ OutputVector

◆ StateArray

◆ StateMatrix

◆ StateVector

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]

◆ SetP()

◆ SetS()

◆ SetXhat() [1/2]

◆ SetXhat() [2/2]

◆ Xhat() [1/2]

◆ Xhat() [2/2]