diff --git a/gtsam/linear/KalmanFilter.cpp b/gtsam/linear/KalmanFilter.cpp index 7ea0726bb..89437068a 100644 --- a/gtsam/linear/KalmanFilter.cpp +++ b/gtsam/linear/KalmanFilter.cpp @@ -45,7 +45,21 @@ namespace gtsam { /* ************************************************************************* */ KalmanFilter::KalmanFilter(size_t n, GaussianConditional* density) : - n_(n), I_(eye(n_, n_)),density_(density) { + n_(n), I_(eye(n_, n_)), density_(density) { + } + + /* ************************************************************************* */ + KalmanFilter KalmanFilter::add(GaussianFactor* newFactor) { + + // Create a factor graph + GaussianFactorGraph factorGraph; + + // push back previous solution and new factor + factorGraph.push_back(density_->toFactor()); + factorGraph.push_back(GaussianFactor::shared_ptr(newFactor)); + + // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) + return KalmanFilter(n_, solve(factorGraph)); } /* ************************************************************************* */ @@ -65,7 +79,7 @@ namespace gtsam { // Create a factor graph f(x0), eliminate it into P(x0) GaussianFactorGraph factorGraph; // 0.5*(x-x0)'*inv(Sigma)*(x-x0) - HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0)); + HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0)); factorGraph.push_back(factor); density_.reset(solve(factorGraph)); } @@ -91,27 +105,17 @@ namespace gtsam { } /* ************************************************************************* */ - KalmanFilter KalmanFilter::predict(const Matrix& F, const Matrix& B, const Vector& u, - const SharedDiagonal& model) { - // We will create a small factor graph f1-(x0)-f2-(x1) - // where factor f1 is just the prior from time t0, P(x0) - // and factor f2 is from the motion model - GaussianFactorGraph factorGraph; - - // push back f1 - factorGraph.push_back(density_->toFactor()); + KalmanFilter KalmanFilter::predict(const Matrix& F, const Matrix& B, + const Vector& u, const SharedDiagonal& model) { // The factor related to the motion model is defined as // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T - factorGraph.add(0, -F, 1, I_, B * u, model); - - // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - return KalmanFilter(n_,solve(factorGraph)); + return add(new JacobianFactor(0, -F, 1, I_, B * u, model)); } /* ************************************************************************* */ - KalmanFilter KalmanFilter::predictQ(const Matrix& F, const Matrix& B, const Vector& u, - const Matrix& Q) { + KalmanFilter KalmanFilter::predictQ(const Matrix& F, const Matrix& B, + const Vector& u, const Matrix& Q) { #ifndef NDEBUG int n = F.cols(); @@ -122,57 +126,32 @@ namespace gtsam { assert(Q.cols() == n); #endif - // Same scheme as in predict: - GaussianFactorGraph factorGraph; - factorGraph.push_back(density_->toFactor()); - // The factor related to the motion model is defined as // f2(x_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: // TODO: starts to seem more elaborate than straight-up KF equations? Matrix M = inverse(Q), Ft = trans(F); - Matrix G12 = -Ft*M, G11 = -G12*F, G22 = M; - Vector b = B*u, g2 = M*b, g1 = -Ft*g2; - double f = dot(b,g2); - HessianFactor::shared_ptr factor(new HessianFactor(0, 1, G11, G12, g1, G22, g2, f)); - factorGraph.push_back(factor); - - // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - return KalmanFilter(n_,solve(factorGraph)); + Matrix G12 = -Ft * M, G11 = -G12 * F, G22 = M; + Vector b = B * u, g2 = M * b, g1 = -Ft * g2; + double f = dot(b, g2); + return add(new HessianFactor(0, 1, G11, G12, g1, G22, g2, f)); } /* ************************************************************************* */ - KalmanFilter KalmanFilter::predict2(const Matrix& A0, const Matrix& A1, const Vector& b, - const SharedDiagonal& model) { - - // Same scheme as in predict: - GaussianFactorGraph factorGraph; - factorGraph.push_back(density_->toFactor()); - - // However, now the factor related to the motion model is defined as + KalmanFilter KalmanFilter::predict2(const Matrix& A0, const Matrix& A1, + const Vector& b, const SharedDiagonal& model) { + // Nhe factor related to the motion model is defined as // f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2 - factorGraph.add(0, A0, 1, A1, b, model); - return KalmanFilter(n_,solve(factorGraph)); + return add(new JacobianFactor(0, A0, 1, A1, b, model)); } /* ************************************************************************* */ KalmanFilter KalmanFilter::update(const Matrix& H, const Vector& z, const SharedDiagonal& model) { - // We will create a small factor graph f1-(x0)-f2 - // where factor f1 is the predictive density - // and factor f2 is from the measurement model - GaussianFactorGraph factorGraph; - - // push back f1 - factorGraph.push_back(density_->toFactor()); - // The factor related to the measurements would be defined as // f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T - factorGraph.add(0, H, z, model); - - // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) - return KalmanFilter(n_,solve(factorGraph)); + return add(new JacobianFactor(0, H, z, model)); } /* ************************************************************************* */ diff --git a/gtsam/linear/KalmanFilter.h b/gtsam/linear/KalmanFilter.h index 17db2ee6b..e3b624e68 100644 --- a/gtsam/linear/KalmanFilter.h +++ b/gtsam/linear/KalmanFilter.h @@ -20,6 +20,7 @@ * @author Frank Dellaert */ +#include #include namespace gtsam { @@ -42,6 +43,9 @@ namespace gtsam { /// private constructor KalmanFilter(size_t n, GaussianConditional* density); + /// add a new factor and marginalize to new Kalman filter + KalmanFilter add(GaussianFactor* newFactor); + public: /**