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
12/**
13 * @file easyPoint2KalmanFilter.cpp
14 *
15 * simple linear Kalman filter on a moving 2D point, but done using factor graphs
16 * This example uses the templated ExtendedKalmanFilter class to perform the same
17 * operations as in elaboratePoint2KalmanFilter
18 *
19 * @date Aug 19, 2011
20 * @author Frank Dellaert
21 * @author Stephen Williams
22 */
23
24#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
25#include <gtsam/inference/Symbol.h>
26#include <gtsam/nonlinear/PriorFactor.h>
27#include <gtsam/slam/BetweenFactor.h>
28#include <gtsam/geometry/Point2.h>
29
30using namespace std;
31using namespace gtsam;
32
33// Define Types for Linear System Test
34typedef Point2 LinearMeasurement;
35
36int main() {
37
38 // Create the Kalman Filter initialization point
39 Point2 x_initial(0.0, 0.0);
40 SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.1, 0.1));
41
42 // Create Key for initial pose
43 Symbol x0('x',0);
44
45 // Create an ExtendedKalmanFilter object
46 ExtendedKalmanFilter<Point2> ekf(x0, x_initial, P_initial);
47
48 // Now predict the state at t=1, i.e. argmax_{x1} P(x1) = P(x1|x0) P(x0)
49 // In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
50 // For the Kalman Filter, this requires a motion model, f(x_{t}) = x_{t+1|t)
51 // Assuming the system is linear, this will be of the form f(x_{t}) = F*x_{t} + B*u_{t} + w
52 // where F is the state transition model/matrix, B is the control input model,
53 // and w is zero-mean, Gaussian white noise with covariance Q
54 // Note, in some models, Q is actually derived as G*w*G^T where w models uncertainty of some
55 // physical property, such as velocity or acceleration, and G is derived from physics
56 //
57 // For the purposes of this example, let us assume we are using a constant-position model and
58 // the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1]
59 // and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1].
60 Vector u = Vector2(1.0, 0.0);
61 SharedDiagonal Q = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.1, 0.1), smart: true);
62
63 // This simple motion can be modeled with a BetweenFactor
64 // Create Key for next pose
65 Symbol x1('x',1);
66 // Predict delta based on controls
67 Point2 difference(1,0);
68 // Create Factor
69 BetweenFactor<Point2> factor1(x0, x1, difference, Q);
70
71 // Predict the new value with the EKF class
72 Point2 x1_predict = ekf.predict(motionFactor: factor1);
73 traits<Point2>::Print(m: x1_predict, str: "X1 Predict");
74
75
76
77 // Now, a measurement, z1, has been received, and the Kalman Filter should be "Updated"/"Corrected"
78 // This is equivalent to saying P(x1|z1) ~ P(z1|x1)*P(x1)
79 // For the Kalman Filter, this requires a measurement model h(x_{t}) = \hat{z}_{t}
80 // Assuming the system is linear, this will be of the form h(x_{t}) = H*x_{t} + v
81 // where H is the observation model/matrix, and v is zero-mean, Gaussian white noise with covariance R
82 //
83 // For the purposes of this example, let us assume we have something like a GPS that returns
84 // the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise
85 // R = [0.25 0 ; 0 0.25].
86 SharedDiagonal R = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.25, 0.25), smart: true);
87
88 // This simple measurement can be modeled with a PriorFactor
89 Point2 z1(1.0, 0.0);
90 PriorFactor<Point2> factor2(x1, z1, R);
91
92 // Update the Kalman Filter with the measurement
93 Point2 x1_update = ekf.update(measurementFactor: factor2);
94 traits<Point2>::Print(m: x1_update, str: "X1 Update");
95
96
97
98 // Do the same thing two more times...
99 // Predict
100 Symbol x2('x',2);
101 difference = Point2(1,0);
102 BetweenFactor<Point2> factor3(x1, x2, difference, Q);
103 Point2 x2_predict = ekf.predict(motionFactor: factor3);
104 traits<Point2>::Print(m: x2_predict, str: "X2 Predict");
105
106 // Update
107 Point2 z2(2.0, 0.0);
108 PriorFactor<Point2> factor4(x2, z2, R);
109 Point2 x2_update = ekf.update(measurementFactor: factor4);
110 traits<Point2>::Print(m: x2_update, str: "X2 Update");
111
112
113
114 // Do the same thing one more time...
115 // Predict
116 Symbol x3('x',3);
117 difference = Point2(1,0);
118 BetweenFactor<Point2> factor5(x2, x3, difference, Q);
119 Point2 x3_predict = ekf.predict(motionFactor: factor5);
120 traits<Point2>::Print(m: x3_predict, str: "X3 Predict");
121
122 // Update
123 Point2 z3(3.0, 0.0);
124 PriorFactor<Point2> factor6(x3, z3, R);
125 Point2 x3_update = ekf.update(measurementFactor: factor6);
126 traits<Point2>::Print(m: x3_update, str: "X3 Update");
127
128 return 0;
129}
130