Public Types | Static Protected Member Functions | Protected Attributes | Private Member Functions | List of all members
gtsam::ExtendedKalmanFilter< VALUE > Class Template Reference

#include <ExtendedKalmanFilter.h>

Public Types

typedef NoiseModelFactor1< VALUE > MeasurementFactor
 
typedef NoiseModelFactor2< VALUE, VALUE > MotionFactor
 
typedef boost::shared_ptr< ExtendedKalmanFilter< VALUE > > shared_ptr
 
typedef VALUE T
 

Static Protected Member Functions

static T solve_ (const GaussianFactorGraph &linearFactorGraph, const Values &linearizationPoints, Key x, JacobianFactor::shared_ptr *newPrior)
 

Protected Attributes

JacobianFactor::shared_ptr priorFactor_
 
T x_
 

Private Member Functions

 BOOST_CONCEPT_ASSERT ((IsTestable< VALUE >))
 
 BOOST_CONCEPT_ASSERT ((IsManifold< VALUE >))
 

Standard Constructors

 ExtendedKalmanFilter (Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial)
 

Testable

void print (const std::string &s="") const
 print More...
 

Interface

T predict (const NoiseModelFactor &motionFactor)
 
T update (const NoiseModelFactor &measurementFactor)
 
const JacobianFactor::shared_ptr Density () const
 Return current predictive (if called after predict)/posterior (if called after update) More...
 

Detailed Description

template<class VALUE>
class gtsam::ExtendedKalmanFilter< VALUE >

This is a generic Extended Kalman Filter class implemented using nonlinear factors. GTSAM basically does SRIF with Cholesky to solve the filter problem, making this an efficient, numerically stable Kalman Filter implementation.

The Kalman Filter relies on two models: a motion model that predicts the next state using the current state, and a measurement model that predicts the measurement value at a given state. Because these two models are situation-dependent, base classes for each have been provided above, from which the user must derive a class and incorporate the actual modeling equations.

The class provides a "predict" and "update" function to perform these steps independently. TODO: a "predictAndUpdate" that combines both steps for some computational savings.

Definition at line 45 of file ExtendedKalmanFilter.h.

Member Typedef Documentation

template<class VALUE>
typedef NoiseModelFactor1<VALUE> gtsam::ExtendedKalmanFilter< VALUE >::MeasurementFactor

Definition at line 56 of file ExtendedKalmanFilter.h.

template<class VALUE>
typedef NoiseModelFactor2<VALUE, VALUE> gtsam::ExtendedKalmanFilter< VALUE >::MotionFactor

Definition at line 55 of file ExtendedKalmanFilter.h.

template<class VALUE>
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > gtsam::ExtendedKalmanFilter< VALUE >::shared_ptr

Definition at line 51 of file ExtendedKalmanFilter.h.

template<class VALUE>
typedef VALUE gtsam::ExtendedKalmanFilter< VALUE >::T

Definition at line 52 of file ExtendedKalmanFilter.h.

Constructor & Destructor Documentation

template<class VALUE >
gtsam::ExtendedKalmanFilter< VALUE >::ExtendedKalmanFilter ( Key  key_initial,
T  x_initial,
noiseModel::Gaussian::shared_ptr  P_initial 
)

Definition at line 65 of file ExtendedKalmanFilter-inl.h.

Member Function Documentation

template<class VALUE>
gtsam::ExtendedKalmanFilter< VALUE >::BOOST_CONCEPT_ASSERT ( (IsTestable< VALUE >)  )
private
template<class VALUE>
gtsam::ExtendedKalmanFilter< VALUE >::BOOST_CONCEPT_ASSERT ( (IsManifold< VALUE >)  )
private
template<class VALUE>
const JacobianFactor::shared_ptr gtsam::ExtendedKalmanFilter< VALUE >::Density ( ) const
inline

Return current predictive (if called after predict)/posterior (if called after update)

Definition at line 99 of file ExtendedKalmanFilter.h.

template<class VALUE >
ExtendedKalmanFilter< VALUE >::T gtsam::ExtendedKalmanFilter< VALUE >::predict ( const NoiseModelFactor motionFactor)

Calculate predictive density P(x_) ~ P(x_min) P(x_min, x_) The motion model should be given as a factor with key1 for x_min and key2_ for x

Definition at line 80 of file ExtendedKalmanFilter-inl.h.

template<class VALUE>
void gtsam::ExtendedKalmanFilter< VALUE >::print ( const std::string &  s = "") const
inline

print

Definition at line 76 of file ExtendedKalmanFilter.h.

template<class VALUE >
ExtendedKalmanFilter< VALUE >::T gtsam::ExtendedKalmanFilter< VALUE >::solve_ ( const GaussianFactorGraph linearFactorGraph,
const Values linearizationPoints,
Key  x,
JacobianFactor::shared_ptr newPrior 
)
staticprotected

Definition at line 30 of file ExtendedKalmanFilter-inl.h.

template<class VALUE >
ExtendedKalmanFilter< VALUE >::T gtsam::ExtendedKalmanFilter< VALUE >::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 factor on x

Definition at line 105 of file ExtendedKalmanFilter-inl.h.

Member Data Documentation

template<class VALUE>
JacobianFactor::shared_ptr gtsam::ExtendedKalmanFilter< VALUE >::priorFactor_
protected

Definition at line 60 of file ExtendedKalmanFilter.h.

template<class VALUE>
T gtsam::ExtendedKalmanFilter< VALUE >::x_
protected

Definition at line 59 of file ExtendedKalmanFilter.h.


The documentation for this class was generated from the following files:


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:58:09