gtsam 4.1.1 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
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10 * -------------------------------------------------------------------------- */
11
19#pragma once
20
25
26namespace 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
Chordal Bayes Net, the result of eliminating a factor graph.
Linear Factor Graph where all factors are Gaussians.
Class to perform generic Kalman Filtering using nonlinear factor graphs.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:165
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:125
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianConditional.h:42
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:69
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:96
This is a generic Extended Kalman Filter class implemented using nonlinear factors.
Definition: ExtendedKalmanFilter.h:45
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:162
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize a non-linearFactorN to get a GaussianFactor, Hence .
Definition: NonlinearFactor.cpp:141
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
void insert(Key j, const Value &val)