12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1924 from borglab/feature/kf_docs 

Kalman filter update
release/4.3a0
Frank Dellaert 2024-12-11 15:24:54 -05:00 committed by GitHub
parent b76c6d8250 1849c7b424
commit a648bd84c4
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 223 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
gtsam/linear/KalmanFilter.cpp
View File

@ -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
@ -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);
@ -66,45 +80,54 @@ 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());
@ -112,50 +135,52 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
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

226
gtsam/linear/KalmanFilter.h
View File

@ -11,10 +11,10 @@
/**
* @file KalmanFilter.h
* @brief Simple linear Kalman filter. Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.
* @brief Simple linear Kalman filter implemented using factor graphs, i.e.,
* performs Cholesky or QR-based SRIF (Square-Root Information Filter).
* @date Sep 3, 2011
* @author Stephen Williams
* @author Frank Dellaert
* @authors Stephen Williams, Frank Dellaert
*/
#pragma once
@ -32,120 +32,186 @@ namespace gtsam {
/**
* Kalman Filter class
*
* Knows how to maintain a Gaussian density under linear-Gaussian motion and
* measurement models. It uses the square-root information form, as usual in GTSAM.
* Maintains a Gaussian density under linear-Gaussian motion and
* measurement models using the square-root information form.
*
* The filter is functional, in that it does not have state: you call init() to create
* an initial state, then predict() and update() that create new states out of an old state.
* The filter is functional; it does not maintain internal state. Instead:
* - Use `init()` to create an initial filter state,
* - Call `predict()` and `update()` to create new states.
*/
class GTSAM_EXPORT KalmanFilter {
public:
public:
/**
* This Kalman filter is a Square-root Information filter
* The type below allows you to specify the factorization variant.
* @enum Factorization
* @brief Specifies the factorization variant to use.
*/
enum Factorization {
QR, CHOLESKY
};
enum Factorization { QR, CHOLESKY };
/**
* The Kalman filter state is simply a GaussianDensity
* @typedef State
* @brief The Kalman filter state, represented as a shared pointer to a
* GaussianDensity.
*/
typedef GaussianDensity::shared_ptr State;
private:
const size_t n_; /** dimensionality of state */
const Matrix I_; /** identity matrix of size n*n */
const GaussianFactorGraph::Eliminate function_; /** algorithm */
State solve(const GaussianFactorGraph& factorGraph) const;
State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
public:
// Constructor
KalmanFilter(size_t n, Factorization method =
KALMANFILTER_DEFAULT_FACTORIZATION) :
n_(n), I_(Matrix::Identity(n_, n_)), function_(
method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
GaussianFactorGraph::Eliminate(EliminateCholesky)) {
}
private:
const size_t n_; ///< Dimensionality of the state.
const Matrix I_; ///< Identity matrix of size \f$ n \times n \f$.
const GaussianFactorGraph::Eliminate
function_; ///< Elimination algorithm used.
/**
* Create initial state, i.e., prior density at time k=0
* In Kalman Filter notation, these are x_{0|0} and P_{0|0}
* @param x0 estimate at time 0
* @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
* Solve the factor graph.
* @param factorGraph The Gaussian factor graph to solve.
* @return The resulting Kalman filter state.
*/
State solve(const GaussianFactorGraph& factorGraph) const;
/**
* Fuse two states.
* @param p The prior state.
* @param newFactor The new factor to incorporate.
* @return The resulting fused state.
*/
State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
public:
/**
* Constructor.
* @param n Dimensionality of the state.
* @param method Factorization method (default: QR unless compile-flag set).
*/
KalmanFilter(size_t n,
Factorization method = KALMANFILTER_DEFAULT_FACTORIZATION)
: n_(n),
I_(Matrix::Identity(n_, n_)),
function_(method == QR
? GaussianFactorGraph::Eliminate(EliminateQR)
: GaussianFactorGraph::Eliminate(EliminateCholesky)) {}
/**
* Create the initial state (prior density at time \f$ k=0 \f$).
*
* In Kalman Filter notation:
* - \f$ x_{0|0} \f$: Initial state estimate.
* - \f$ P_{0|0} \f$: Initial covariance matrix.
*
* @param x0 Estimate of the state at time 0 (\f$ x_{0|0} \f$).
* @param P0 Covariance matrix (\f$ P_{0|0} \f$), given as a diagonal Gaussian
* model.
* @return Initial Kalman filter state.
*/
State init(const Vector& x0, const SharedDiagonal& P0) const;
/// version of init with a full covariance matrix
/**
* Create the initial state with a full covariance matrix.
* @param x0 Initial state estimate.
* @param P0 Full covariance matrix.
* @return Initial Kalman filter state.
*/
State init(const Vector& x0, const Matrix& P0) const;
/// print
/**
* Print the Kalman filter details.
* @param s Optional string prefix.
*/
void print(const std::string& s = "") const;
/** Return step index k, starts at 0, incremented at each predict. */
static Key step(const State& p) {
return p->firstFrontalKey();
}
/**
* Return the step index \f$ k \f$ (starts at 0, incremented at each predict
* step).
* @param p The current state.
* @return Step index.
*/
static Key step(const State& p) { return p->firstFrontalKey(); }
/**
* Predict the state P(x_{t+1}|Z^t)
* In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
* Details and parameters:
* In a linear Kalman Filter, the motion model is f(x_{t}) = F*x_{t} + B*u_{t} + w
* where F is the state transition model/matrix, B is the control input model,
* and w is zero-mean, Gaussian white noise with covariance Q.
* Predict the next state \f$ P(x_{k+1}|Z^k) \f$.
*
* In Kalman Filter notation:
* - \f$ x_{k+1|k} \f$: Predicted state.
* - \f$ P_{k+1|k} \f$: Predicted covariance.
*
* Motion model:
* \f[
* x_{k+1} = F \cdot x_k + B \cdot u_k + w
* \f]
* where \f$ w \f$ is zero-mean Gaussian noise with covariance \f$ Q \f$.
*
* @param p Previous state (\f$ x_k \f$).
* @param F State transition matrix (\f$ F \f$).
* @param B Control input matrix (\f$ B \f$).
* @param u Control vector (\f$ u_k \f$).
* @param modelQ Noise model (\f$ Q \f$, diagonal Gaussian).
* @return Predicted state (\f$ x_{k+1|k} \f$).
*/
State predict(const State& p, const Matrix& F, const Matrix& B,
const Vector& u, const SharedDiagonal& modelQ) const;
const Vector& u, const SharedDiagonal& modelQ) const;
/*
* Version of predict with full covariance
* Q is normally derived as G*w*G^T where w models uncertainty of some
* physical property, such as velocity or acceleration, and G is derived from physics.
* This version allows more realistic models than a diagonal covariance matrix.
/**
* Predict the next state with a full covariance matrix.
*
*@note Q is normally derived as G*w*G^T where w models uncertainty of some
* physical property, such as velocity or acceleration, and G is derived from
* physics. This version allows more realistic models than a diagonal matrix.
*
* @param p Previous state.
* @param F State transition matrix.
* @param B Control input matrix.
* @param u Control vector.
* @param Q Full covariance matrix (\f$ Q \f$).
* @return Predicted state.
*/
State predictQ(const State& p, const Matrix& F, const Matrix& B,
const Vector& u, const Matrix& Q) const;
const Vector& u, const Matrix& Q) const;
/**
* Predict the state P(x_{t+1}|Z^t)
* In Kalman Filter notation, this is x_{t+1|t} and P_{t+1|t}
* After the call, that is the density that can be queried.
* Details and parameters:
* This version of predict takes GaussianFactor motion model [A0 A1 b]
* with an optional noise model.
* Predict the next state using a GaussianFactor motion model.
* @param p Previous state.
* @param A0 Factor matrix.
* @param A1 Factor matrix.
* @param b Constant term vector.
* @param model Noise model (optional).
* @return Predicted state.
*/
State predict2(const State& p, const Matrix& A0, const Matrix& A1,
const Vector& b, const SharedDiagonal& model) const;
const Vector& b, const SharedDiagonal& model = nullptr) const;
/**
* Update Kalman filter with a measurement
* For the Kalman Filter, the measurement function, h(x_{t}) = z_{t}
* will be of the form h(x_{t}) = H*x_{t} + v
* where H is the observation model/matrix, and v is zero-mean,
* Gaussian white noise with covariance R.
* Update the Kalman filter with a measurement.
*
* Observation model:
* \f[
* z_k = H \cdot x_k + v
* \f]
* where \f$ v \f$ is zero-mean Gaussian noise with covariance R.
* In this version, R is restricted to diagonal Gaussians (model parameter)
*
* @param p Previous state.
* @param H Observation matrix.
* @param z Measurement vector.
* @param model Noise model (diagonal Gaussian).
* @return Updated state.
*/
State update(const State& p, const Matrix& H, const Vector& z,
const SharedDiagonal& model) const;
const SharedDiagonal& model) const;
/*
* Version of update with full covariance
* Q is normally derived as G*w*G^T where w models uncertainty of some
* physical property, such as velocity or acceleration, and G is derived from physics.
* This version allows more realistic models than a diagonal covariance matrix.
/**
* Update the Kalman filter with a measurement using a full covariance matrix.
* @param p Previous state.
* @param H Observation matrix.
* @param z Measurement vector.
* @param R Full covariance matrix.
* @return Updated state.
*/
State updateQ(const State& p, const Matrix& H, const Vector& z,
const Matrix& Q) const;
const Matrix& R) const;
/**
* Return the dimensionality of the state.
* @return Dimensionality of the state.
*/
size_t dim() const { return n_; }
};
} // \namespace gtsam
/* ************************************************************************* */
} // namespace gtsam