easyPoint2KalmanFilter.cpp
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 
25 #include <gtsam/inference/Symbol.h>
28 #include <gtsam/geometry/Point2.h>
29 
30 using namespace std;
31 using namespace gtsam;
32 
33 // Define Types for Linear System Test
35 
36 int main() {
37 
38  // Create the Kalman Filter initialization point
39  Point2 x_initial(0.0, 0.0);
40  SharedDiagonal P_initial = noiseModel::Diagonal::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(Vector2(0.1, 0.1), 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(factor1);
73  traits<Point2>::Print(x1_predict, "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(Vector2(0.25, 0.25), 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(factor2);
94  traits<Point2>::Print(x1_update, "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(factor1);
104  traits<Point2>::Print(x2_predict, "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(factor4);
110  traits<Point2>::Print(x2_update, "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(factor5);
120  traits<Point2>::Print(x3_predict, "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(factor6);
126  traits<Point2>::Print(x3_update, "X3 Update");
127 
128  return 0;
129 }
static const Unit3 z2
Vector2 Point2
Definition: Point2.h:32
Rot2 R(Rot2::fromAngle(0.1))
Point2 LinearMeasurement
Definition: BFloat16.h:88
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
T update(const NoiseModelFactor &measurementFactor)
Class to perform generic Kalman Filtering using nonlinear factor graphs.
static const Unit3 z3
Eigen::VectorXd Vector
Definition: Vector.h:38
T predict(const NoiseModelFactor &motionFactor)
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
traits
Definition: chartTesting.h:28
static Symbol x0('x', 0)
int main()
Eigen::Vector2d Vector2
Definition: Vector.h:42
The quaternion class used to represent 3D orientations and rotations.
Pose3 x1
Definition: testPose3.cpp:663
2D Point
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:62


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:11