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

Made sure worked with non-diagonal noise model

release/4.3a0
Frank Dellaert 2012-01-18 05:08:52 +00:00
parent 4225f37846
commit fb3a959193
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -124,7 +124,7 @@ TEST( KalmanFilter, predict ) {
Matrix F = Matrix_(2,2, 1.0,0.1, 0.2,1.1); 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); 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); Vector u = Vector_(3, 1.0, 0.0, 2.0);
Matrix R = Matrix_(2,2, 1.0,0.0, 0.0,3.0); Matrix R = Matrix_(2,2, 1.0,0.5, 0.0,3.0);
Matrix M = trans(R)*R; Matrix M = trans(R)*R;
Matrix Q = inverse(M); Matrix Q = inverse(M);
@ -138,7 +138,7 @@ TEST( KalmanFilter, predict ) {
// Ensure predictQ and predict2 give same answer for non-trivial inputs // Ensure predictQ and predict2 give same answer for non-trivial inputs
kalmanFilter1.predictQ(F, B, u, Q); kalmanFilter1.predictQ(F, B, u, Q);
// We have A1 = -F, A2 = I_, b = B*u: // We have A1 = -F, A2 = I_, b = B*u, pre-multipled with R to match Q noise model
Matrix A1 = -R*F, A2 = R; Matrix A1 = -R*F, A2 = R;
Vector b = R*B*u; Vector b = R*B*u;
SharedDiagonal nop = noiseModel::Isotropic::Sigma(2, 1.0); SharedDiagonal nop = noiseModel::Isotropic::Sigma(2, 1.0);