Implementation of the Degenerate measurement Linear Kalman filter. More...

#include <dkf.h>

Inheritance diagram for mrs_lib::DKF< n_states, n_inputs >:

Collaboration diagram for mrs_lib::DKF< n_states, n_inputs >:

Public Types
using	Base_class = LKF< n_states, n_inputs, -1 >
	Base class of this class.

using	x_t = typename Base_class::x_t
	State vector type $n \times 1$ .

using	u_t = typename Base_class::u_t
	Input vector type $m \times 1$ .

using	z_t = typename Base_class::z_t
	Measurement vector type $p \times 1$ .

using	P_t = typename Base_class::P_t
	State uncertainty covariance matrix type $n \times n$ .

using	R_t = typename Base_class::R_t
	Measurement noise covariance matrix type $p \times p$ .

using	Q_t = typename Base_class::Q_t
	Process noise covariance matrix type $n \times n$ .

using	statecov_t = typename Base_class::statecov_t
	Helper struct for passing around the state and its covariance in one variable.

typedef Eigen::Matrix< double, n, n >	A_t
	System transition matrix type $n \times n$ .

typedef Eigen::Matrix< double, n, m >	B_t
	Input to state mapping matrix type $n \times m$ .

typedef Eigen::Matrix< double, p, n >	H_t
	State to measurement mapping matrix type $p \times n$ .

typedef Eigen::Matrix< double, n, p >	K_t
	Kalman gain matrix type $n \times p$ .

using	mat2_t = Eigen::Matrix< double, 2, 2 >

using	mat3_t = Eigen::Matrix< double, 3, 3 >

using	pt3_t = mrs_lib::geometry::vec3_t

using	pt2_t = mrs_lib::geometry::vec2_t

using	vec3_t = mrs_lib::geometry::vec3_t

Public Types inherited from mrs_lib::LKF< n_states, n_inputs, -1 >
using	Base_class = KalmanFilter< n, m, p >
	Base class of this class.

using	x_t = typename Base_class::x_t
	State vector type $n \times 1$ .

using	u_t = typename Base_class::u_t
	Input vector type $m \times 1$ .

using	z_t = typename Base_class::z_t
	Measurement vector type $p \times 1$ .

using	P_t = typename Base_class::P_t
	State uncertainty covariance matrix type $n \times n$ .

using	R_t = typename Base_class::R_t
	Measurement noise covariance matrix type $p \times p$ .

using	Q_t = typename Base_class::Q_t
	Process noise covariance matrix type $n \times n$ .

using	statecov_t = typename Base_class::statecov_t
	Helper struct for passing around the state and its covariance in one variable.

typedef Eigen::Matrix< double, n, n >	A_t
	System transition matrix type $n \times n$ .

typedef Eigen::Matrix< double, n, m >	B_t
	Input to state mapping matrix type $n \times m$ .

typedef Eigen::Matrix< double, p, n >	H_t
	State to measurement mapping matrix type $p \times n$ .

typedef Eigen::Matrix< double, n, p >	K_t
	Kalman gain matrix type $n \times p$ .

Public Types inherited from mrs_lib::KalmanFilter< n_states, n_inputs, n_measurements >
typedef Eigen::Matrix< double, n, 1 >	x_t
	State vector type $n \times 1$ .

typedef Eigen::Matrix< double, m, 1 >	u_t
	Input vector type $m \times 1$ .

typedef Eigen::Matrix< double, p, 1 >	z_t
	Measurement vector type $p \times 1$ .

typedef Eigen::Matrix< double, n, n >	P_t
	State uncertainty covariance matrix type $n \times n$ .

typedef Eigen::Matrix< double, p, p >	R_t
	Measurement noise covariance matrix type $p \times p$ .

typedef Eigen::Matrix< double, n, n >	Q_t
	Process noise covariance matrix type $n \times n$ .

Public Member Functions
	DKF ()
	Convenience default constructor.

	DKF (const A_t &A, const B_t &B)
	The main constructor.

virtual std::enable_if_t<((n > 3)\|\|(n==Eigen::Dynamic)), statecov_t >	correctLine (const statecov_t &sc, const pt3_t &line_origin, const vec3_t &line_direction, const double line_variance) const
	Applies the correction (update, measurement, data) step of the Kalman filter.

virtual std::enable_if_t<((n > 3)\|\|(n==Eigen::Dynamic)), statecov_t >	correctPlane (const statecov_t &sc, const pt3_t &plane_origin, const vec3_t &plane_normal, const double plane_variance) const
	Applies the correction (update, measurement, data) step of the Kalman filter.

Public Member Functions inherited from mrs_lib::LKF< n_states, n_inputs, -1 >
	LKF ()
	Convenience default constructor.

	LKF (const A_t &A, const B_t &B, const H_t &H)
	The main constructor.

virtual statecov_t	correct (const statecov_t &sc, const z_t &z, const R_t &R) const override
	Applies the correction (update, measurement, data) step of the Kalman filter.

virtual statecov_t	predict (const statecov_t &sc, const u_t &u, const Q_t &Q, double dt) const override
	Applies the prediction (time) step of the Kalman filter.

Static Public Attributes
static constexpr int	n = Base_class::n
	Length of the state vector of the system.

static constexpr int	m = Base_class::m
	Length of the input vector of the system.

