diff --git a/.cproject b/.cproject index adefb5a6c..dd3c433c5 100644 --- a/.cproject +++ b/.cproject @@ -105,16 +105,6 @@ - - - - - - - - - - @@ -125,36 +115,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -187,16 +147,6 @@ - - - - - - - - - - @@ -207,36 +157,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -270,16 +190,6 @@ - - - - - - - - - - @@ -290,36 +200,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -1052,6 +932,14 @@ true true + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + make -j2 @@ -1904,6 +1792,14 @@ true true + + make + -j2 + tests/testKalmanFilter.run + true + true + true + make -j2 @@ -2183,6 +2079,14 @@ true true + + make + -j2 + doxygen-doc + true + true + true + make -j5 diff --git a/gtsam.h b/gtsam.h index 15361cd5a..a4110ebd5 100644 --- a/gtsam.h +++ b/gtsam.h @@ -57,6 +57,14 @@ // Everything is in the gtsam namespace, so we avoid copying everything in using namespace gtsam; +//************************************************************************* +// base +//************************************************************************* + +//************************************************************************* +// geometry +//************************************************************************* + class Point2 { Point2(); Point2(double x, double y); @@ -197,6 +205,16 @@ class Pose3 { Rot3 rotation() const; }; +//************************************************************************* +// inference +//************************************************************************* + + + +//************************************************************************* +// linear +//************************************************************************* + class SharedGaussian { SharedGaussian(Matrix covariance); void print(string s) const; @@ -292,7 +310,8 @@ class GaussianSequentialSolver { }; class KalmanFilter { - KalmanFilter(Vector x, const SharedDiagonal& model); + KalmanFilter(Vector x0, const SharedDiagonal& P0); + KalmanFilter(Vector x0, const Matrix& P0); void print(string s) const; Vector mean() const; Matrix information() const; @@ -302,6 +321,10 @@ class KalmanFilter { void update(Matrix H, Vector z, const SharedDiagonal& model); }; +//************************************************************************* +// nonlinear +//************************************************************************* + class Ordering { Ordering(); void print(string s) const; @@ -309,6 +332,10 @@ class Ordering { void push_back(string key); }; +//************************************************************************* +// slam +//************************************************************************* + // Planar SLAM example domain #include namespace planarSLAM { diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 8b37f8cb3..76ea27ccc 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -23,6 +23,8 @@ #include #include #include +#include +#include namespace gtsam { @@ -42,12 +44,24 @@ namespace gtsam { } /* ************************************************************************* */ - KalmanFilter::KalmanFilter(const Vector& x, const SharedDiagonal& model) : + KalmanFilter::KalmanFilter(const Vector& x0, const SharedDiagonal& P0) : + n_(x0.size()), I_(eye(n_, n_)) { + + // Create a factor graph f(x0), eliminate it into P(x0) + GaussianFactorGraph factorGraph; + factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma + density_.reset(solve(factorGraph)); + } + + /* ************************************************************************* */ + KalmanFilter::KalmanFilter(const Vector& x, const Matrix& P0) : n_(x.size()), I_(eye(n_, n_)) { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; - factorGraph.add(0, I_, x, model); + // 0.5*(x-x0)'*inv(Sigma)*(x-x0) + HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0)); + factorGraph.push_back(factor); density_.reset(solve(factorGraph)); } diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 5d75fd3c1..61a49c621 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -25,6 +25,7 @@ namespace gtsam { class SharedDiagonal; + class SharedGaussian; /** * Linear Kalman Filter @@ -43,10 +44,18 @@ namespace gtsam { /** * Constructor from prior density at time k=0 * In Kalman Filter notation, these are is x_{0|0} and P_{0|0} - * @param x estimate at time 0 - * @param P covariance at time 0, restricted to diagonal Gaussian 'model' for now + * @param x0 estimate at time 0 + * @param P0 covariance at time 0, given as a diagonal Gaussian 'model' */ - KalmanFilter(const Vector& x, const SharedDiagonal& model); + KalmanFilter(const Vector& x0, const SharedDiagonal& P0); + + /** + * Constructor from prior density at time k=0 + * In Kalman Filter notation, these are is x_{0|0} and P_{0|0} + * @param x0 estimate at time 0 + * @param P0 covariance at time 0, full Gaussian + */ + KalmanFilter(const Vector& x0, const Matrix& P0); /// print void print(const std::string& s="") const { diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index 4ceda0cfb..281c8ac7e 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include using namespace std; @@ -34,6 +36,25 @@ struct State: Vector { } }; +/* ************************************************************************* */ +TEST( KalmanFilter, constructor ) { + // Create the Kalman Filter initialization point + State x_initial(0.0,0.0); + SharedDiagonal P1 = noiseModel::Isotropic::Sigma(2,0.1); + + // Create an KalmanFilter object + KalmanFilter kf1(x_initial, P1); + Matrix Sigma = Matrix_(2,2,0.01,0.0,0.0,0.01); + EXPECT(assert_equal(Sigma,kf1.covariance())); + + // Create one with a sharedGaussian + KalmanFilter kf2(x_initial, Sigma); + EXPECT(assert_equal(Sigma,kf2.covariance())); + + // Now make sure both agree + EXPECT(assert_equal(kf1.covariance(),kf2.covariance())); +} + /* ************************************************************************* */ TEST( KalmanFilter, linear1 ) {