From ced0569b6dd3b4c95248df407588e9f2008291af Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Fri, 2 Mar 2012 17:13:07 +0000 Subject: [PATCH] Fix index out-of-bounds bug in Kalman Filter test. --- tests/testExtendedKalmanFilter.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 927499050..d9e5643a4 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -362,8 +362,8 @@ public: TEST( ExtendedKalmanFilter, nonlinear ) { // Create the set of expected output TestValues (generated using Matlab Kalman Filter) - Point2 expected_predict[11]; - Point2 expected_update[11]; + Point2 expected_predict[10]; + Point2 expected_update[10]; expected_predict[0] = Point2(0.81,0.99); expected_update[0] = Point2(0.824926197027,0.29509808); expected_predict[1] = Point2(0.680503230541,0.24343413); @@ -409,11 +409,11 @@ TEST( ExtendedKalmanFilter, nonlinear ) { Point2 x_predict, x_update; for(unsigned int i = 0; i < 10; ++i){ // 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); // 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); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6));