Loading...
Searching...
No Matches
KalmanFilter.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/linear/GaussianDensity.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#ifndef KALMANFILTER_DEFAULT_FACTORIZATION
#define KALMANFILTER_DEFAULT_FACTORIZATION QR
#endif
namespace gtsam {
class GTSAM_EXPORT KalmanFilter {
public:
enum Factorization {
QR, CHOLESKY
};
typedef GaussianDensity::shared_ptr State;
private:
const size_t n_;
const Matrix I_;
const GaussianFactorGraph::Eliminate function_;
State solve(const GaussianFactorGraph& factorGraph) const;
State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
public:
// Constructor
KalmanFilter(size_t n, Factorization method =
KALMANFILTER_DEFAULT_FACTORIZATION) :
n_(n), I_(Matrix::Identity(n_, n_)), function_(
method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
GaussianFactorGraph::Eliminate(EliminateCholesky)) {
}
State init(const Vector& x0, const SharedDiagonal& P0) const;
State init(const Vector& x0, const Matrix& P0) const;
void print(const std::string& s = "") const;
static Key step(const State& p) {
return p->firstFrontalKey();
}
State predict(const State& p, const Matrix& F, const Matrix& B,
const Vector& u, const SharedDiagonal& modelQ) const;
/*
* Version of predict with full covariance
* Q is normally derived as G*w*G^T where w models uncertainty of some
* physical property, such as velocity or acceleration, and G is derived from physics.
* This version allows more realistic models than a diagonal covariance matrix.
*/
State predictQ(const State& p, const Matrix& F, const Matrix& B,
const Vector& u, const Matrix& Q) const;
State predict2(const State& p, const Matrix& A0, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) const;
State update(const State& p, const Matrix& H, const Vector& z,
const SharedDiagonal& model) const;
/*
* Version of update with full covariance
* Q is normally derived as G*w*G^T where w models uncertainty of some
* physical property, such as velocity or acceleration, and G is derived from physics.
* This version allows more realistic models than a diagonal covariance matrix.
*/
State updateQ(const State& p, const Matrix& H, const Vector& z,
const Matrix& Q) const;
};
} // \namespace gtsam
/* ************************************************************************* */
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
NoiseModel.h
GaussianDensity.h
A Gaussian Density.
gtsam::EliminateCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:525
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::EliminateableFactorGraph< GaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
Definition EliminateableFactorGraph.h:89
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
gtsam::KalmanFilter::Factorization
Factorization
This Kalman filter is a Square-root Information filter The type below allows you to specify the facto...
Definition KalmanFilter.h:49
gtsam::KalmanFilter::step
static Key step(const State &p)
Return step index k, starts at 0, incremented at each predict.
Definition KalmanFilter.h:92
gtsam::KalmanFilter::State
GaussianDensity::shared_ptr State
The Kalman filter state is simply a GaussianDensity.
Definition KalmanFilter.h:56