Loading...
Searching...
No Matches
ExtendedKalmanFilter.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
// \callgraph
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
template <class VALUE>
class ExtendedKalmanFilter {
// Check that VALUE type is a testable Manifold
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
public:
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
typedef VALUE T;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
//@deprecated: any NoiseModelFactor will do, as long as they have the right keys
typedef NoiseModelFactorN<VALUE, VALUE> MotionFactor;
typedef NoiseModelFactorN<VALUE> MeasurementFactor;
#endif
protected:
T x_; // linearization point
JacobianFactor::shared_ptr priorFactor_; // Gaussian density on x_
static T solve_(const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints,
Key x, JacobianFactor::shared_ptr* newPrior);
public:
ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial);
void print(const std::string& s = "") const {
std::cout << s << "\n";
x_.print(s + "x");
priorFactor_->print(s + "density");
}
T predict(const NoiseModelFactor& motionFactor);
T update(const NoiseModelFactor& measurementFactor);
const JacobianFactor::shared_ptr Density() const {
return priorFactor_;
}
};
} // namespace
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
NonlinearFactor.h
Non-linear factor base classes.
ExtendedKalmanFilter-inl.h
Class to perform generic Kalman Filtering using nonlinear factor graphs.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
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::Density
const JacobianFactor::shared_ptr Density() const
Return current predictive (if called after predict)/posterior (if called after update).
Definition ExtendedKalmanFilter.h:102
gtsam::ExtendedKalmanFilter::predict
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
gtsam::ExtendedKalmanFilter::print
void print(const std::string &s="") const
print
Definition ExtendedKalmanFilter.h:78
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65