diff --git a/.cproject b/.cproject index 1bbf1328c..42f61fa16 100644 --- a/.cproject +++ b/.cproject @@ -375,14 +375,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -409,6 +401,7 @@ make + tests/testBayesTree.run true false @@ -416,6 +409,7 @@ make + testBinaryBayesNet.run true false @@ -463,6 +457,7 @@ make + testSymbolicBayesNet.run true false @@ -470,6 +465,7 @@ make + tests/testSymbolicFactor.run true false @@ -477,6 +473,7 @@ make + testSymbolicFactorGraph.run true false @@ -492,11 +489,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -523,7 +529,6 @@ make - testGraph.run true false @@ -595,7 +600,6 @@ make - testInference.run true false @@ -603,7 +607,6 @@ make - testGaussianFactor.run true false @@ -611,7 +614,6 @@ make - testJunctionTree.run true false @@ -619,7 +621,6 @@ make - testSymbolicBayesNet.run true false @@ -627,7 +628,6 @@ make - testSymbolicFactorGraph.run true false @@ -721,7 +721,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -729,6 +737,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -769,15 +785,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -785,14 +793,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -1107,6 +1107,7 @@ make + testErrors.run true false @@ -1506,7 +1507,6 @@ make - testSimulated2DOriented.run true false @@ -1546,7 +1546,6 @@ make - testSimulated2D.run true false @@ -1554,7 +1553,6 @@ make - testSimulated3D.run true false @@ -1802,7 +1800,6 @@ make - tests/testGaussianISAM2 true false @@ -1824,6 +1821,46 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + make -j2 @@ -1920,54 +1957,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - check - true - true - true - make -j2 @@ -2008,6 +1997,22 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + diff --git a/gtsam.h b/gtsam.h index ba6c10006..cde64b439 100644 --- a/gtsam.h +++ b/gtsam.h @@ -114,6 +114,16 @@ class GaussianFactorGraph { Matrix sparseJacobian_() const; }; +class KalmanFilter { + KalmanFilter(Vector x, const SharedDiagonal& model); + void print(string s) const; + Vector mean() const; + Matrix information() const; + Matrix covariance() const; + void predict(Matrix F, Matrix B, Vector u, const SharedDiagonal& model); + void update(Matrix H, Vector z, const SharedDiagonal& model); +}; + class Landmark2 { Landmark2(); Landmark2(double x, double y); diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 6e8206b5e..5aa697bec 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -43,6 +43,15 @@ namespace gtsam { */ KalmanFilter(const Vector& x, const SharedDiagonal& model); + /// print + void print(const std::string& s="") const { + std::cout << s << "\n"; + Vector m = mean(); + Matrix P = covariance(); + gtsam::print(m,"mean: "); + gtsam::print(P,"covariance: "); + } + /** Return mean of posterior P(x|Z) at given all measurements Z */ Vector mean() const; diff --git a/tests/matlab/testKalmanFilter.m b/tests/matlab/testKalmanFilter.m new file mode 100644 index 000000000..4a5b5694d --- /dev/null +++ b/tests/matlab/testKalmanFilter.m @@ -0,0 +1,77 @@ +% /* ---------------------------------------------------------------------------- +% +% * 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 testKalmanFilter.cpp +% * @brief Test simple linear Kalman filter on a moving 2D point +% * @date Sep 3, 2011 +% * @author Stephen Williams +% * @author Frank Dellaert +% * @author Richard Roberts +% */ + +%% Create the controls and measurement properties for our example +F = eye(2,2); +B = eye(2,2); +u = [1.0; 0.0]; +modelQ = SharedDiagonal([0.1;0.1]); +Q = 0.01*eye(2,2); +H = eye(2,2); +z1 = [1.0, 0.0]'; +z2 = [2.0, 0.0]'; +z3 = [3.0, 0.0]'; +modelR = SharedDiagonal([0.1;0.1]); +R = 0.01*eye(2,2); + +%% Create the set of expected output TestValues +expected0 = [0.0, 0.0]'; +P00 = 0.01*eye(2,2); + +expected1 = [1.0, 0.0]'; +P01 = P00 + Q; +I11 = inv(P01) + inv(R); + +expected2 = [2.0, 0.0]'; +P12 = inv(I11) + Q; +I22 = inv(P12) + inv(R); + +expected3 = [3.0, 0.0]'; +P23 = inv(I22) + Q; +I33 = inv(P23) + inv(R); + +%% Create the Kalman Filter initialization point +x_initial = [0.0;0.0]; +P_initial = SharedDiagonal([0.1;0.1]); + +%% Create an KalmanFilter object +kalmanFilter = KalmanFilter(x_initial, P_initial) +EQUALITY('expected0,kalmanFilter.mean', expected0,kalmanFilter.mean); +EQUALITY('expected0,kalmanFilter.mean', P00,kalmanFilter.covariance); + +%% Run iteration 1 +kalmanFilter.predict(F, B, u, modelQ); +EQUALITY('expected1,kalmanFilter.mean', expected1,kalmanFilter.mean); +EQUALITY('P01,kalmanFilter.covariance', P01,kalmanFilter.covariance); +kalmanFilter.update(H,z1,modelR); +EQUALITY('expected1,kalmanFilter.mean', expected1,kalmanFilter.mean); +EQUALITY('I11,kalmanFilter.information', I11,kalmanFilter.information); + +%% Run iteration 2 +kalmanFilter.predict(F, B, u, modelQ); +EQUALITY('expected2,kalmanFilter.mean', expected2,kalmanFilter.mean); +kalmanFilter.update(H,z2,modelR); +EQUALITY('expected2,kalmanFilter.mean', expected2,kalmanFilter.mean); + +%% Run iteration 3 +kalmanFilter.predict(F, B, u, modelQ); +EQUALITY('expected3,kalmanFilter.mean', expected3,kalmanFilter.mean); +kalmanFilter.update(H,z3,modelR); +EQUALITY('expected3,kalmanFilter.mean', expected3,kalmanFilter.mean);