Drake
Drake C++ Documentation
Loading...
Searching...
No Matches

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 continuous-time dynamical system of the form: More...

Functions

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 in SteadyStateKalmanFilter and DiscreteTimeSteadyStateKalmanFilter.
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.
Eigen::MatrixXd ObservabilityMatrix (const LinearSystem< double > &sys)
 Returns the observability matrix: O = [ C; CA; ...; CA^{n-1} ].
bool IsObservable (const LinearSystem< double > &sys, std::optional< double > threshold=std::nullopt)
 Returns true iff the observability matrix is full column rank.
bool IsDetectable (const LinearSystem< double > &sys, std::optional< double > threshold=std::nullopt)
 Returns true iff the system is detectable.

Function Documentation

◆ IsDetectable()

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

Returns true iff the system is detectable.

◆ IsObservable()

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

Returns true iff the observability matrix is full column rank.

◆ ObservabilityMatrix()

Eigen::MatrixXd ObservabilityMatrix ( const LinearSystem< double > & sys)

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

◆ SteadyStateKalmanFilter() [1/2]

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 in SteadyStateKalmanFilter and DiscreteTimeSteadyStateKalmanFilter.

Parameters
systemThe LinearSystem describing the system to be observed.
WThe process noise covariance matrix, E[ww'], of size num_states x num_states.
VThe measurement noise covariance matrix, E[vv'], of size num_outputs x num_outputs.
Returns
The constructed observer system.
Exceptions
std::exceptionif V is not positive definite.

◆ SteadyStateKalmanFilter() [2/2]

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.

If system has continuous-time dynamics: ẋ = 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.

If system has discrete-time dynamics: x[n+1] = f(x[n],u[n]), and the output: y[n] = g(x[n],u[n]), then the resulting observer will have the form x̂[n+1] = f(x̂[n],u[n]) + L(y − g(x̂[n],u[n])), where x̂[n+1] 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
systemThe System describing the system to be observed.
contextThe context describing a fixed-point of the system (plus any additional parameters).
WThe process noise covariance matrix, E[ww'], of size num_states x num_states.
VThe measurement noise covariance matrix, E[vv'], of size num_outputs x num_outputs.
Returns
The constructed observer system.
Exceptions
std::exceptionif V is not positive definite.