Drake
Drake C++ Documentation
kalman_filter.h File Reference
#include <memory>
#include <Eigen/Dense>
#include "drake/systems/estimators/luenberger_observer.h"
#include "drake/systems/primitives/linear_system.h"
Include dependency graph for kalman_filter.h:

Namespaces

 drake
 
 drake::systems
 
 drake::systems::estimators
 

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::unique_ptr< 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::unique_ptr< System< double >> system, std::unique_ptr< 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...