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