gtsam  4.1.0
gtsam
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 
25 namespace gtsam {
26 
44 template <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  //@deprecated: any NoiseModelFactor will do, as long as they have the right keys
57 
58  protected:
59  T x_; // linearization point
60  JacobianFactor::shared_ptr priorFactor_; // Gaussian density on x_
61 
62  static T solve_(const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints,
63  Key x, JacobianFactor::shared_ptr* newPrior);
64 
65  public:
68 
69  ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial);
70 
74 
76  void print(const std::string& s = "") const {
77  std::cout << s << "\n";
78  x_.print(s + "x");
79  priorFactor_->print(s + "density");
80  }
81 
85 
90  T predict(const NoiseModelFactor& motionFactor);
91 
96  T update(const NoiseModelFactor& measurementFactor);
97 
100  return priorFactor_;
101  }
102 
104 };
105 
106 } // namespace
107 
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::IsTestable
Definition: Testable.h:57
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
ExtendedKalmanFilter-inl.h
Class to perform generic Kalman Filtering using nonlinear factor graphs.
gtsam::ExtendedKalmanFilter
This is a generic Extended Kalman Filter class implemented using nonlinear factors.
Definition: ExtendedKalmanFilter.h:45
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:96
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:154
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
NonlinearFactor.h
Non-linear factor base classes.
gtsam::ExtendedKalmanFilter::update
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
gtsam::ExtendedKalmanFilter::print
void print(const std::string &s="") const
print
Definition: ExtendedKalmanFilter.h:76
gtsam::ExtendedKalmanFilter::Density
const JacobianFactor::shared_ptr Density() const
Return current predictive (if called after predict)/posterior (if called after update)
Definition: ExtendedKalmanFilter.h:99
gtsam::NoiseModelFactor2< VALUE, VALUE >
gtsam::ExtendedKalmanFilter::predict
T predict(const NoiseModelFactor &motionFactor)
Calculate predictive density P(x_) ~ \int P(x_min) P(x_min, x_) The motion model should be given as a...
Definition: ExtendedKalmanFilter-inl.h:80
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71