130 lines
4.6 KiB
C++
130 lines
4.6 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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;
|
|
}
|