KalmanFilter.cpp
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 
25 
26 #include <gtsam/base/Testable.h>
29 
30 #ifndef NDEBUG
31 #include <cassert>
32 #endif
33 
34 // In the code below we often get a covariance matrix Q, and we need to create a
35 // JacobianFactor with cost 0.5 * |Ax - b|^T Q^{-1} |Ax - b|. Factorizing Q as
36 // Q = L L^T
37 // and hence
38 // Q^{-1} = L^{-T} L^{-1} = M^T M, with M = L^{-1}
39 // We then have
40 // 0.5 * |Ax - b|^T Q^{-1} |Ax - b|
41 // = 0.5 * |Ax - b|^T M^T M |Ax - b|
42 // = 0.5 * |MAx - Mb|^T |MAx - Mb|
43 // The functor below efficiently multiplies with M by calling L.solve().
44 namespace {
45 using Matrix = gtsam::Matrix;
46 struct InverseL : public Eigen::LLT<Matrix> {
47  InverseL(const Matrix& Q) : Eigen::LLT<Matrix>(Q) {}
48  Matrix operator*(const Matrix& A) const { return matrixL().solve(A); }
49 };
50 } // namespace
51 
52 namespace gtsam {
53 /* ************************************************************************* */
54 // Auxiliary function to solve factor graph and return pointer to root
55 // conditional
57  const GaussianFactorGraph& factorGraph) const {
58  // Eliminate the graph using the provided Eliminate function
59  Ordering ordering(factorGraph.keys());
60  const auto bayesNet = factorGraph.eliminateSequential(ordering, function_);
61 
62  // As this is a filter, all we need is the posterior P(x_t).
63  // This is the last GaussianConditional in the resulting BayesNet
64  GaussianConditional::shared_ptr posterior = bayesNet->back();
65  return std::make_shared<GaussianDensity>(*posterior);
66 }
67 
68 /* ************************************************************************* */
69 // Auxiliary function to create a small graph for predict or update and solve
71  const State& p, GaussianFactor::shared_ptr newFactor) const {
72  // Create a factor graph
73  GaussianFactorGraph factorGraph;
74  factorGraph.push_back(p);
75  factorGraph.push_back(newFactor);
76 
77  // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
78  return solve(factorGraph);
79 }
80 
81 /* ************************************************************************* */
83  const SharedDiagonal& P0) const {
84  // Create a factor graph f(x0), eliminate it into P(x0)
85  GaussianFactorGraph factorGraph;
86  factorGraph.emplace_shared<JacobianFactor>(0, I_, x0,
87  P0); // |x-x0|^2_P0
88  return solve(factorGraph);
89 }
90 
91 /* ************************************************************************* */
93  const Matrix& P0) const {
94  // Create a factor graph f(x0), eliminate it into P(x0)
95  GaussianFactorGraph factorGraph;
96 
97  // Perform Cholesky decomposition of P0 = LL^T
98  InverseL M(P0);
99 
100  // Premultiply I and x0 with M=L^{-1}
101  const Matrix A = M * I_; // = M
102  const Vector b = M * x0; // = M*x0
103  factorGraph.emplace_shared<JacobianFactor>(0, A, b);
104  return solve(factorGraph);
105 }
106 
107 /* ************************************************************************* */
108 void KalmanFilter::print(const std::string& s) const {
109  std::cout << "KalmanFilter " << s << ", dim = " << n_ << std::endl;
110 }
111 
112 /* ************************************************************************* */
114  const Matrix& B, const Vector& u,
115  const SharedDiagonal& model) const {
116  // The factor related to the motion model is defined as
117  // f2(x_{k},x_{k+1}) =
118  // (F*x_{k} + B*u - x_{k+1}) * Q^-1 * (F*x_{k} + B*u - x_{k+1})^T
119  Key k = step(p);
120  return fuse(p,
121  std::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
122 }
123 
124 /* ************************************************************************* */
126  const Matrix& B, const Vector& u,
127  const Matrix& Q) const {
128 #ifndef NDEBUG
129  const DenseIndex n = dim();
130  assert(F.cols() == n);
131  assert(F.rows() == n);
132  assert(B.rows() == n);
133  assert(B.cols() == u.size());
134  assert(Q.rows() == n);
135  assert(Q.cols() == n);
136 #endif
137 
138  // Perform Cholesky decomposition of Q = LL^T
139  InverseL M(Q);
140 
141  // Premultiply -F, I, and B * u with M=L^{-1}
142  const Matrix A1 = -(M * F);
143  const Matrix A2 = M * I_;
144  const Vector b = M * (B * u);
145  return predict2(p, A1, A2, b);
146 }
147 
148 /* ************************************************************************* */
150  const Matrix& A1, const Vector& b,
151  const SharedDiagonal& model) const {
152  // Nhe factor related to the motion model is defined as
153  // f2(x_{k},x_{k+1}) = |A0*x_{k} + A1*x_{k+1} - b|^2
154  Key k = step(p);
155  return fuse(p, std::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model));
156 }
157 
158 /* ************************************************************************* */
160  const Vector& z,
161  const SharedDiagonal& model) const {
162  // The factor related to the measurements would be defined as
163  // f2 = (h(x_{k}) - z_{k}) * R^-1 * (h(x_{k}) - z_{k})^T
164  // = (x_{k} - z_{k}) * R^-1 * (x_{k} - z_{k})^T
165  Key k = step(p);
166  return fuse(p, std::make_shared<JacobianFactor>(k, H, z, model));
167 }
168 
169 /* ************************************************************************* */
171  const Vector& z,
172  const Matrix& Q) const {
173  Key k = step(p);
174 
175  // Perform Cholesky decomposition of Q = LL^T
176  InverseL M(Q);
177 
178  // Pre-multiply H and z with M=L^{-1}, respectively
179  const Matrix A = M * H;
180  const Vector b = M * z;
181  return fuse(p, std::make_shared<JacobianFactor>(k, A, b));
182 }
183 
184 /* ************************************************************************* */
185 
186 } // 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
gtsam::EliminateableFactorGraph::eliminateSequential
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:29
Eigen
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
gtsam::GaussianFactorGraph::keys
Keys keys() const
Definition: GaussianFactorGraph.cpp:48
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::KalmanFilter::n_
const size_t n_
Dimensionality of the state.
Definition: KalmanFilter.h:58
Testable.h
Concept check for values that can be used in unit tests.
gtsam::KalmanFilter::I_
const Matrix I_
Identity matrix of size .
Definition: KalmanFilter.h:59
gtsam::KalmanFilter::State
GaussianDensity::shared_ptr State
The Kalman filter state, represented as a shared pointer to a GaussianDensity.
Definition: KalmanFilter.h:55
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
B
Definition: test_numpy_dtypes.cpp:299
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
A0
static const double A0[]
Definition: expn.h:5
Eigen::operator*
const EIGEN_DEVICE_FUNC Product< MatrixDerived, PermutationDerived, AliasFreeProduct > operator*(const MatrixBase< MatrixDerived > &matrix, const PermutationBase< PermutationDerived > &permutation)
Definition: PermutationMatrix.h:515
gtsam::KalmanFilter::dim
size_t dim() const
Definition: KalmanFilter.h:214
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::KalmanFilter::predict
State predict(const State &p, const Matrix &F, const Matrix &B, const Vector &u, const SharedDiagonal &modelQ) const
Definition: KalmanFilter.cpp:113
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
gtsam::KalmanFilter::fuse
State fuse(const State &p, GaussianFactor::shared_ptr newFactor) const
Definition: KalmanFilter.cpp:70
gtsam::KalmanFilter::predict2
State predict2(const State &p, const Matrix &A0, const Matrix &A1, const Vector &b, const SharedDiagonal &model=nullptr) const
Definition: KalmanFilter.cpp:149
A
Definition: test_numpy_dtypes.cpp:298
different_sigmas::bayesNet
const HybridBayesNet bayesNet
Definition: testHybridBayesNet.cpp:242
Eigen::LLT::matrixL
Traits::MatrixL matrixL() const
Definition: LLT.h:135
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
A2
static const double A2[]
Definition: expn.h:7
gtsam::KalmanFilter::print
void print(const std::string &s="") const
Definition: KalmanFilter.cpp:108
x0
static Symbol x0('x', 0)
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
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
ordering
static enum @1096 ordering
gtsam::KalmanFilter::predictQ
State predictQ(const State &p, const Matrix &F, const Matrix &B, const Vector &u, const Matrix &Q) const
Definition: KalmanFilter.cpp:125
JacobianFactor.h
gtsam::b
const G & b
Definition: Group.h:79
KalmanFilter.h
Simple linear Kalman filter implemented using factor graphs, i.e., performs Cholesky or QR-based SRIF...
gtsam::KalmanFilter::solve
State solve(const GaussianFactorGraph &factorGraph) const
Definition: KalmanFilter.cpp:56
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
Eigen::LLT
Standard Cholesky decomposition (LL^T) of a matrix and associated features.
Definition: LLT.h:66
State
Definition: testKalmanFilter.cpp:31
gtsam
traits
Definition: SFMdata.h:40
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition: types.h:103
p
float * p
Definition: Tutorial_Map_using.cpp:9
A1
static const double A1[]
Definition: expn.h:6
gtsam::GaussianConditional::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:46
gtsam::KalmanFilter::updateQ
State updateQ(const State &p, const Matrix &H, const Vector &z, const Matrix &R) const
Definition: KalmanFilter.cpp:170
gtsam::KalmanFilter::update
State update(const State &p, const Matrix &H, const Vector &z, const SharedDiagonal &model) const
Definition: KalmanFilter.cpp:159
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::KalmanFilter::init
State init(const Vector &x0, const SharedDiagonal &P0) const
Definition: KalmanFilter.cpp:82
gtsam::Ordering
Definition: inference/Ordering.h:33
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


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