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

const correctness

release/4.3a0
dellaert 2014-03-06 19:39:57 -05:00
parent bb3820780d
commit a1433dbd31
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/linear/KalmanFilter.cpp
View File

@ -53,7 +53,7 @@ KalmanFilter::solve(const GaussianFactorGraph& factorGraph) const {
/* ************************************************************************* */ /* ************************************************************************* */
// Auxiliary function to create a small graph for predict or update and solve // Auxiliary function to create a small graph for predict or update and solve
KalmanFilter::State // KalmanFilter::State //
KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) { KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) const {
// Create a factor graph // Create a factor graph
GaussianFactorGraph factorGraph; GaussianFactorGraph factorGraph;
@ -65,7 +65,7 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) {
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x0, KalmanFilter::State KalmanFilter::init(const Vector& x0,
const SharedDiagonal& P0) { const SharedDiagonal& P0) const {
// Create a factor graph f(x0), eliminate it into P(x0) // Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph; GaussianFactorGraph factorGraph;
@ -74,7 +74,7 @@ KalmanFilter::State KalmanFilter::init(const Vector& x0,
} }
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) { KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) const {
// Create a factor graph f(x0), eliminate it into P(x0) // Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph; GaussianFactorGraph factorGraph;
@ -89,7 +89,7 @@ void KalmanFilter::print(const string& s) const {
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F, KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
const Matrix& B, const Vector& u, const SharedDiagonal& model) { const Matrix& B, const Vector& u, const SharedDiagonal& model) const {
// The factor related to the motion model is defined as // 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_{t},x_{t+1}) = (F*x_{t} + B*u - x_{t+1}) * Q^-1 * (F*x_{t} + B*u - x_{t+1})^T
@ -100,7 +100,7 @@ KalmanFilter::State KalmanFilter::predict(const State& p, const Matrix& F,
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F, KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
const Matrix& B, const Vector& u, const Matrix& Q) { const Matrix& B, const Vector& u, const Matrix& Q) const {
#ifndef NDEBUG #ifndef NDEBUG
DenseIndex n = F.cols(); DenseIndex n = F.cols();
@ -126,7 +126,7 @@ KalmanFilter::State KalmanFilter::predictQ(const State& p, const Matrix& F,
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0, KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
const Matrix& A1, const Vector& b, const SharedDiagonal& model) { const Matrix& A1, const Vector& b, const SharedDiagonal& model) const {
// Nhe factor related to the motion model is defined as // 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_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
Key k = step(p); Key k = step(p);
@ -135,7 +135,7 @@ KalmanFilter::State KalmanFilter::predict2(const State& p, const Matrix& A0,
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H, KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
const Vector& z, const SharedDiagonal& model) { const Vector& z, const SharedDiagonal& model) const {
// The factor related to the measurements would be defined as // The factor related to the measurements would be defined as
// f2 = (h(x_{t}) - z_{t}) * R^-1 * (h(x_{t}) - z_{t})^T // 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 // = (x_{t} - z_{t}) * R^-1 * (x_{t} - z_{t})^T
@ -145,7 +145,7 @@ KalmanFilter::State KalmanFilter::update(const State& p, const Matrix& H,
/* ************************************************************************* */ /* ************************************************************************* */
KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H, KalmanFilter::State KalmanFilter::updateQ(const State& p, const Matrix& H,
const Vector& z, const Matrix& Q) { const Vector& z, const Matrix& Q) const {
Key k = step(p); Key k = step(p);
Matrix M = inverse(Q), Ht = trans(H); Matrix M = inverse(Q), Ht = trans(H);
Matrix G = Ht * M * H; Matrix G = Ht * M * H;

16
gtsam/linear/KalmanFilter.h
View File

@ -62,7 +62,7 @@ private:
const GaussianFactorGraph::Eliminate function_; /** algorithm */ const GaussianFactorGraph::Eliminate function_; /** algorithm */
State solve(const GaussianFactorGraph& factorGraph) const; State solve(const GaussianFactorGraph& factorGraph) const;
State fuse(const State& p, GaussianFactor::shared_ptr newFactor); State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
public: public:
@ -80,10 +80,10 @@ public:
* @param x0 estimate at time 0 * @param x0 estimate at time 0
* @param P0 covariance at time 0, given as a diagonal Gaussian 'model' * @param P0 covariance at time 0, given as a diagonal Gaussian 'model'
*/ */
State init(const Vector& x0, const SharedDiagonal& P0); State init(const Vector& x0, const SharedDiagonal& P0) const;
/// version of init with a full covariance matrix /// version of init with a full covariance matrix
State init(const Vector& x0, const Matrix& P0); State init(const Vector& x0, const Matrix& P0) const;
/// print /// print
void print(const std::string& s = "") const; void print(const std::string& s = "") const;
@ -102,7 +102,7 @@ public:
* and w is zero-mean, Gaussian white noise with covariance Q. * and w is zero-mean, Gaussian white noise with covariance Q.
*/ */
State predict(const State& p, const Matrix& F, const Matrix& B, State predict(const State& p, const Matrix& F, const Matrix& B,
const Vector& u, const SharedDiagonal& modelQ); const Vector& u, const SharedDiagonal& modelQ) const;
/* /*
* Version of predict with full covariance * Version of predict with full covariance
@ -111,7 +111,7 @@ public:
* This version allows more realistic models than a diagonal covariance matrix. * This version allows more realistic models than a diagonal covariance matrix.
*/ */
State predictQ(const State& p, const Matrix& F, const Matrix& B, State predictQ(const State& p, const Matrix& F, const Matrix& B,
const Vector& u, const Matrix& Q); const Vector& u, const Matrix& Q) const;
/** /**
* Predict the state P(x_{t+1}|Z^t) * Predict the state P(x_{t+1}|Z^t)
@ -122,7 +122,7 @@ public:
* with an optional noise model. * with an optional noise model.
*/ */
State predict2(const State& p, const Matrix& A0, const Matrix& A1, State predict2(const State& p, const Matrix& A0, const Matrix& A1,
const Vector& b, const SharedDiagonal& model); const Vector& b, const SharedDiagonal& model) const;
/** /**
* Update Kalman filter with a measurement * Update Kalman filter with a measurement
@ -133,7 +133,7 @@ public:
* In this version, R is restricted to diagonal Gaussians (model parameter) * In this version, R is restricted to diagonal Gaussians (model parameter)
*/ */
State update(const State& p, const Matrix& H, const Vector& z, State update(const State& p, const Matrix& H, const Vector& z,
const SharedDiagonal& model); const SharedDiagonal& model) const;
/* /*
* Version of update with full covariance * Version of update with full covariance
@ -142,7 +142,7 @@ public:
* This version allows more realistic models than a diagonal covariance matrix. * This version allows more realistic models than a diagonal covariance matrix.
*/ */
State updateQ(const State& p, const Matrix& H, const Vector& z, State updateQ(const State& p, const Matrix& H, const Vector& z,
const Matrix& Q); const Matrix& Q) const;
}; };
} // \namespace gtsam } // \namespace gtsam