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

Fixed index bug in ExtendedKalmanFilter unit test

release/4.3a0
Stephen Williams 2012-01-30 23:37:47 +00:00
parent 537a1a3fae
commit 4c54d05da9
1 changed files with 35 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
tests/testExtendedKalmanFilter.cpp
View File

@ -369,26 +369,39 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
// Create the set of expected output TestValues (generated using Matlab Kalman Filter) // Create the set of expected output TestValues (generated using Matlab Kalman Filter)
Point2 expected_predict[10]; Point2 expected_predict[10];
Point2 expected_update[10]; Point2 expected_update[10];
expected_predict[1] = Point2(0.81,0.99); expected_predict[0] = Point2(0.81,0.99);
expected_update[1] = Point2(0.824926197027,0.29509808); expected_update[0] = Point2(0.824926197027,0.29509808);
expected_predict[2] = Point2(0.680503230541,0.24343413); expected_predict[1] = Point2(0.680503230541,0.24343413);
expected_update[2] = Point2(0.773360464065,0.43518355); expected_update[1] = Point2(0.773360464065,0.43518355);
expected_predict[3] = Point2(0.598086407378,0.33655375); expected_predict[2] = Point2(0.598086407378,0.33655375);
expected_update[3] = Point2(0.908781566884,0.60766713); expected_update[2] = Point2(0.908781566884,0.60766713);
expected_predict[4] = Point2(0.825883936308,0.55223668); expected_predict[3] = Point2(0.825883936308,0.55223668);
expected_update[4] = Point2(1.23221370495,0.74372849); expected_update[3] = Point2(1.23221370495,0.74372849);
expected_predict[5] = Point2(1.51835061468,0.91643243); expected_predict[4] = Point2(1.51835061468,0.91643243);
expected_update[5] = Point2(1.32823362774,0.855855); expected_update[4] = Point2(1.32823362774,0.855855);
expected_predict[6] = Point2(1.76420456986,1.1367754); expected_predict[5] = Point2(1.76420456986,1.1367754);
expected_update[6] = Point2(1.36378040243,1.0623832); expected_update[5] = Point2(1.36378040243,1.0623832);
expected_predict[7] = Point2(1.85989698605,1.4488574); expected_predict[6] = Point2(1.85989698605,1.4488574);
expected_update[7] = Point2(1.24068063304,1.3431964); expected_update[6] = Point2(1.24068063304,1.3431964);
expected_predict[8] = Point2(1.53928843321,1.6664778); expected_predict[7] = Point2(1.53928843321,1.6664778);
expected_update[8] = Point2(1.04229257957,1.4856408); expected_update[7] = Point2(1.04229257957,1.4856408);
expected_predict[9] = Point2(1.08637382142,1.5484724); expected_predict[8] = Point2(1.08637382142,1.5484724);
expected_update[9] = Point2(1.13201933157,1.5795507); expected_update[8] = Point2(1.13201933157,1.5795507);
expected_predict[10] = Point2(1.28146776705,1.7880819); expected_predict[9] = Point2(1.28146776705,1.7880819);
expected_update[10] = Point2(1.22159588179,1.7434982); expected_update[9] = Point2(1.22159588179,1.7434982);
// Measurements
double z[10];
z[0] = 1.0;
z[1] = 2.0;
z[2] = 3.0;
z[3] = 4.0;
z[4] = 5.0;
z[5] = 6.0;
z[6] = 7.0;
z[7] = 8.0;
z[8] = 9.0;
z[9] = 10.0;
// Create the Kalman Filter initialization point // Create the Kalman Filter initialization point
Point2 x_initial(0.90, 1.10); Point2 x_initial(0.90, 1.10);
@ -399,13 +412,13 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
// Enter Predict-Update Loop // Enter Predict-Update Loop
Point2 x_predict, x_update; Point2 x_predict, x_update;
for(unsigned int i = 1; i < 9; ++i){ for(unsigned int i = 0; i < 10; ++i){
// Create motion factor // Create motion factor
NonlinearMotionModel motionFactor(TestKey(i-1), TestKey(i)); NonlinearMotionModel motionFactor(TestKey(i-1), TestKey(i));
x_predict = ekf.predict(motionFactor); x_predict = ekf.predict(motionFactor);
// Create a measurement factor // Create a measurement factor
NonlinearMeasurementModel measurementFactor(TestKey(i), Vector_(1, (double)i)); NonlinearMeasurementModel measurementFactor(TestKey(i), Vector_(1, z[i]));
x_update = ekf.update(measurementFactor); x_update = ekf.update(measurementFactor);
EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6));