better docs
Frank Dellaert
parent
c93f752e74
commit
a425e6e3d4
|
|
@ -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:
|
||||
|
||||
/**
|
||||
* 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 the state.
|
||||
const Matrix I_; ///< Identity matrix of size \f$ n \times n \f$.
|
||||
const GaussianFactorGraph::Eliminate
|
||||
function_; ///< Elimination algorithm used.
|
||||
|
||||
const size_t n_; /** dimensionality of state */
|
||||
const Matrix I_; /** identity matrix of size n*n */
|
||||
const GaussianFactorGraph::Eliminate function_; /** algorithm */
|
||||
|
||||
/**
|
||||
* 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
|
||||
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)) {
|
||||
}
|
||||
/**
|
||||
* 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 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'
|
||||
* 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;
|
||||
|
||||
/*
|
||||
* 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;
|
||||
|
||||
/**
|
||||
* 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;
|
||||
|
||||
/*
|
||||
* 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
|
||||
Loading…
Reference in New Issue