static constexpr int	p = Base_class::p
	Length of the measurement vector of the system.

Static Public Attributes inherited from mrs_lib::LKF< n_states, n_inputs, -1 >
static constexpr int	n
	Length of the state vector of the system.

static constexpr int	m
	Length of the input vector of the system.

static constexpr int	p
	Length of the measurement vector of the system.

Static Public Attributes inherited from mrs_lib::KalmanFilter< n_states, n_inputs, n_measurements >
static const int	n = n_states
	Length of the state vector of the system.

static const int	m = n_inputs
	Length of the input vector of the system.

static const int	p = n_measurements
	Length of the measurement vector of the system.

Additional Inherited Members
Public Attributes inherited from mrs_lib::LKF< n_states, n_inputs, -1 >
A_t	A
	The system transition matrix $n \times n$ .

B_t	B
	The input to state mapping matrix $n \times m$ .

H_t	H
	The state to measurement mapping matrix $p \times n$ .

Protected Member Functions inherited from mrs_lib::LKF< n_states, n_inputs, -1 >
virtual K_t	computeKalmanGain (const statecov_t &sc, const z_t &z, const R_t &R, const H_t &H) const

statecov_t ::type	correction_impl (const statecov_t &sc, const z_t &z, const R_t &R, const H_t &H) const

Static Protected Member Functions inherited from mrs_lib::LKF< n_states, n_inputs, -1 >
static P_t	covariance_predict (const A_t &A, const P_t &P, const Q_t &Q, const double dt)

static std::enable_if< check==0, x_t >::type	state_predict (const A_t &A, const x_t &x, const B_t &B, const u_t &u)

static std::enable_if< check!=0, x_t >::type	state_predict (const A_t &A, const x_t &x, const B_t &B, const u_t &u)

static R_t	invert_W (R_t W)

Detailed Description

template<int n_states, int n_inputs>
class mrs_lib::DKF< n_states, n_inputs >

Implementation of the Degenerate measurement Linear Kalman filter.

Template Parameters

n_states	number of states of the system (length of the $\mathbf{x}$ vector).
n_inputs	number of inputs of the system (length of the $\mathbf{u}$ vector).
n_measurements	number of measurements of the system (length of the $\mathbf{z}$ vector).

Constructor & Destructor Documentation

◆ DKF() [1/2]

template<int n_states, int n_inputs>

mrs_lib::DKF< n_states, n_inputs >::DKF ( )

inline

Convenience default constructor.

This constructor should not be used if applicable. If used, the main constructor has to be called afterwards, before using this class, otherwise the LKF object is invalid (not initialized).

◆ DKF() [2/2]

template<int n_states, int n_inputs>

mrs_lib::DKF< n_states, n_inputs >::DKF	(	const A_t &	A,
		const B_t &	B
	)

inline

The main constructor.

Parameters

A	The state transition matrix.
B	The input to state mapping transition matrix.
H	The state to measurement mapping transition matrix.

Member Function Documentation

◆ correctLine()

template<int n_states, int n_inputs>

virtual std::enable_if_t<((n > 3)\|\|(n==Eigen::Dynamic)), statecov_t > mrs_lib::DKF< n_states, n_inputs >::correctLine	(	const statecov_t &	sc,
		const pt3_t &	line_origin,
		const vec3_t &	line_direction,
		const double	line_variance
	)		const

inlinevirtual

Applies the correction (update, measurement, data) step of the Kalman filter.

This method applies the linear Kalman filter correction step to the state and covariance passed in sc using a measurement in the form of a line direcition vector, a point on the line and a perpendicular variance. The updated state and covariance after the correction step is returned.

Parameters

sc	The state and covariance to which the correction step is to be applied.
line_origin	A point lying on the measurement line
line_direction	A vector defining the span of the measurement line
line_variance	Variance defining the uncertainty of the measured state in the direction perpendicular to the measurement line. The uncertainty in the parallel direciton is assumed to be infinite for this case of DKF.

Returns: The state and covariance after the correction update.

◆ correctPlane()

template<int n_states, int n_inputs>

virtual std::enable_if_t<((n > 3)\|\|(n==Eigen::Dynamic)), statecov_t > mrs_lib::DKF< n_states, n_inputs >::correctPlane	(	const statecov_t &	sc,
		const pt3_t &	plane_origin,
		const vec3_t &	plane_normal,
		const double	plane_variance
	)		const

inlinevirtual

Applies the correction (update, measurement, data) step of the Kalman filter.

This method applies the linear Kalman filter correction step to the state and covariance passed in sc using a measurement in the form of a plane normal vector, a point on the plane and a perpendicular variance. The updated state and covariance after the correction step is returned.

Parameters

sc	The state and covariance to which the correction step is to be applied.
plane_origin	A point lying on the measurement plane
plane_normal	The normal vector of the measurement plane
plane_variance	Variance defining the uncertainty of the measured state in the direction perpendicular to the measurement plane. The uncertainty in the span of the plane is assumed to be infinite for this case of DKF.

Returns: The state and covariance after the correction update.

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

include/mrs_lib/dkf.h

Public Types

Public Member Functions

Static Public Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ DKF() [1/2]

◆ DKF() [2/2]

Member Function Documentation

◆ correctLine()

◆ correctPlane()