1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file testExtendedKalmanFilter
14 * @author Stephen Williams
15 */
16
17#include <gtsam/nonlinear/PriorFactor.h>
18#include <gtsam/slam/BetweenFactor.h>
19#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
20#include <gtsam/nonlinear/NonlinearFactor.h>
21#include <gtsam/inference/Symbol.h>
22#include <gtsam/geometry/Point2.h>
23
24#include <CppUnitLite/TestHarness.h>
25
26using namespace gtsam;
27
28// Convenience for named keys
29using symbol_shorthand::X;
30using symbol_shorthand::L;
31
32/* ************************************************************************* */
33TEST( ExtendedKalmanFilter, linear ) {
34
35 // Create the TestKeys for our example
36 Symbol x0('x',0), x1('x',1), x2('x',2), x3('x',3);
37
38 // Create the Kalman Filter initialization point
39 Point2 x_initial(0.0, 0.0);
40 SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.1, 0.1));
41
42 // Create an ExtendedKalmanFilter object
43 ExtendedKalmanFilter<Point2> ekf(x0, x_initial, P_initial);
44
45 // Create the controls and measurement properties for our example
46 double dt = 1.0;
47 Vector u = Vector2(1.0, 0.0);
48 Point2 difference(u*dt);
49 SharedDiagonal Q = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.1, 0.1), smart: true);
50 Point2 z1(1.0, 0.0);
51 Point2 z2(2.0, 0.0);
52 Point2 z3(3.0, 0.0);
53 SharedDiagonal R = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.25, 0.25), smart: true);
54
55 // Create the set of expected output TestValues
56 Point2 expected1(1.0, 0.0);
57 Point2 expected2(2.0, 0.0);
58 Point2 expected3(3.0, 0.0);
59
60 // Run iteration 1
61 // Create motion factor
62 BetweenFactor<Point2> factor1(x0, x1, difference, Q);
63 Point2 predict1 = ekf.predict(motionFactor: factor1);
64 EXPECT(assert_equal(expected1,predict1));
65
66 // Create the measurement factor
67 PriorFactor<Point2> factor2(x1, z1, R);
68 Point2 update1 = ekf.update(measurementFactor: factor2);
69 EXPECT(assert_equal(expected1,update1));
70
71 // Run iteration 2
72 BetweenFactor<Point2> factor3(x1, x2, difference, Q);
73 Point2 predict2 = ekf.predict(motionFactor: factor3);
74 EXPECT(assert_equal(expected2,predict2));
75
76 PriorFactor<Point2> factor4(x2, z2, R);
77 Point2 update2 = ekf.update(measurementFactor: factor4);
78 EXPECT(assert_equal(expected2,update2));
79
80 // Run iteration 3
81 BetweenFactor<Point2> factor5(x2, x3, difference, Q);
82 Point2 predict3 = ekf.predict(motionFactor: factor5);
83 EXPECT(assert_equal(expected3,predict3));
84
85 PriorFactor<Point2> factor6(x3, z3, R);
86 Point2 update3 = ekf.update(measurementFactor: factor6);
87 EXPECT(assert_equal(expected3,update3));
88
89 return;
90}
91
92
93// Create Motion Model Factor
94class NonlinearMotionModel : public NoiseModelFactorN<Point2,Point2> {
95
96 typedef NoiseModelFactorN<Point2, Point2> Base;
97 typedef NonlinearMotionModel This;
98
99protected:
100 Matrix Q_;
101 Matrix Q_invsqrt_;
102
103public:
104
105 // Provide access to the Matrix& version of evaluateError:
106 using Base::evaluateError;
107
108 NonlinearMotionModel(){}
109
110 NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) :
111 Base(noiseModel::Diagonal::Sigmas(sigmas: Vector2(1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) {
112
113 // Initialize motion model parameters:
114 // w is vector of motion noise sigmas. The final covariance is calculated as G*w*w'*G'
115 // In this model, Q is fixed, so it may be calculated in the constructor
116 Matrix G(2,2);
117 Matrix w(2,2);
118
119 G << 1.0, 0.0, 0.0, 1.0;
120 w << 1.0, 0.0, 0.0, 1.0;
121
122 w = G*w;
123 Q_ = w*w.transpose();
124 Q_invsqrt_ = inverse_square_root(A: Q_);
125 }
126
127 ~NonlinearMotionModel() override {}
128
129 // Calculate the next state prediction using the nonlinear function f()
130 Point2 f(const Point2& x_t0) const {
131
132 // Calculate f(x)
133 double x = x_t0.x() * x_t0.x();
134 double y = x_t0.x() * x_t0.y();
135 Point2 x_t1(x,y);
136
137 // In this example, the noiseModel remains constant
138 return x_t1;
139 }
140
141 // Calculate the Jacobian of the nonlinear function f()
142 Matrix F(const Point2& x_t0) const {
143 // Calculate Jacobian of f()
144 Matrix F = Matrix(2,2);
145 F(0,0) = 2*x_t0.x();
146 F(0,1) = 0.0;
147 F(1,0) = x_t0.y();
148 F(1,1) = x_t0.x();
149
150 return F;
151 }
152
153 // Calculate the inverse square root of the motion model covariance, Q
154 Matrix QInvSqrt(const Point2& x_t0) const {
155 // This motion model has a constant covariance
156 return Q_invsqrt_;
157 }
158
159 /* print */
160 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
161 std::cout << s << ": NonlinearMotionModel\n";
162 std::cout << " TestKey1: " << keyFormatter(key<1>()) << std::endl;
163 std::cout << " TestKey2: " << keyFormatter(key<2>()) << std::endl;
164 }
165
166 /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
167 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
168 const This *e = dynamic_cast<const This*> (&f);
169 return (e != nullptr) && (key<1>() == e->key<1>()) && (key<2>() == e->key<2>());
170 }
171
172 /**
173 * calculate the error of the factor
174 * Override for systems with unusual noise models
175 */
176 double error(const Values& c) const override {
177 Vector w = whitenedError(c);
178 return 0.5 * w.dot(other: w);
179 }
180
181 /** get the dimension of the factor (number of rows on linearization) */
182 size_t dim() const override {
183 return 2;
184 }
185
186 /** Vector of errors, whitened ! */
187 Vector whitenedError(const Values& c) const {
188 return QInvSqrt(x_t0: c.at<Point2>(j: key<1>()))*unwhitenedError(x: c);
189 }
190
191 /**
192 * Linearize a non-linearFactor2 to get a GaussianFactor
193 * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
194 * Hence b = z - h(x1,x2) = - error_vector(x)
195 */
196 std::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
197 using X1 = ValueType<1>;
198 using X2 = ValueType<2>;
199 const X1& x1 = c.at<X1>(j: key<1>());
200 const X2& x2 = c.at<X2>(j: key<2>());
201 Matrix A1, A2;
202 Vector b = -evaluateError(x: x1, x: x2, H&: A1, H&: A2);
203 SharedDiagonal constrained =
204 std::dynamic_pointer_cast<noiseModel::Constrained>(r: this->noiseModel_);
205 if (constrained.get() != nullptr) {
206 return JacobianFactor::shared_ptr(new JacobianFactor(key<1>(), A1, key<2>(),
207 A2, b, constrained));
208 }
209 // "Whiten" the system before converting to a Gaussian Factor
210 Matrix Qinvsqrt = QInvSqrt(x_t0: x1);
211 A1 = Qinvsqrt*A1;
212 A2 = Qinvsqrt*A2;
213 b = Qinvsqrt*b;
214 return GaussianFactor::shared_ptr(new JacobianFactor(key<1>(), A1, key<2>(),
215 A2, b, noiseModel::Unit::Create(dim: b.size())));
216 }
217
218 /** vector of errors */
219 Vector evaluateError(const Point2& p1, const Point2& p2,
220 OptionalMatrixType H1, OptionalMatrixType H2) const override {
221
222 // error = p2 - f(p1)
223 // H1 = d error / d p1 = -d f/ d p1 = -F
224 // H2 = d error / d p2 = I
225 Point2 prediction = f(x_t0: p1);
226
227 if(H1)
228 *H1 = -F(x_t0: p1);
229
230 if(H2)
231 *H2 = Matrix::Identity(rows: dim(),cols: dim());
232
233 // Return the error between the prediction and the supplied value of p2
234 return (p2 - prediction);
235 }
236
237};
238
239// Create Measurement Model Factor
240class NonlinearMeasurementModel : public NoiseModelFactorN<Point2> {
241
242 typedef NoiseModelFactorN<Point2> Base;
243 typedef NonlinearMeasurementModel This;
244
245protected:
246 Vector z_; /** The measurement */
247 Matrix R_; /** The measurement error covariance */
248 Matrix R_invsqrt_; /** The inv sqrt of the measurement error covariance */
249
250public:
251
252 // Provide access to the Matrix& version of evaluateError:
253 using Base::evaluateError;
254
255 NonlinearMeasurementModel(){}
256
257 NonlinearMeasurementModel(const Symbol& TestKey, Vector z) :
258 Base(noiseModel::Unit::Create(dim: z.size()), TestKey), z_(z), R_(1,1) {
259
260 // Initialize nonlinear measurement model parameters:
261 // z(t) = H*x(t) + v
262 // where v ~ N(0, noiseModel_)
263 R_ << 1.0;
264 R_invsqrt_ = inverse_square_root(A: R_);
265 }
266
267 ~NonlinearMeasurementModel() override {}
268
269 // Calculate the predicted measurement using the nonlinear function h()
270 // Byproduct: updates Jacobian H, and noiseModel (R)
271 Vector h(const Point2& x_t1) const {
272
273 // Calculate prediction
274 Vector z_hat(1);
275 z_hat(0) = 2*x_t1.x()*x_t1.x() - x_t1.x()*x_t1.y() - 2.5*x_t1.x() + 7*x_t1.y();
276
277 return z_hat;
278 }
279
280 Matrix H(const Point2& x_t1) const {
281 // Update Jacobian
282 Matrix H(1,2);
283 H(0,0) = 4*x_t1.x() - x_t1.y() - 2.5;
284 H(0,1) = -1*x_t1.x() + 7;
285
286 return H;
287 }
288
289 // Calculate the inverse square root of the motion model covariance, Q
290 Matrix RInvSqrt(const Point2& x_t0) const {
291 // This motion model has a constant covariance
292 return R_invsqrt_;
293 }
294
295 /* print */
296 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
297 std::cout << s << ": NonlinearMeasurementModel\n";
298 std::cout << " TestKey: " << keyFormatter(key()) << std::endl;
299 }
300
301 /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
302 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
303 const This *e = dynamic_cast<const This*> (&f);
304 return (e != nullptr) && Base::equals(f);
305 }
306
307 /**
308 * calculate the error of the factor
309 * Override for systems with unusual noise models
310 */
311 double error(const Values& c) const override {
312 Vector w = whitenedError(c);
313 return 0.5 * w.dot(other: w);
314 }
315
316 /** get the dimension of the factor (number of rows on linearization) */
317 size_t dim() const override {
318 return 1;
319 }
320
321 /** Vector of errors, whitened ! */
322 Vector whitenedError(const Values& c) const {
323 return RInvSqrt(x_t0: c.at<Point2>(j: key()))*unwhitenedError(x: c);
324 }
325
326 /**
327 * Linearize a nonlinearFactor1 measurement factor into a GaussianFactor
328 * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z
329 * Hence b = z - h(x1) = - error_vector(x)
330 */
331 std::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
332 using X = ValueType<1>;
333 const X& x1 = c.at<X>(j: key());
334 Matrix A1;
335 Vector b = -evaluateError(x: x1, H&: A1);
336 SharedDiagonal constrained =
337 std::dynamic_pointer_cast<noiseModel::Constrained>(r: this->noiseModel_);
338 if (constrained.get() != nullptr) {
339 return JacobianFactor::shared_ptr(new JacobianFactor(key(), A1, b, constrained));
340 }
341 // "Whiten" the system before converting to a Gaussian Factor
342 Matrix Rinvsqrt = RInvSqrt(x_t0: x1);
343 A1 = Rinvsqrt*A1;
344 b = Rinvsqrt*b;
345 return GaussianFactor::shared_ptr(new JacobianFactor(key(), A1, b, noiseModel::Unit::Create(dim: b.size())));
346 }
347
348 /** vector of errors */
349 Vector evaluateError(const Point2& p, OptionalMatrixType H1) const override {
350 // error = z - h(p)
351 // H = d error / d p = -d h/ d p = -H
352 Vector z_hat = h(x_t1: p);
353
354 if(H1){
355 *H1 = -H(x_t1: p);
356 }
357
358 // Return the error between the prediction and the supplied value of p2
359 return z_ - z_hat;
360 }
361
362};
363
364
365/* ************************************************************************* */
366TEST( ExtendedKalmanFilter, nonlinear ) {
367
368 // Create the set of expected output TestValues (generated using Matlab Kalman Filter)
369 Point2 expected_predict[10];
370 Point2 expected_update[10];
371 expected_predict[0] = Point2(0.81,0.99);
372 expected_update[0] = Point2(0.824926197027,0.29509808);
373 expected_predict[1] = Point2(0.680503230541,0.24343413);
374 expected_update[1] = Point2(0.773360464065,0.43518355);
375 expected_predict[2] = Point2(0.598086407378,0.33655375);
376 expected_update[2] = Point2(0.908781566884,0.60766713);
377 expected_predict[3] = Point2(0.825883936308,0.55223668);
378 expected_update[3] = Point2(1.23221370495,0.74372849);
379 expected_predict[4] = Point2(1.51835061468,0.91643243);
380 expected_update[4] = Point2(1.32823362774,0.855855);
381 expected_predict[5] = Point2(1.76420456986,1.1367754);
382 expected_update[5] = Point2(1.36378040243,1.0623832);
383 expected_predict[6] = Point2(1.85989698605,1.4488574);
384 expected_update[6] = Point2(1.24068063304,1.3431964);
385 expected_predict[7] = Point2(1.53928843321,1.6664778);
386 expected_update[7] = Point2(1.04229257957,1.4856408);
387 expected_predict[8] = Point2(1.08637382142,1.5484724);
388 expected_update[8] = Point2(1.13201933157,1.5795507);
389 expected_predict[9] = Point2(1.28146776705,1.7880819);
390 expected_update[9] = Point2(1.22159588179,1.7434982);
391
392 // Measurements
393 double z[10];
394 z[0] = 1.0;
395 z[1] = 2.0;
396 z[2] = 3.0;
397 z[3] = 4.0;
398 z[4] = 5.0;
399 z[5] = 6.0;
400 z[6] = 7.0;
401 z[7] = 8.0;
402 z[8] = 9.0;
403 z[9] = 10.0;
404
405 // Create the Kalman Filter initialization point
406 Point2 x_initial(0.90, 1.10);
407 SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.1, 0.1));
408
409 // Create an ExtendedKalmanFilter object
410 ExtendedKalmanFilter<Point2> ekf(X(j: 0), x_initial, P_initial);
411
412 // Enter Predict-Update Loop
413 Point2 x_predict(0,0), x_update(0,0);
414 for(unsigned int i = 0; i < 10; ++i){
415 // Create motion factor
416 NonlinearMotionModel motionFactor(X(j: i), X(j: i+1));
417 x_predict = ekf.predict(motionFactor);
418
419 // Create a measurement factor
420 NonlinearMeasurementModel measurementFactor(X(j: i+1), (Vector(1) << z[i]).finished());
421 x_update = ekf.update(measurementFactor);
422
423 EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6));
424 EXPECT(assert_equal(expected_update[i], x_update, 1e-6));
425 }
426
427 return;
428}
429
430
431/* ************************************************************************* */
432int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
433/* ************************************************************************* */
434