40template <
int States,
int Inputs,
int Outputs>
112 double P(
int i,
int j)
const {
return m_P(i, j); }
131 double Xhat(
int i)
const {
return m_xHat(i); }
146 void SetXhat(
int i,
double value) { m_xHat(i) = value; }
171 Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
185 Correct<Outputs>(u, y, m_h,
R, m_residualFuncY, m_addFuncX);
243 units::second_t m_dt;
A Kalman filter combines predictions from a model and measurements to give an estimate of the true sy...
Definition: ExtendedKalmanFilter.h:41
void Correct(const InputVector &u, const OutputVector &y)
Correct the state estimate x-hat using the measurements in y.
Definition: ExtendedKalmanFilter.h:170
const StateMatrix & P() const
Returns the error covariance matrix P.
Definition: ExtendedKalmanFilter.h:104
ExtendedKalmanFilter(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 extended Kalman filter.
Definition: ExtendedKalmanFilter.inc:21
Vectord< Inputs > InputVector
Definition: ExtendedKalmanFilter.h:44
void Reset()
Resets the observer.
Definition: ExtendedKalmanFilter.h:151
void Predict(const InputVector &u, units::second_t dt)
Project the model into the future with a new control input u.
Definition: ExtendedKalmanFilter.inc:92
Matrixd< States, States > StateMatrix
Definition: ExtendedKalmanFilter.h:50
Vectord< Outputs > OutputVector
Definition: ExtendedKalmanFilter.h:45
void SetP(const StateMatrix &P)
Set the current error covariance matrix P.
Definition: ExtendedKalmanFilter.h:119
void SetXhat(int i, double value)
Set an element of the initial state estimate x-hat.
Definition: ExtendedKalmanFilter.h:146
const StateVector & Xhat() const
Returns the state estimate x-hat.
Definition: ExtendedKalmanFilter.h:124
Vectord< States > StateVector
Definition: ExtendedKalmanFilter.h:43
void SetXhat(const StateVector &xHat)
Set initial state estimate x-hat.
Definition: ExtendedKalmanFilter.h:138
double P(int i, int j) const
Returns an element of the error covariance matrix P.
Definition: ExtendedKalmanFilter.h:112
void Correct(const InputVector &u, const OutputVector &y, const Matrixd< Outputs, Outputs > &R)
Correct the state estimate x-hat using the measurements in y.
Definition: ExtendedKalmanFilter.h:183
double Xhat(int i) const
Returns an element of the state estimate x-hat.
Definition: ExtendedKalmanFilter.h:131
This class is a wrapper around std::array that does compile time size checking.
Definition: array.h:26
Definition: AprilTagPoseEstimator.h:15
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition: EigenCore.h:21
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
static constexpr const unit_t< compound_unit< energy::joules, inverse< temperature::kelvin >, inverse< substance::moles > > > R(8.3144598)
Gas constant.
static constexpr const unit_t< compound_unit< energy::joule, time::seconds > > h(6.626070040e-34)
Planck constant.