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 
41 class GTSAM_EXPORT KalmanFilter {
42 
43 public:
44 
50  QR, CHOLESKY
51  };
52 
57 
58 private:
59 
60  const size_t n_;
61  const Matrix I_;
64  State solve(const GaussianFactorGraph& factorGraph) const;
65  State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
66 
67 public:
68 
69  // Constructor
70  KalmanFilter(size_t n, Factorization method =
72  n_(n), I_(Matrix::Identity(n_, n_)), function_(
73  method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
75  }
76 
83  State init(const Vector& x0, const SharedDiagonal& P0) const;
84 
86  State init(const Vector& x0, const Matrix& P0) const;
87 
89  void print(const std::string& s = "") const;
90 
92  static Key step(const State& p) {
93  return p->firstFrontalKey();
94  }
95 
104  State predict(const State& p, const Matrix& F, const Matrix& B,
105  const Vector& u, const SharedDiagonal& modelQ) const;
106 
107  /*
108  * Version of predict with full covariance
109  * Q is normally derived as G*w*G^T where w models uncertainty of some
110  * physical property, such as velocity or acceleration, and G is derived from physics.
111  * This version allows more realistic models than a diagonal covariance matrix.
112  */
113  State predictQ(const State& p, const Matrix& F, const Matrix& B,
114  const Vector& u, const Matrix& Q) const;
115 
124  State predict2(const State& p, const Matrix& A0, const Matrix& A1,
125  const Vector& b, const SharedDiagonal& model) const;
126 
135  State update(const State& p, const Matrix& H, const Vector& z,
136  const SharedDiagonal& model) const;
137 
138  /*
139  * Version of update with full covariance
140  * Q is normally derived as G*w*G^T where w models uncertainty of some
141  * physical property, such as velocity or acceleration, and G is derived from physics.
142  * This version allows more realistic models than a diagonal covariance matrix.
143  */
144  State updateQ(const State& p, const Matrix& H, const Vector& z,
145  const Matrix& Q) const;
146 };
147 
148 } // \namespace gtsam
149 
150 /* ************************************************************************* */
151 
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
static Matrix A1
Key F(std::uint64_t j)
def update(text)
Definition: relicense.py:46
noiseModel::Diagonal::shared_ptr model
GaussianDensity::shared_ptr State
Definition: KalmanFilter.h:56
int n
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
const GaussianFactorGraph::Eliminate function_
Definition: KalmanFilter.h:62
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
#define KALMANFILTER_DEFAULT_FACTORIZATION
Definition: KalmanFilter.h:27
A Gaussian Density.
Eigen::VectorXd Vector
Definition: Vector.h:38
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
KalmanFilter(size_t n, Factorization method=KALMANFILTER_DEFAULT_FACTORIZATION)
Definition: KalmanFilter.h:70
RealScalar s
Linear Factor Graph where all factors are Gaussians.
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
const G & b
Definition: Group.h:86
std::shared_ptr< This > shared_ptr
shared_ptr to this class
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
static Symbol x0('x', 0)
The quaternion class used to represent 3D orientations and rotations.
std::shared_ptr< GaussianDensity > shared_ptr
const size_t n_
Definition: KalmanFilter.h:60
const Matrix I_
Definition: KalmanFilter.h:61
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1882
float * p
const Point3 P0(0, 0, 0)
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
static Key step(const State &p)
Definition: KalmanFilter.h:92


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:29