Plan/outline for Kalman filter examples
parent
e62c2bf5e9
commit
fd3acbd2c9
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
||||
}
|
Loading…
Reference in New Issue