gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
ExtendedKalmanFilter.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
19// \callgraph
20#pragma once
21
24
25namespace gtsam {
26
44template <class VALUE>
46 // Check that VALUE type is a testable Manifold
47 BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
48 BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
49
50 public:
51 typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
52 typedef VALUE T;
53
54#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
55 //@deprecated: any NoiseModelFactor will do, as long as they have the right keys
56 typedef NoiseModelFactorN<VALUE, VALUE> MotionFactor;
57 typedef NoiseModelFactorN<VALUE> MeasurementFactor;
58#endif
59
60 protected:
61 T x_; // linearization point
62 JacobianFactor::shared_ptr priorFactor_; // Gaussian density on x_
63
64 static T solve_(const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints,
65 Key x, JacobianFactor::shared_ptr* newPrior);
66
67 public:
70
71 ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial);
72
76
78 void print(const std::string& s = "") const {
79 std::cout << s << "\n";
80 x_.print(s + "x");
81 priorFactor_->print(s + "density");
82 }
83
87
93 T predict(const NoiseModelFactor& motionFactor);
94
99 T update(const NoiseModelFactor& measurementFactor);
100
103 return priorFactor_;
104 }
105
107};
108
109} // namespace
110
Factor Graph consisting of non-linear factors.
Non-linear factor base classes.
Class to perform generic Kalman Filtering using nonlinear factor graphs.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
This is a generic Extended Kalman Filter class implemented using nonlinear factors.
Definition ExtendedKalmanFilter.h:45
T update(const NoiseModelFactor &measurementFactor)
Calculate posterior density P(x_) ~ L(z|x) P(x) The likelihood L(z|x) should be given as a unary fact...
Definition ExtendedKalmanFilter-inl.h:105
const JacobianFactor::shared_ptr Density() const
Return current predictive (if called after predict)/posterior (if called after update)
Definition ExtendedKalmanFilter.h:102
T predict(const NoiseModelFactor &motionFactor)
Calculate predictive density The motion model should be given as a factor with key1 for and key2 fo...
Definition ExtendedKalmanFilter-inl.h:80
void print(const std::string &s="") const
print
Definition ExtendedKalmanFilter.h:78
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65