diff --git a/examples/README b/examples/README index 2342bae7b..a27b6b7ed 100644 --- a/examples/README +++ b/examples/README @@ -2,6 +2,13 @@ This directory contains a number of exapmples that illustrate the use of GTSAM: SimpleRotation: a super-simple example of optimizing a single rotation according to a single prior +Kalman Filter Examples +====================== +elaboratePoint2KalmanFilter: simple linear Kalman filter on a moving 2D point, but done using factor graphs +easyPoint2KalmanFilter: uses the cool generic templated Kalman filter class to do the same +errorStateKalmanFilter: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias +fullStateKalmanFilter: simple 1D example with a full-state filter + 2D Pose SLAM ============ Pose2SLAMExample_easy: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp new file mode 100644 index 000000000..724fab387 --- /dev/null +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * Point2KalmanFilter.cpp + * + * simple linear Kalman filter on a moving 2D point, but done using factor graphs + * + * Created on: Aug 19, 2011 + * @Author: Frank Dellaert + * @Author: Stephen Williams + */ + +using namespace std; +using namespace gtsam; + +int main() { + + // [code below basically does SRIF with LDL] + + // Ground truth example + // Start at origin, move to the right (x-axis): 0,0 0,1 0,2 + // Motion model is just moving to the right (x'-x)^2 + // Measurements are GPS like, (x-z)^2, where z is a 2D measurement + // i.e., we should get 0,0 0,1 0,2 if there is no noise + + // Init state x1 (2D point) at origin, i.e., Bayes net P(x1) + + // Update using z1, P(x1|z1) ~ P(x1)P(z1|x1) ~ f1(x1) f2(x1;z1) + // Hence, make small factor graph f1-(x1)-f2 + // Eliminate to get Bayes net P(x1|z1) + + // Predict x2 P(x2|z1) = \int P(x2|x1) P(x1|z1) + // Make a small factor graph f1-(x1)-f2-(x2) + // where factor f1 is just the posterior from time t1, P(x1|z1) + // where factor f2 is the motion model (x'-x)^2 + // Now, eliminate this in order x1, x2, to get Bayes net P(x1|x2)P(x2) + // so, we just keep the root of the Bayes net, P(x2) which is really P(x2|z1) + + // Update using z2, yielding Bayes net P(x2|z1,z2) + // repeat.... + + // Combined predict-update is more efficient + // Predict x3, update using z3 + // P(x3|z1,z2,z3) ~ P(z3|x3) \int_{x2} P(x3|x2) P(x2|z1,z2) + // form factor graph f3-(x3)-f2-(x2)-f1 + // where f3 ~ P(z3|x3), f2 ~ P(x3|x2), and f1 ~ P(x2|z1,z2) + // Now, eliminate this in order x2, x3, to get Bayes net P(x2|x3)P(x3) + // and again, just keep the root of the Bayes net, P(x3) which is really P(x3|z1,z2,z3) + + + // print out posterior on x3 + + +}