diff --git a/gtsam.h b/gtsam.h index 030a6ea01..b8aba73ff 100644 --- a/gtsam.h +++ b/gtsam.h @@ -370,6 +370,8 @@ class KalmanFilter { const gtsam::SharedDiagonal& model); gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, Vector z, const gtsam::SharedDiagonal& model); + gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, Vector z, + Matrix Q); }; //************************************************************************* diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 224af162b..56a2692f3 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -145,7 +145,7 @@ namespace gtsam { } /* ************************************************************************* */ - KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, const Vector& z, + KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, const Vector& z, const Matrix& Q) { Index k = step(p); Matrix M = inverse(Q), Ht = trans(H); diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index a5c21f117..bff512b95 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -143,7 +143,7 @@ namespace gtsam { * physical property, such as velocity or acceleration, and G is derived from physics. * This version allows more realistic models than a diagonal covariance matrix. */ - State update(const State& p, const Matrix& H, const Vector& z, + State updateQ(const State& p, const Matrix& H, const Vector& z, const Matrix& Q); }; diff --git a/gtsam/linear/tests/testKalmanFilter.cpp b/gtsam/linear/tests/testKalmanFilter.cpp index f00ac8290..3fb8a91c6 100644 --- a/gtsam/linear/tests/testKalmanFilter.cpp +++ b/gtsam/linear/tests/testKalmanFilter.cpp @@ -272,8 +272,8 @@ TEST( KalmanFilter, QRvsCholesky ) { // do the above update again, this time with a full Matrix Q Matrix modelQ = diag(emul(sigmas,sigmas)); - KalmanFilter::State pa3 = kfa.update(pa, H, z, modelQ); - KalmanFilter::State pb3 = kfb.update(pb, H, z, modelQ); + KalmanFilter::State pa3 = kfa.updateQ(pa, H, z, modelQ); + KalmanFilter::State pb3 = kfb.updateQ(pb, H, z, modelQ); // Check that they yield the same mean and information matrix EXPECT(assert_equal(pa3->mean(), pb3->mean()));