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 
gtsam::KalmanFilter::step
static Key step(const State &p)
Definition: KalmanFilter.h:92
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_
Definition: KalmanFilter.h:60
gtsam::KalmanFilter::I_
const Matrix I_
Definition: KalmanFilter.h:61
gtsam::internal::DoglegState
Definition: DoglegOptimizer.cpp:54
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:38
A0
static const double A0[]
Definition: expn.h:5
gtsam::KalmanFilter::QR
@ QR
Definition: KalmanFilter.h:50
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:155
gtsam::KalmanFilter::KalmanFilter
KalmanFilter(size_t n, Factorization method=KALMANFILTER_DEFAULT_FACTORIZATION)
Definition: KalmanFilter.h:70
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
Definition: KalmanFilter.h:49
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::KalmanFilter::State
GaussianDensity::shared_ptr State
Definition: KalmanFilter.h:56
gtsam
traits
Definition: SFMdata.h:40
NoiseModel.h
gtsam::KalmanFilter
Definition: KalmanFilter.h:41
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:517
gtsam::KalmanFilter::function_
const GaussianFactorGraph::Eliminate function_
Definition: KalmanFilter.h:62
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
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:00