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

Thoroughly tested predictQ by comparing with predict2. Uncovered a bug in QR if HessianFactor is involved.

release/4.3a0
Frank Dellaert 2012-01-18 05:06:39 +00:00
parent c75bb0707a
commit 4225f37846
2 changed files with 46 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
gtsam/linear/KalmanFilter.cpp
View File

@ -32,7 +32,7 @@ namespace gtsam {
GaussianConditional* solve(GaussianFactorGraph& factorGraph) { GaussianConditional* solve(GaussianFactorGraph& factorGraph) {
// Solve the factor graph // Solve the factor graph
const bool useQR = true; // make sure we use QR (numerically stable) const bool useQR = false; // make sure we use QR (numerically stable)
GaussianSequentialSolver solver(factorGraph, useQR); GaussianSequentialSolver solver(factorGraph, useQR);
GaussianBayesNet::shared_ptr bayesNet = solver.eliminate(); GaussianBayesNet::shared_ptr bayesNet = solver.eliminate();
@ -124,32 +124,19 @@ namespace gtsam {
// 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
// See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u: // See documentation in HessianFactor, we have A1 = -F, A2 = I_, b = B*u:
Vector b = B*u; // TODO: starts to seem more elaborate than straight-up KF equations?
Matrix M = inverse(Q); Matrix M = inverse(Q), Ft = trans(F);
Matrix Ft = trans(F); Matrix G12 = -Ft*M, G11 = -G12*F, G22 = M;
Matrix G12 = -Ft*M; Vector b = B*u, g2 = M*b, g1 = -Ft*g2;
Matrix G11 = -G12*F;
Matrix G22 = M;
Vector g2 = M*b;
Vector g1 = -Ft*g2;
double f = dot(b,g2); double f = dot(b,g2);
HessianFactor::shared_ptr factor(new HessianFactor(0, 1, G11, G12, g1, G22, g2, f)); HessianFactor::shared_ptr factor(new HessianFactor(0, 1, G11, G12, g1, G22, g2, f));
//#define DEBUG_PREDICTQ
#ifdef DEBUG_PREDICTQ
gtsam::print(b);
gtsam::print(M);
gtsam::print(G11);
gtsam::print(G12);
gtsam::print(G22);
gtsam::print(g1);
gtsam::print(g2);
cout << f << endl;
factor->print("factor");
#endif
factorGraph.push_back(factor); factorGraph.push_back(factor);
#ifdef DEBUG_PREDICTQ
Matrix AbtAb = factorGraph.denseHessian();
gtsam::print(AbtAb);
#endif
// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1) // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
density_.reset(solve(factorGraph)); density_.reset(solve(factorGraph));
} }
@ -165,13 +152,10 @@ namespace gtsam {
// However, now the factor related to the motion model is defined as // However, now the 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
factorGraph.add(0, A0, 1, A1, b, model); factorGraph.add(0, A0, 1, A1, b, model);
#ifdef DEBUG_PREDICTQ
gtsam::print(Matrix(trans(A0)*A0)); #ifdef DEBUG_PREDICTQ
gtsam::print(Matrix(trans(A0)*A1)); Matrix AbtAb = factorGraph.denseHessian();
gtsam::print(Matrix(trans(A1)*A1)); gtsam::print(AbtAb);
gtsam::print(Matrix(trans(A0)*b));
gtsam::print(Matrix(trans(A1)*b));
cout << dot(b,b) << endl;
#endif #endif
density_.reset(solve(factorGraph)); density_.reset(solve(factorGraph));

34
gtsam/linear/tests/testKalmanFilter.cpp
View File

@ -104,8 +104,8 @@ TEST( KalmanFilter, linear1 ) {
EXPECT(assert_equal(expected1,kalmanFilter.mean())); EXPECT(assert_equal(expected1,kalmanFilter.mean()));
EXPECT(assert_equal(I11,kalmanFilter.information())); EXPECT(assert_equal(I11,kalmanFilter.information()));
// Run iteration 2 // Run iteration 2 (with full covariance)
kalmanFilter.predict(F, B, u, modelQ); kalmanFilter.predictQ(F, B, u, Q);
EXPECT(assert_equal(expected2,kalmanFilter.mean())); EXPECT(assert_equal(expected2,kalmanFilter.mean()));
kalmanFilter.update(H,z2,modelR); kalmanFilter.update(H,z2,modelR);
EXPECT(assert_equal(expected2,kalmanFilter.mean())); EXPECT(assert_equal(expected2,kalmanFilter.mean()));
@ -117,6 +117,36 @@ TEST( KalmanFilter, linear1 ) {
EXPECT(assert_equal(expected3,kalmanFilter.mean())); EXPECT(assert_equal(expected3,kalmanFilter.mean()));
} }
/* ************************************************************************* */
TEST( KalmanFilter, predict ) {
// Create dynamics model
Matrix F = Matrix_(2,2, 1.0,0.1, 0.2,1.1);
Matrix B = Matrix_(2,3, 1.0,0.1,0.2, 1.1,1.2,0.8);
Vector u = Vector_(3, 1.0, 0.0, 2.0);
Matrix R = Matrix_(2,2, 1.0,0.0, 0.0,3.0);
Matrix M = trans(R)*R;
Matrix Q = inverse(M);
// Create the Kalman Filter initialization point
State x_initial(0.0,0.0);
SharedDiagonal P_initial = noiseModel::Isotropic::Sigma(2,1);
// Create two KalmanFilter objects
KalmanFilter kalmanFilter1(x_initial, P_initial);
KalmanFilter kalmanFilter2(x_initial, P_initial);
// Ensure predictQ and predict2 give same answer for non-trivial inputs
kalmanFilter1.predictQ(F, B, u, Q);
// We have A1 = -F, A2 = I_, b = B*u:
Matrix A1 = -R*F, A2 = R;
Vector b = R*B*u;
SharedDiagonal nop = noiseModel::Isotropic::Sigma(2, 1.0);
kalmanFilter2.predict2(A1,A2,b,nop);
EXPECT(assert_equal(kalmanFilter1.mean(),kalmanFilter2.mean()));
EXPECT(assert_equal(kalmanFilter1.covariance(),kalmanFilter2.covariance()));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;