Loading...
Searching...
No Matches
ExtendedKalmanFilter-inl.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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
namespace gtsam {
/* ************************************************************************* */
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
const GaussianFactorGraph& linearFactorGraph,
const Values& linearizationPoint, Key lastKey,
JacobianFactor::shared_ptr* newPrior)
{
// Compute the marginal on the last key
// Solve the linear factor graph, converting it into a linear Bayes Network
// P(x0,x1) = P(x0|x1)*P(x1)
Ordering lastKeyAsOrdering;
lastKeyAsOrdering += lastKey;
const GaussianConditional::shared_ptr marginal =
linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front();
// Extract the current estimate of x1,P1
VectorValues result = marginal->solve(VectorValues());
const T& current = linearizationPoint.at<T>(lastKey);
T x = traits<T>::Retract(current, result[lastKey]);
// Create a Jacobian Factor from the root node of the produced Bayes Net.
// This will act as a prior for the next iteration.
// The linearization point of this prior must be moved to the new estimate of x,
// and the key/index needs to be reset to 0, the first key in the next iteration.
assert(marginal->nrFrontals() == 1);
assert(marginal->nrParents() == 0);
*newPrior = boost::make_shared<JacobianFactor>(
marginal->keys().front(),
marginal->getA(marginal->begin()),
marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey],
marginal->get_model());
return x;
}
/* ************************************************************************* */
template <class VALUE>
ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(
Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial)
: x_(x_initial) // Set the initial linearization point
{
// Create a Jacobian Prior Factor directly P_initial.
// Since x0 is set to the provided mean, the b vector in the prior will be zero
// TODO(Frank): is there a reason why noiseModel is not simply P_initial?
int n = traits<T>::GetDimension(x_initial);
priorFactor_ = JacobianFactor::shared_ptr(
new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n),
noiseModel::Unit::Create(n)));
}
/* ************************************************************************* */
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::predict(
const NoiseModelFactor& motionFactor) {
const auto keys = motionFactor.keys();
// Create a Gaussian Factor Graph
GaussianFactorGraph linearFactorGraph;
// Add in previous posterior as prior on the first state
linearFactorGraph.push_back(priorFactor_);
// Linearize motion model and add it to the Kalman Filter graph
Values linearizationPoint;
linearizationPoint.insert(keys[0], x_);
linearizationPoint.insert(keys[1], x_); // TODO should this really be x_ ?
linearFactorGraph.push_back(motionFactor.linearize(linearizationPoint));
// Solve the factor graph and update the current state estimate
// and the posterior for the next iteration.
x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_);
return x_;
}
/* ************************************************************************* */
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::update(
const NoiseModelFactor& measurementFactor) {
const auto keys = measurementFactor.keys();
// Create a Gaussian Factor Graph
GaussianFactorGraph linearFactorGraph;
// Add in the prior on the first state
linearFactorGraph.push_back(priorFactor_);
// Linearize measurement factor and add it to the Kalman Filter graph
Values linearizationPoint;
linearizationPoint.insert(keys[0], x_);
linearFactorGraph.push_back(measurementFactor.linearize(linearizationPoint));
// Solve the factor graph and update the current state estimate
// and the prior factor for the next iteration
x_ = solve_(linearFactorGraph, linearizationPoint, keys[0], &priorFactor_);
return x_;
}
} // namespace gtsam
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
NonlinearFactor.h
Non-linear factor base classes.
ExtendedKalmanFilter.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::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Ordering
Definition Ordering.h:34
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
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::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
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::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::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
gtsam::NoiseModelFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize a non-linearFactorN to get a GaussianFactor, Hence .
Definition NonlinearFactor.cpp:152
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...