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

Fix index out-of-bounds bug in Kalman Filter test.

release/4.3a0
Stephen Williams 2012-03-02 17:13:07 +00:00
parent f80818cf3f
commit ced0569b6d
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
tests/testExtendedKalmanFilter.cpp
View File

@ -362,8 +362,8 @@ public:
TEST( ExtendedKalmanFilter, nonlinear ) { 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[11]; Point2 expected_predict[10];
Point2 expected_update[11]; Point2 expected_update[10];
expected_predict[0] = Point2(0.81,0.99); expected_predict[0] = Point2(0.81,0.99);
expected_update[0] = Point2(0.824926197027,0.29509808); expected_update[0] = Point2(0.824926197027,0.29509808);
expected_predict[1] = Point2(0.680503230541,0.24343413); expected_predict[1] = Point2(0.680503230541,0.24343413);
@ -409,11 +409,11 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
Point2 x_predict, x_update; Point2 x_predict, x_update;
for(unsigned int i = 0; i < 10; ++i){ for(unsigned int i = 0; i < 10; ++i){
// Create motion factor // Create motion factor
NonlinearMotionModel motionFactor(Symbol('x',i-1), Symbol('x',i)); NonlinearMotionModel motionFactor(Symbol('x',i), Symbol('x',i+1));
x_predict = ekf.predict(motionFactor); x_predict = ekf.predict(motionFactor);
// Create a measurement factor // Create a measurement factor
NonlinearMeasurementModel measurementFactor(Symbol('x',i), Vector_(1, z[i])); NonlinearMeasurementModel measurementFactor(Symbol('x',i+1), 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));