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 
27 #include <gtsam/base/Testable.h>
28 
29 
30 using namespace std;
31 
32 namespace gtsam {
33 
34 /* ************************************************************************* */
35 // Auxiliary function to solve factor graph and return pointer to root conditional
37 KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
38 
39  // Eliminate the graph using the provided Eliminate function
40  Ordering ordering(factorGraph.keys());
41  const auto bayesNet = //
42  factorGraph.eliminateSequential(ordering, function_);
43 
44  // As this is a filter, all we need is the posterior P(x_t).
45  // This is the last GaussianConditional in the resulting BayesNet
46  GaussianConditional::shared_ptr posterior = bayesNet->back();
47  return std::make_shared<GaussianDensity>(*posterior);
48 }
49 
50 /* ************************************************************************* */
51 // Auxiliary function to create a small graph for predict or update and solve
53 KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {
54 
55  // Create a factor graph
56  GaussianFactorGraph factorGraph;
57  factorGraph.push_back(p);
58  factorGraph.push_back(newFactor);
59 
60  // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
61  return solve(factorGraph);
62 }
63 
64 /* ************************************************************************* */
66  const SharedDiagonal& P0) const {
67 
68  // Create a factor graph f(x0), eliminate it into P(x0)
69  GaussianFactorGraph factorGraph;
70  factorGraph.emplace_shared<JacobianFactor>(0, I_, x0, P0); // |x-x0|^2_diagSigma
71  return solve(factorGraph);
72 }
73 
74 /* ************************************************************************* */
76 
77  // Create a factor graph f(x0), eliminate it into P(x0)
78  GaussianFactorGraph factorGraph;
79  factorGraph.emplace_shared<HessianFactor>(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)
80  return solve(factorGraph);
81 }
82 
83 /* ************************************************************************* */
84 void KalmanFilter::print(const string& s) const {
85  cout << "KalmanFilter " << s << ", dim = " << n_ << endl;
86 }
87 
88 /* ************************************************************************* */
89 KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
90  const Matrix& B, const Vector& u, const SharedDiagonal& model) const {
91 
92  // The factor related to the motion model is defined as
93  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
94  Key k = step(p);
95  return fuse(p,
96  std::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
97 }
98 
99 /* ************************************************************************* */
100 KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
101  const Matrix& B, const Vector& u, const Matrix& Q) const {
102 
103 #ifndef NDEBUG
104  DenseIndex n = F.cols();
105  assert(F.rows() == n);
106  assert(B.rows() == n);
107  assert(B.cols() == u.size());
108  assert(Q.rows() == n);
109  assert(Q.cols() == n);
110 #endif
111 
112  // The factor related to the motion model is defined as
113  // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
114  // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u:
115  // TODO: starts to seem more elaborate than straight-up KF equations?
116  Matrix M = Q.inverse(), Ft = trans(F);
117  Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M;
118  Vector b = B * u, g2 = M * b, g1 = -Ft * g2;
119  double f = dot(b, g2);
120  Key k = step(p);
121  return fuse(p,
122  std::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f));
123 }
124 
125 /* ************************************************************************* */
126 KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
127  const Matrix& A1, const Vector& b, const SharedDiagonal& model) const {
128  // Nhe factor related to the motion model is defined as
129  // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
130  Key k = step(p);
131  return fuse(p, std::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model));
132 }
133 
134 /* ************************************************************************* */
136  const Vector& z, const SharedDiagonal& model) const {
137  // The factor related to the measurements would be defined as
138  // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T
139  // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
140  Key k = step(p);
141  return fuse(p, std::make_shared<JacobianFactor>(k, H, z, model));
142 }
143 
144 /* ************************************************************************* */
145 KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
146  const Vector& z, const Matrix& Q) const {
147  Key k = step(p);
148  Matrix M = Q.inverse(), Ht = trans(H);
149  Matrix G = Ht * M * H;
150  Vector g = Ht * M * z;
151  double f = dot(z, M * z);
152  return fuse(p, std::make_shared<HessianFactor>(k, G, g, f));
153 }
154 
155 /* ************************************************************************* */
156 
157 } // \namespace gtsam
158 
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::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
relicense.update
def update(text)
Definition: relicense.py:46
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
gtsam::GaussianFactorGraph::keys
Keys keys() const
Definition: GaussianFactorGraph.cpp:48
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition: HessianFactor.h:100
s
RealScalar s
Definition: level1_cplx_impl.h:126
Testable.h
Concept check for values that can be used in unit tests.
g1
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
b
Scalar * b
Definition: benchVecAdd.cpp:17
x
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 x
Definition: gnuplot_common_settings.hh:12
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
trans
static char trans
Definition: blas_interface.hh:58
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
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:38
A0
static const double A0[]
Definition: expn.h:5
dot
Scalar EIGEN_BLAS_FUNC() dot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy)
Definition: level1_real_impl.h:49
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
different_sigmas::bayesNet
const HybridBayesNet bayesNet
Definition: testHybridBayesNet.cpp:242
g2
Pose3 g2(g1.expmap(h *V1_g1))
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::State
internal::DoglegState State
Definition: DoglegOptimizer.cpp:62
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:764
x0
static Symbol x0('x', 0)
gtsam::utils.visual_isam.step
def step(data, isam, result, truth, currPoseIndex, isamArgs=())
Definition: visual_isam.py:82
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
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
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
ordering
static enum @1096 ordering
JacobianFactor.h
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
KalmanFilter.h
Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF,...
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
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
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
G
JacobiRotation< float > G
Definition: Jacobi_makeGivens.cpp:2
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
P0
static double P0[5]
Definition: ndtri.c:59
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
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 Fri Nov 1 2024 03:33:00