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
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
GaussianFactorGraph factorGraph;
@ -65,7 +65,7 @@ KalmanFilter::fuse(const State& p, GaussianFactor::shared_ptr newFactor) {
/* ************************************************************************* */
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)
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)
GaussianFactorGraph factorGraph;
@ -89,7 +89,7 @@ void KalmanFilter::print(const string& s) const {
/* ************************************************************************* */
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
// 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,
const Matrix& B, const Vector& u, const Matrix& Q) {
const Matrix& B, const Vector& u, const Matrix& Q) const {
#ifndef NDEBUG
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,
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
// f2(x_{t},x_{t+1}) = |A0*x_{t} + A1*x_{t+1} - b|^2
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,
const Vector& z, const SharedDiagonal& model) {
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
@ -145,7 +145,7 @@ KalmanFilter::State KalmanFilter::update(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);
Matrix M = inverse(Q), Ht = trans(H);
Matrix G = Ht * M * H;

16
gtsam/linear/KalmanFilter.h
View File

@ -62,7 +62,7 @@ private:
const GaussianFactorGraph::Eliminate function_; /** algorithm */
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:
@ -80,10 +80,10 @@ public:
* @param x0 estimate at time 0
* @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
State init(const Vector& x0, const Matrix& P0);
State init(const Vector& x0, const Matrix& P0) const;
/// print
void print(const std::string& s = "") const;
@ -102,7 +102,7 @@ public:
* and w is zero-mean, Gaussian white noise with covariance Q.
*/
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
@ -111,7 +111,7 @@ public:
* This version allows more realistic models than a diagonal covariance matrix.
*/
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)
@ -122,7 +122,7 @@ public:
* with an optional noise model.
*/
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
@ -133,7 +133,7 @@ public:
* In this version, R is restricted to diagonal Gaussians (model parameter)
*/
State update(const State& p, const Matrix& H, const Vector& z,
const SharedDiagonal& model);
const SharedDiagonal& model) const;
/*
* Version of update with full covariance
@ -142,7 +142,7 @@ public:
* This version allows more realistic models than a diagonal covariance matrix.
*/
State updateQ(const State& p, const Matrix& H, const Vector& z,
const Matrix& Q);
const Matrix& Q) const;
};
} // \namespace gtsam