KalmanFilter.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #pragma once
21 
25 
26 #ifndef KALMANFILTER_DEFAULT_FACTORIZATION
27 #define KALMANFILTER_DEFAULT_FACTORIZATION QR
28 #endif
29 
30 namespace gtsam {
31 
42 class GTSAM_EXPORT KalmanFilter {
43  public:
48  enum Factorization { QR, CHOLESKY };
49 
56 
57  private:
58  const size_t n_;
59  const Matrix I_;
62 
68  State solve(const GaussianFactorGraph& factorGraph) const;
69 
76  State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
77 
78  public:
84  KalmanFilter(size_t n,
86  : n_(n),
87  I_(Matrix::Identity(n_, n_)),
88  function_(method == QR
89  ? GaussianFactorGraph::Eliminate(EliminateQR)
90  : GaussianFactorGraph::Eliminate(EliminateCholesky)) {}
91 
104  State init(const Vector& x0, const SharedDiagonal& P0) const;
105 
112  State init(const Vector& x0, const Matrix& P0) const;
113 
118  void print(const std::string& s = "") const;
119 
126  static Key step(const State& p) { return p->firstFrontalKey(); }
127 
148  State predict(const State& p, const Matrix& F, const Matrix& B,
149  const Vector& u, const SharedDiagonal& modelQ) const;
150 
165  State predictQ(const State& p, const Matrix& F, const Matrix& B,
166  const Vector& u, const Matrix& Q) const;
167 
177  State predict2(const State& p, const Matrix& A0, const Matrix& A1,
178  const Vector& b, const SharedDiagonal& model = nullptr) const;
179 
196  State update(const State& p, const Matrix& H, const Vector& z,
197  const SharedDiagonal& model) const;
198 
207  State updateQ(const State& p, const Matrix& H, const Vector& z,
208  const Matrix& R) const;
209 
214  size_t dim() const { return n_; }
215 };
216 
217 } // namespace gtsam
gtsam::KalmanFilter::step
static Key step(const State &p)
Definition: KalmanFilter.h:126
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
relicense.update
def update(text)
Definition: relicense.py:46
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::KalmanFilter::n_
const size_t n_
Dimensionality of the state.
Definition: KalmanFilter.h:58
gtsam::KalmanFilter::I_
const Matrix I_
Identity matrix of size .
Definition: KalmanFilter.h:59
gtsam::internal::DoglegState
Definition: DoglegOptimizer.cpp:54
gtsam::KalmanFilter::State
GaussianDensity::shared_ptr State
The Kalman filter state, represented as a shared pointer to a GaussianDensity.
Definition: KalmanFilter.h:55
B
Definition: test_numpy_dtypes.cpp:299
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
A0
static const double A0[]
Definition: expn.h:5
gtsam::KalmanFilter::QR
@ QR
Definition: KalmanFilter.h:48
gtsam::KalmanFilter::dim
size_t dim() const
Definition: KalmanFilter.h:214
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:156
gtsam::KalmanFilter::KalmanFilter
KalmanFilter(size_t n, Factorization method=KALMANFILTER_DEFAULT_FACTORIZATION)
Definition: KalmanFilter.h:84
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
x0
static Symbol x0('x', 0)
GaussianDensity.h
A Gaussian Density.
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::KalmanFilter::Factorization
Factorization
Specifies the factorization variant to use.
Definition: KalmanFilter.h:48
init
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:2006
gtsam::symbol_shorthand::F
Key F(std::uint64_t j)
Definition: inference/Symbol.h:153
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::b
const G & b
Definition: Group.h:79
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
State
Definition: testKalmanFilter.cpp:31
gtsam
traits
Definition: SFMdata.h:40
NoiseModel.h
gtsam::KalmanFilter
Definition: KalmanFilter.h:42
p
float * p
Definition: Tutorial_Map_using.cpp:9
A1
static const double A1[]
Definition: expn.h:6
gtsam::EliminateableFactorGraph< GaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition: EliminateableFactorGraph.h:88
KALMANFILTER_DEFAULT_FACTORIZATION
#define KALMANFILTER_DEFAULT_FACTORIZATION
Definition: KalmanFilter.h:27
gtsam::EliminateCholesky
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: HessianFactor.cpp:518
gtsam::KalmanFilter::function_
const GaussianFactorGraph::Eliminate function_
Elimination algorithm used.
Definition: KalmanFilter.h:61
P0
static double P0[5]
Definition: ndtri.c:59
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::GaussianDensity::shared_ptr
std::shared_ptr< GaussianDensity > shared_ptr
Definition: GaussianDensity.h:36
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:779


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:51