gtsam  4.1.0
gtsam
ExtendedKalmanFilter-inl.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
25 
26 namespace gtsam {
27 
28  /* ************************************************************************* */
29  template<class VALUE>
30  typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
31  const GaussianFactorGraph& linearFactorGraph,
32  const Values& linearizationPoint, Key lastKey,
34  {
35  // Compute the marginal on the last key
36  // Solve the linear factor graph, converting it into a linear Bayes Network
37  // P(x0,x1) = P(x0|x1)*P(x1)
38  Ordering lastKeyAsOrdering;
39  lastKeyAsOrdering += lastKey;
40  const GaussianConditional::shared_ptr marginal =
41  linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front();
42 
43  // Extract the current estimate of x1,P1
44  VectorValues result = marginal->solve(VectorValues());
45  const T& current = linearizationPoint.at<T>(lastKey);
46  T x = traits<T>::Retract(current, result[lastKey]);
47 
48  // Create a Jacobian Factor from the root node of the produced Bayes Net.
49  // This will act as a prior for the next iteration.
50  // The linearization point of this prior must be moved to the new estimate of x,
51  // and the key/index needs to be reset to 0, the first key in the next iteration.
52  assert(marginal->nrFrontals() == 1);
53  assert(marginal->nrParents() == 0);
54  *newPrior = boost::make_shared<JacobianFactor>(
55  marginal->keys().front(),
56  marginal->getA(marginal->begin()),
57  marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey],
58  marginal->get_model());
59 
60  return x;
61  }
62 
63  /* ************************************************************************* */
64  template <class VALUE>
65  ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(
66  Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial)
67  : x_(x_initial) // Set the initial linearization point
68  {
69  // Create a Jacobian Prior Factor directly P_initial.
70  // Since x0 is set to the provided mean, the b vector in the prior will be zero
71  // TODO(Frank): is there a reason why noiseModel is not simply P_initial?
72  int n = traits<T>::GetDimension(x_initial);
73  priorFactor_ = JacobianFactor::shared_ptr(
74  new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n),
75  noiseModel::Unit::Create(n)));
76  }
77 
78  /* ************************************************************************* */
79  template<class VALUE>
80  typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::predict(
81  const NoiseModelFactor& motionFactor) {
82  const auto keys = motionFactor.keys();
83 
84  // Create a Gaussian Factor Graph
85  GaussianFactorGraph linearFactorGraph;
86 
87  // Add in previous posterior as prior on the first state
88  linearFactorGraph.push_back(priorFactor_);
89 
90  // Linearize motion model and add it to the Kalman Filter graph
91  Values linearizationPoint;
92  linearizationPoint.insert(keys[0], x_);
93  linearizationPoint.insert(keys[1], x_); // TODO should this really be x_ ?
94  linearFactorGraph.push_back(motionFactor.linearize(linearizationPoint));
95 
96  // Solve the factor graph and update the current state estimate
97  // and the posterior for the next iteration.
98  x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_);
99 
100  return x_;
101  }
102 
103  /* ************************************************************************* */
104  template<class VALUE>
105  typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::update(
106  const NoiseModelFactor& measurementFactor) {
107  const auto keys = measurementFactor.keys();
108 
109  // Create a Gaussian Factor Graph
110  GaussianFactorGraph linearFactorGraph;
111 
112  // Add in the prior on the first state
113  linearFactorGraph.push_back(priorFactor_);
114 
115  // Linearize measurement factor and add it to the Kalman Filter graph
116  Values linearizationPoint;
117  linearizationPoint.insert(keys[0], x_);
118  linearFactorGraph.push_back(measurementFactor.linearize(linearizationPoint));
119 
120  // Solve the factor graph and update the current state estimate
121  // and the prior factor for the next iteration
122  x_ = solve_(linearFactorGraph, linearizationPoint, keys[0], &priorFactor_);
123 
124  return x_;
125  }
126 
127 } // namespace gtsam
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:162
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ExtendedKalmanFilter
This is a generic Extended Kalman Filter class implemented using nonlinear factors.
Definition: ExtendedKalmanFilter.h:45
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:96
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:154
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:42
NonlinearFactor.h
Non-linear factor base classes.
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:140
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
ExtendedKalmanFilter.h
Class to perform generic Kalman Filtering using nonlinear factor graphs.
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:133