Estimators

Detailed Description

Implementations of estimators that operate as Systems in a block diagram.

Algorithms that synthesize controllers are located in State Estimation.

Classes
class	LuenbergerObserver< T >
	A simple state observer for a dynamical system of the form: \[\dot{x} = f(x,u) \] \[y = g(x,u) \] the observer dynamics takes the form \[\dot{\hat{x}} = f(\hat{x},u) + L(y - g(\hat{x},u)) \] where \(\hat{x}\) is the estimated state of the original system. More...

Functions
Eigen::MatrixXd	SteadyStateKalmanFilter (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &C, const Eigen::Ref< const Eigen::MatrixXd > &W, const Eigen::Ref< const Eigen::MatrixXd > &V)
	Computes the optimal observer gain, L, for the linear system defined by \[ \dot{x} = Ax + Bu + w, \] \[ y = Cx + Du + v. \] The resulting observer is of the form \[ \dot{\hat{x}} = A\hat{x} + Bu + L(y - C\hat{x} - Du). \] The process noise, w, and the measurement noise, v, are assumed to be iid mean-zero Gaussian. More...
std::unique_ptr< LuenbergerObserver< double > >	SteadyStateKalmanFilter (std::shared_ptr< const LinearSystem< double >> system, const Eigen::Ref< const Eigen::MatrixXd > &W, const Eigen::Ref< const Eigen::MatrixXd > &V)
	Creates a Luenberger observer system using the optimal steady-state Kalman filter gain matrix, L, as described above. More...
std::unique_ptr< LuenbergerObserver< double > >	SteadyStateKalmanFilter (std::shared_ptr< const System< double >> system, const Context< double > &context, const Eigen::Ref< const Eigen::MatrixXd > &W, const Eigen::Ref< const Eigen::MatrixXd > &V)
	Creates a Luenberger observer system using the steady-state Kalman filter observer gain. More...
Eigen::MatrixXd	ObservabilityMatrix (const LinearSystem< double > &sys)
	Returns the observability matrix: O = [ C; CA; ...; CA^{n-1} ]. More...
bool	IsObservable (const LinearSystem< double > &sys, std::optional< double > threshold=std::nullopt)
	Returns true iff the observability matrix is full column rank. More...
bool	IsDetectable (const LinearSystem< double > &sys, std::optional< double > threshold=std::nullopt)
	Returns true iff the system is detectable. More...

Function Documentation

IsDetectable()

bool drake::systems::IsDetectable	(	const LinearSystem< double > &	sys,
		std::optional< double >	threshold = std::nullopt
	)

Returns true iff the system is detectable.

IsObservable()

bool drake::systems::IsObservable	(	const LinearSystem< double > &	sys,
		std::optional< double >	threshold = std::nullopt
	)

Returns true iff the observability matrix is full column rank.

ObservabilityMatrix()

Eigen::MatrixXd drake::systems::ObservabilityMatrix

(

const LinearSystem< double > &

sys

)

Returns the observability matrix: O = [ C; CA; ...; CA^{n-1} ].

SteadyStateKalmanFilter() [1/3]

Eigen::MatrixXd drake::systems::estimators::SteadyStateKalmanFilter	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::MatrixXd > &	C,
		const Eigen::Ref< const Eigen::MatrixXd > &	W,
		const Eigen::Ref< const Eigen::MatrixXd > &	V
	)

Computes the optimal observer gain, L, for the linear system defined by

\[ \dot{x} = Ax + Bu + w, \]

\[ y = Cx + Du + v. \]

The resulting observer is of the form

\[ \dot{\hat{x}} = A\hat{x} + Bu + L(y - C\hat{x} - Du). \]

The process noise, w, and the measurement noise, v, are assumed to be iid mean-zero Gaussian.

This is a simplified form of the full Kalman filter obtained by assuming that the state-covariance matrix has already converged to its steady-state solution.

Parameters

A	The state-space dynamics matrix of size num_states x num_states.
C	The state-space output matrix of size num_outputs x num_states.
W	The process noise covariance matrix, E[ww'], of size num_states x num_states.
V	The measurement noise covariance matrix, E[vv'], of size num_.

Returns

The steady-state observer gain matrix of size num_states x num_outputs.

Exceptions

std::exception

if V is not positive definite.

SteadyStateKalmanFilter() [2/3]

std::unique_ptr<LuenbergerObserver<double> > drake::systems::estimators::SteadyStateKalmanFilter	(	std::shared_ptr< const LinearSystem< double >>	system,
		const Eigen::Ref< const Eigen::MatrixXd > &	W,
		const Eigen::Ref< const Eigen::MatrixXd > &	V
	)

Creates a Luenberger observer system using the optimal steady-state Kalman filter gain matrix, L, as described above.

Parameters

system	The LinearSystem describing the system to be observed.
W	The process noise covariance matrix, E[ww'], of size num_states x num_states.
V	The measurement noise covariance matrix, E[vv'], of size num_.

Returns

The constructed observer system.

Exceptions

std::exception

if V is not positive definite.

SteadyStateKalmanFilter() [3/3]

std::unique_ptr<LuenbergerObserver<double> > drake::systems::estimators::SteadyStateKalmanFilter	(	std::shared_ptr< const System< double >>	system,
		const Context< double > &	context,
		const Eigen::Ref< const Eigen::MatrixXd > &	W,
		const Eigen::Ref< const Eigen::MatrixXd > &	V
	)

Creates a Luenberger observer system using the steady-state Kalman filter observer gain.

Assuming system has the (continuous-time) dynamics: dx/dt = f(x,u), and the output: y = g(x,u), then the resulting observer will have the form dx̂/dt = f(x̂,u) + L(y - g(x̂,u)), where x̂ is the estimated state and the gain matrix, L, is designed as a steady-state Kalman filter using a linearization of f(x,u) at context as described above.

Parameters

system	The System describing the system to be observed.
context	The context describing a fixed-point of the system (plus any additional parameters).
W	The process noise covariance matrix, E[ww'], of size num_states x num_states.
V	The measurement noise covariance matrix, E[vv'], of size num_.

Returns

The constructed observer system.

Exceptions

std::exception

if V is not positive definite.