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
  GTSAM_CONCEPT_ASSERT(IsTestable<VALUE>);
  GTSAM_CONCEPT_ASSERT(IsManifold<VALUE>);
 
 public:
  typedef std::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
  typedef VALUE T;
 
 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>