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 {
  public:

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

  protected:
    T x_; // linearization point
    JacobianFactor::shared_ptr priorFactor_; // density

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

  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 MotionFactor& motionFactor);

    T update(const MeasurementFactor& measurementFactor);

  };

} // namespace

#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>