Skip to content

Commit

Permalink
Merge pull request #1924 from borglab/feature/kf_docs
Browse files Browse the repository at this point in the history 
Kalman filter update
  • Loading branch information
@dellaert
dellaert authored Dec 11, 2024
2 parents b76c6d8 + 1849c7b commit a648bd8
Show file tree
Hide file tree
Showing 2 changed files with 219 additions and 128 deletions.
129 changes: 77 additions & 52 deletions gtsam/linear/KalmanFilter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,29 +20,44 @@
* @author Frank Dellaert
*/


#include <gtsam/linear/KalmanFilter.h>

#include <gtsam/base/Testable.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/Testable.h>

#ifndef NDEBUG
#include <cassert>
#endif

using namespace std;
// In the code below we often get a covariance matrix Q, and we need to create a
// JacobianFactor with cost 0.5 * |Ax - b|^T Q^{-1} |Ax - b|. Factorizing Q as
// Q = L L^T
// and hence
// Q^{-1} = L^{-T} L^{-1} = M^T M, with M = L^{-1}
// We then have
// 0.5 * |Ax - b|^T Q^{-1} |Ax - b|
// = 0.5 * |Ax - b|^T M^T M |Ax - b|
// = 0.5 * |MAx - Mb|^T |MAx - Mb|
// The functor below efficiently multiplies with M by calling L.solve().
namespace {
using Matrix = gtsam::Matrix;
struct InverseL : public Eigen::LLT<Matrix> {
InverseL(const Matrix& Q) : Eigen::LLT<Matrix>(Q) {}
Matrix operator*(const Matrix& A) const { return matrixL().solve(A); }
};
} // namespace

namespace gtsam {

/* ************************************************************************* */
// Auxiliary function to solve factor graph and return pointer to root conditional
KalmanFilter::State //
KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {

// Auxiliary function to solve factor graph and return pointer to root
// conditional
KalmanFilter::State KalmanFilter::solve(
const GaussianFactorGraph& factorGraph) const {
// Eliminate the graph using the provided Eliminate function
Ordering ordering(factorGraph.keys());
const auto bayesNet = //
factorGraph.eliminateSequential(ordering, function_);
const auto bayesNet = factorGraph.eliminateSequential(ordering, function_);

// As this is a filter, all we need is the posterior P(x_t).
// This is the last GaussianConditional in the resulting BayesNet
Expand All @@ -52,9 +67,8 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {

/* ************************************************************************* */
// Auxiliary function to create a small graph for predict or update and solve
KalmanFilter::State //
KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {

KalmanFilter::State KalmanFilter::fuse(
const State& p, GaussianFactor::shared_ptr newFactor) const {
// Create a factor graph
GaussianFactorGraph factorGraph;
factorGraph.push_back(p);
Expand All @@ -66,96 +80,107 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x0,
const SharedDiagonal& P0) const {

const SharedDiagonal& P0) const {
// Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph;
factorGraph.emplace_shared<JacobianFactor>(0, I_, x0, P0); // |x-x0|^2_diagSigma
factorGraph.emplace_shared<JacobianFactor>(0, I_, x0,
P0); // |x-x0|^2_P0
return solve(factorGraph);
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const {

KalmanFilter::State KalmanFilter::init(const Vector& x0,
const Matrix& P0) const {
// Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph;
factorGraph.emplace_shared<HessianFactor>(0, x, P0); // 0.5*(x-x0)'*inv(Sigma)*(x-x0)

// Perform Cholesky decomposition of P0 = LL^T
InverseL M(P0);

// Premultiply I and x0 with M=L^{-1}
const Matrix A = M * I_; // = M
const Vector b = M * x0; // = M*x0
factorGraph.emplace_shared<JacobianFactor>(0, A, b);
return solve(factorGraph);
}

/* ************************************************************************* */
void KalmanFilter::print(const string& s) const {
cout << "KalmanFilter " << s << ", dim = " << n_ << endl;
void KalmanFilter::print(const std::string& s) const {
std::cout << "KalmanFilter " << s << ", dim = " << n_ << std::endl;
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
const Matrix& B, const Vector& u, const SharedDiagonal& model) const {

const Matrix& B, const Vector& u,
const SharedDiagonal& model) const {
// 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
// f2(x_{k},x_{k+1}) =
// (F*x_{k} + B*u - x_{k+1}) * Q^-1 * (F*x_{k} + B*u - x_{k+1})^T
Key k = step(p);
return fuse(p,
std::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
std::make_shared<JacobianFactor>(k, -F, k + 1, I_, B * u, model));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
const Matrix& B, const Vector& u, const Matrix& Q) const {

const Matrix& B, const Vector& u,
const Matrix& Q) const {
#ifndef NDEBUG
DenseIndex n = F.cols();
const DenseIndex n = dim();
assert(F.cols() == n);
assert(F.rows() == n);
assert(B.rows() == n);
assert(B.cols() == u.size());
assert(Q.rows() == n);
assert(Q.cols() == n);
#endif

// 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 = Q.inverse(), 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);
Key k = step(p);
return fuse(p,
std::make_shared<HessianFactor>(k, k + 1, G11, G12, g1, G22, g2, f));
// Perform Cholesky decomposition of Q = LL^T
InverseL M(Q);

// Premultiply -F, I, and B * u with M=L^{-1}
const Matrix A1 = -(M * F);
const Matrix A2 = M * I_;
const Vector b = M * (B * u);
return predict2(p, A1, A2, b);
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
const Matrix& A1, const Vector& b, const SharedDiagonal& model) const {
const Matrix& A1, const Vector& b,
const SharedDiagonal& model) const {
// 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
// f2(x_{k},x_{k+1}) = |A0*x_{k} + A1*x_{k+1} - b|^2
Key k = step(p);
return fuse(p, std::make_shared<JacobianFactor>(k, A0, k + 1, A1, b, model));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
const Vector& z, const SharedDiagonal& model) const {
const Vector& z,
const SharedDiagonal& model) const {
// 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
// f2 = (h(x_{k}) - z_{k}) * R^-1 * (h(x_{k}) - z_{k})^T
// = (x_{k} - z_{k}) * R^-1 * (x_{k} - z_{k})^T
Key k = step(p);
return fuse(p, std::make_shared<JacobianFactor>(k, H, z, model));
}

/* ************************************************************************* */
KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
const Vector& z, const Matrix& Q) const {
const Vector& z,
const Matrix& Q) const {
Key k = step(p);
Matrix M = Q.inverse(), Ht = trans(H);
Matrix G = Ht * M * H;
Vector g = Ht * M * z;
double f = dot(z, M * z);
return fuse(p, std::make_shared<HessianFactor>(k, G, g, f));

// Perform Cholesky decomposition of Q = LL^T
InverseL M(Q);

// Pre-multiply H and z with M=L^{-1}, respectively
const Matrix A = M * H;
const Vector b = M * z;
return fuse(p, std::make_shared<JacobianFactor>(k, A, b));
}

/* ************************************************************************* */

} // \namespace gtsam

} // namespace gtsam
Loading

0 comments on commit a648bd8

Please sign in to comment.