49template <
typename T,
unsigned int XN,
unsigned int ZN,
unsigned int UN>
78 const Eigen::Matrix<T, XN, XN> &P) {
81 is_initialized_ =
true;
90 const Eigen::Matrix<T, XN, XN> &P)
183 const Eigen::Matrix<T, UN, 1> &u = Eigen::Matrix<T, UN, 1>::Zero());
190 void Correct(
const Eigen::Matrix<T, ZN, 1> &z);
221 Eigen::Matrix<T, XN, 1> x_;
224 Eigen::Matrix<T, XN, XN> P_;
227 Eigen::Matrix<T, XN, XN> F_;
230 Eigen::Matrix<T, XN, XN> Q_;
233 Eigen::Matrix<T, ZN, XN> H_;
236 Eigen::Matrix<T, ZN, ZN> R_;
239 Eigen::Matrix<T, XN, UN> B_;
242 Eigen::Matrix<T, ZN, 1> y_;
245 Eigen::Matrix<T, ZN, ZN> S_;
248 Eigen::Matrix<T, XN, ZN> K_;
251 bool is_initialized_ =
false;
254template <
typename T,
unsigned int XN,
unsigned int ZN,
unsigned int UN>
256 const Eigen::Matrix<T, UN, 1> &u) {
259 x_ = F_ * x_ + B_ * u;
261 P_ = F_ * P_ * F_.transpose() + Q_;
264template <
typename T,
unsigned int XN,
unsigned int ZN,
unsigned int UN>
266 const Eigen::Matrix<T, ZN, 1> &z) {
270 S_ =
static_cast<Eigen::Matrix<T, ZN, ZN>
>(H_ * P_ * H_.transpose() + R_);
272 K_ =
static_cast<Eigen::Matrix<T, XN, ZN>
>(P_ * H_.transpose() *
273 PseudoInverse<T, ZN>(S_));
277 P_ =
static_cast<Eigen::Matrix<T, XN, XN>
>(
278 (Eigen::Matrix<T, XN, XN>::Identity() - K_ * H_) * P_);
281template <
typename T,
unsigned int XN,
unsigned int ZN,
unsigned int UN>
283 Eigen::IOFormat clean_fmt(4, 0,
", ",
" ",
"[",
"]");
284 std::stringstream ss;
285 ss <<
"F = " << F_.format(clean_fmt) <<
"\n"
286 <<
"B = " << B_.format(clean_fmt) <<
"\n"
287 <<
"H = " << H_.format(clean_fmt) <<
"\n"
288 <<
"Q = " << Q_.format(clean_fmt) <<
"\n"
289 <<
"R = " << R_.format(clean_fmt) <<
"\n"
290 <<
"x = " << x_.format(clean_fmt) <<
"\n"
291 <<
"P = " << P_.format(clean_fmt) <<
"\n";
Implements a discrete-time Kalman filter.
KalmanFilter(const Eigen::Matrix< T, XN, 1 > &x, const Eigen::Matrix< T, XN, XN > &P)
Constructor which fully initializes the Kalman filter
std::string DebugString() const
Gets debug string containing detailed information about the filter.
void SetStateCovariance(const Eigen::Matrix< T, XN, XN > &P)
Changes the covariance matrix of current state belief distribution.
const Eigen::Matrix< T, ZN, ZN > & GetObservationNoise() const
Get the covariance matrix of the observation noise.
void SetTransitionMatrix(const Eigen::Matrix< T, XN, XN > &F)
Changes the system transition function under zero control.
void Predict(const Eigen::Matrix< T, UN, 1 > &u=Eigen::Matrix< T, UN, 1 >::Zero())
Updates the state belief distribution given the control input u.
KalmanFilter()
Constructor which defers initialization until the initial state distribution parameters are set (with...
void SetTransitionNoise(const Eigen::Matrix< T, XN, XN > &Q)
Changes the covariance matrix of the transition noise.
Eigen::Matrix< T, XN, 1 > GetStateEstimate() const
Gets mean of our current state belief distribution
bool IsInitialized() const
Get initialization state of the filter
const Eigen::Matrix< T, ZN, XN > & GetObservationMatrix() const
Get the observation matrix, which maps states to observations.
void SetObservationNoise(const Eigen::Matrix< T, ZN, ZN > &R)
Changes the covariance matrix of the observation noise.
void SetStateEstimate(const Eigen::Matrix< T, XN, 1 > &x, const Eigen::Matrix< T, XN, XN > &P)
Sets the initial state belief distribution.
const Eigen::Matrix< T, XN, UN > & GetControlMatrix() const
Get the control matrix in the state transition rule.
void SetControlMatrix(const Eigen::Matrix< T, XN, UN > &B)
Changes the control matrix in the state transition rule.
Eigen::Matrix< T, XN, XN > GetStateCovariance() const
Gets covariance of our current state belief distribution
virtual ~KalmanFilter()
Destructor
void Correct(const Eigen::Matrix< T, ZN, 1 > &z)
Updates the state belief distribution given an observation z.
const Eigen::Matrix< T, XN, XN > & GetTransitionNoise() const
Get the covariance matrix of the transition noise.
const Eigen::Matrix< T, XN, XN > & GetTransitionMatrix() const
Get the system transition function under zero control.
void SetObservationMatrix(const Eigen::Matrix< T, ZN, XN > &H)
Changes the observation matrix, which maps states to observations.
Defines some useful matrix operations.