Kalman.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
3  *
4  * Copyright 2007-2012 VTT Technical Research Centre of Finland
5  *
6  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
7  * <http://www.vtt.fi/multimedia/alvar.html>
8  *
9  * ALVAR is free software; you can redistribute it and/or modify it under the
10  * terms of the GNU Lesser General Public License as published by the Free
11  * Software Foundation; either version 2.1 of the License, or (at your option)
12  * any later version.
13  *
14  * This library is distributed in the hope that it will be useful, but WITHOUT
15  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
16  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
17  * for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with ALVAR; if not, see
21  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
22  */
23 
24 #include <iostream>
25 #include <algorithm> // for std::max
26 #include "cxcore.h"
27 #include "cv.h"
28 #include "highgui.h"
29 #include "ar_track_alvar/Kalman.h"
30 #include "ar_track_alvar/Util.h"
31 #include "ar_track_alvar/Alvar.h"
32 
33 namespace alvar {
34 
36  m = k.m;
37  n = k.n;
38  z = cvCloneMat(k.z);
39  H = cvCloneMat(k.H);
40  H_trans = cvCloneMat(k.H_trans);
41  K = cvCloneMat(k.K);
42  z_pred = cvCloneMat(k.z_pred);
43  z_residual = cvCloneMat(k.z_residual);
44  x_gain = cvCloneMat(k.x_gain);
45 }
46 
48  n = _n;
49  m = _m;
50  z = cvCreateMat(m,1,CV_64FC1); cvSetZero(z);
51  H = cvCreateMat(m,n,CV_64FC1); cvSetZero(H);
52  H_trans = cvCreateMat(n,m,CV_64FC1); cvSetZero(H_trans);
53  K = cvCreateMat(n,m,CV_64FC1); cvSetZero(K);
54  z_pred = cvCreateMat(m,1,CV_64FC1); cvSetZero(z_pred);
55  z_residual = cvCreateMat(m,1,CV_64FC1); cvSetZero(z_residual);
56  x_gain = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_gain);
57 }
58 
60  cvReleaseMat(&z);
61  cvReleaseMat(&H);
62  cvReleaseMat(&H_trans);
63  cvReleaseMat(&K);
64  cvReleaseMat(&z_pred);
65  cvReleaseMat(&z_residual);
66  cvReleaseMat(&x_gain);
67 }
68 
69 void KalmanSensorCore::update_x(CvMat *x_pred, CvMat *x) {
70  // x = x_pred + K * (z - H*x_pred)
71  cvMatMul(H, x_pred, z_pred);
72  cvScaleAdd(z_pred, cvScalar(-1), z, z_residual);
73  cvMatMul(K, z_residual, x_gain);
74  cvScaleAdd(x_pred, cvScalar(1), x_gain, x);
75 }
76 
77 void KalmanCore::predict_x(unsigned long tick) {
78  // x_pred = F * x;
79  cvMatMul(F, x, x_pred);
80 }
81 
83  n = s.n;
84  x = cvCloneMat(s.x);
85  F = cvCloneMat(s.F);
86  x_pred = cvCloneMat(s.x_pred);
87  F_trans = cvCloneMat(s.F_trans);
88 }
89 
91  n = _n;
92  x = cvCreateMat(n,1,CV_64FC1); cvSetZero(x);
93  F = cvCreateMat(n,n,CV_64FC1); cvSetIdentity(F);
94  F_trans = cvCreateMat(n,n,CV_64FC1); cvSetIdentity(F_trans);
95  x_pred = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_pred);
96 }
97 
99  cvReleaseMat(&x);
100  cvReleaseMat(&F);
101  cvReleaseMat(&F_trans);
102  cvReleaseMat(&x_pred);
103 }
104 
106  predict_x(0);
107  return x_pred;
108 }
109 
111  predict();
112  sensor->update_x(x_pred, x);
113  return x;
114 }
115 
117  R = cvCloneMat(k.R);
118  R_tmp = cvCloneMat(k.R_tmp);
119  P_tmp = cvCloneMat(k.P_tmp);
120 }
121 
123  R = cvCreateMat(m,m,CV_64FC1); cvSetZero(R);
124  R_tmp = cvCreateMat(m,m,CV_64FC1); cvSetZero(R);
125  P_tmp = cvCreateMat(n,n,CV_64FC1); cvSetZero(P_tmp);
126 }
127 
129  cvReleaseMat(&R);
130  cvReleaseMat(&R_tmp);
131  cvReleaseMat(&P_tmp);
132 }
133 
134 void KalmanSensor::update_K(CvMat *P_pred) {
135  // K = P * trans(H) * inv(H*P*trans(H) + R)
136  cvTranspose(H, H_trans);
137  cvMatMul(P_pred, H_trans, K);
138  cvMatMul(H, K, R_tmp);
139  cvScaleAdd(R_tmp, cvScalar(1), R, R_tmp);
140  cvInvert(R_tmp, R_tmp);
141  cvMatMul(H_trans, R_tmp, K);
142  cvMatMul(P_pred, K, K);
143 }
144 
145 void KalmanSensor::update_P(CvMat *P_pred, CvMat *P) {
146  //P = (I - K*H) * P_pred
147  cvMatMul(K, H, P_tmp);
148  cvSetIdentity(P);
149  cvScaleAdd(P_tmp, cvScalar(-1), P, P);
150  cvMatMul(P, P_pred, P);
151 }
152 
154  // P_pred = F*P*trans(F) + Q
155  cvTranspose(F, F_trans);
156  cvMatMul(P, F_trans, P_pred);
157  cvMatMul(F, P_pred, P_pred);
158  cvScaleAdd(P_pred, cvScalar(1), Q, P_pred);
159 }
160 
161 Kalman::Kalman(int _n) : KalmanCore(_n) {
162  prev_tick = 0;
163  Q = cvCreateMat(n,n,CV_64FC1); cvSetZero(Q);
164  P = cvCreateMat(n,n,CV_64FC1); cvSetZero(P);
165  P_pred = cvCreateMat(n,n,CV_64FC1); cvSetZero(P_pred);
166 }
167 
169  cvReleaseMat(&Q);
170  cvReleaseMat(&P);
171  cvReleaseMat(&P_pred);
172 }
173 
174 void Kalman::update_F(unsigned long tick) {
175  //cvSetIdentity(F);
176 }
177 
178 CvMat *Kalman::predict(unsigned long tick) {
179  update_F(tick);
180  predict_x(tick);
181  predict_P();
182  return x_pred;
183 }
184 
185 CvMat *Kalman::predict_update(KalmanSensor *sensor, unsigned long tick) {
186  predict(tick);
187  sensor->update_H(x_pred);
188  sensor->update_K(P_pred);
189  sensor->update_x(x_pred, x);
190  sensor->update_P(P_pred, P);
191  prev_tick = tick;
192  return x;
193 }
194 
195 double Kalman::seconds_since_update(unsigned long tick) {
196  unsigned long tick_diff = (prev_tick ? tick-prev_tick : 0);
197  return ((double)tick_diff/1000.0);
198 }
199 
201  // By default we update the H by calculating Jacobian numerically
202  const double step = 0.000001;
203  cvZero(H);
204  for (int i=0; i<n; i++) {
205  CvMat H_column;
206  cvGetCol(H, &H_column, i);
207 
208  cvZero(delta);
209  cvmSet(delta, i, 0, step);
210  cvAdd(x_pred, delta, x_plus);
211  cvmSet(delta, i, 0, -step);
212  cvAdd(x_pred, delta, x_minus);
213 
214  h(x_plus, z_tmp1);
215  h(x_minus, z_tmp2);
216  cvSub(z_tmp1, z_tmp2, &H_column);
217  cvScale(&H_column, &H_column, 1.0/(2*step));
218  }
219 }
220 
221 void KalmanSensorEkf::update_x(CvMat *x_pred, CvMat *x) {
222  // x = x_pred + K * (z - h(x_pred))
223  h(x_pred, z_pred);
224  cvScaleAdd(z_pred, cvScalar(-1), z, z_residual);
225  cvMatMul(K, z_residual, x_gain);
226  cvScaleAdd(x_pred, cvScalar(1), x_gain, x);
227 }
228 
230  delta = cvCloneMat(k.delta);
231  x_plus = cvCloneMat(k.x_plus);
232  x_minus = cvCloneMat(k.x_minus);
233  z_tmp1 = cvCloneMat(k.z_tmp1);
234  z_tmp2 = cvCloneMat(k.z_tmp2);
235 }
236 
238  delta = cvCreateMat(n,1,CV_64FC1); cvSetZero(delta);
239  x_plus = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_plus);
240  x_minus = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_minus);
241  z_tmp1 = cvCreateMat(m,1,CV_64FC1); cvSetZero(z_tmp1);
242  z_tmp2 = cvCreateMat(m,1,CV_64FC1); cvSetZero(z_tmp2);
243 }
244 
246  cvReleaseMat(&delta);
247  cvReleaseMat(&x_plus);
248  cvReleaseMat(&x_minus);
249  cvReleaseMat(&z_tmp1);
250  cvReleaseMat(&z_tmp2);
251 }
252 
253 void KalmanEkf::update_F(unsigned long tick) {
254  // By default we update the F by calculating Jacobian numerically
255  // TODO
256  double dt = (tick-prev_tick)/1000.0;
257  const double step = 0.000001;
258  cvZero(F);
259  for (int i=0; i<n; i++) {
260  CvMat F_column;
261  cvGetCol(F, &F_column, i);
262 
263  cvZero(delta);
264  cvmSet(delta, i, 0, step);
265  cvAdd(x, delta, x_plus);
266  cvmSet(delta, i, 0, -step);
267  cvAdd(x, delta, x_minus);
268 
269  f(x_plus, x_tmp1, dt);
270  f(x_minus, x_tmp2, dt);
271  cvSub(x_tmp1, x_tmp2, &F_column);
272  cvScale(&F_column, &F_column, 1.0/(2*step));
273  }
274 }
275 
276 void KalmanEkf::predict_x(unsigned long tick) {
277  double dt = (tick-prev_tick)/1000.0;
278  f(x, x_pred, dt);
279 }
280 
282  delta = cvCreateMat(n,1,CV_64FC1); cvSetZero(delta);
283  x_plus = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_plus);
284  x_minus = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_minus);
285  x_tmp1 = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_tmp1);
286  x_tmp2 = cvCreateMat(n,1,CV_64FC1); cvSetZero(x_tmp2);
287 }
288 
290  cvReleaseMat(&delta);
291  cvReleaseMat(&x_plus);
292  cvReleaseMat(&x_minus);
293  cvReleaseMat(&x_tmp1);
294  cvReleaseMat(&x_tmp2);
295 }
296 
297 void KalmanVisualize::img_matrix(CvMat *mat, int top, int left) {
298  cvSetImageROI(img, cvRect(top, left, mat->cols, mat->rows));
299  for (int j=0; j<mat->rows; j++) {
300  for (int i=0; i<mat->cols; i++) {
301  double d = cvGet2D(mat, j, i).val[0];
302  if (d < 0) d=-d;
303  double c1=0, c2=0, c3=0;
304  if (d < 0.1) {
305  c1 = 0 + ((d - 0.0)/(0.1 - 0.0)*(127 - 0));
306  } else if(d < 1.0) {
307  c1 = 127 + ((d - 0.1)/(1.0 - 0.1)*(255 - 127));
308  } else if (d < 10.0) {
309  c1 = 255;
310  c2 = 0 + ((d - 1.0)/(10.0 - 1.0)*(255 - 0));
311  } else if (d < 100.0) {
312  c1 = 255;
313  c2 = 255;
314  c3 = 0 + ((d - 10.0)/(100.0 - 10.0)*(255 - 0));
315  } else {
316  c1 = 255; c2 = 255; c3 = 255;
317  }
318  if (d < 0) {
319  cvSet2D(img, j, i, cvScalar(c3, c2, c1)); // BRG
320  } else {
321  cvSet2D(img, j, i, cvScalar(c2, c1, c3)); // BGR
322  }
323  }
324  }
325  cvResetImageROI(img);
326 }
327 
329  n = kalman->get_n();
330  m = sensor->get_m();
331  int img_width = std::max(3+n+3+n+5+m+5, 1+n+1+n+1+n+1+m+1+n+1);
332  int img_height = 1+n+1+std::max(n, m+1+m)+1;
333  img = cvCreateImage(cvSize(img_width, img_height), IPL_DEPTH_8U, 3);
334  cvSet(img, cvScalar(64,64,64));
335  img_legend = cvLoadImage("Legend.png");
336  if (img_legend) {
337  for (img_scale=1; img_scale<50; img_scale++) {
338  if (img_scale*img_width > img_legend->width) {
339  break;
340  }
341  }
342  img_show = cvCreateImage(cvSize(img_width*img_scale, img_legend->height + img_height*img_scale), IPL_DEPTH_8U, 3);
343  cvSet(img_show, cvScalar(64,64,64));
344  cvSetImageROI(img_show, cvRect(0, 0, img_legend->width, img_legend->height));
345  cvCopy(img_legend, img_show);
346  cvResetImageROI(img_show);
347  cvNamedWindow("KalmanVisualize");
348  } else {
349  img_scale = 1;
350  img_show = cvCreateImage(cvSize(img_width*img_scale, img_height*img_scale), IPL_DEPTH_8U, 3);
351  cvSet(img_show, cvScalar(64,64,64));
352  cvNamedWindow("KalmanVisualize",0);
353  }
354 }
355 
356 void KalmanVisualize::out_matrix(CvMat *m, char *name) {
357  if (m->cols == 1) {
358  std::cout<<name<<" = [";
359  for (int j=0; j<m->rows; j++) {
360  std::cout<<" "<<cvGet2D(m, j, 0).val[0];
361  }
362  std::cout<<"]^T"<<std::endl;
363  } else if (m->rows == 1) {
364  std::cout<<name<<" = [";
365  for (int i=0; i<m->cols; i++) {
366  std::cout<<" "<<cvGet2D(m, 0, i).val[0];
367  }
368  std::cout<<"]^T"<<std::endl;
369  } else {
370  std::cout<<name<<" = ["<<std::endl;
371  for (int j=0; j<m->rows; j++) {
372  for (int i=0; i<m->cols; i++) {
373  std::cout<<" "<<cvGet2D(m, j, i).val[0];
374  }
375  std::cout<<std::endl;
376  }
377  std::cout<<"]"<<std::endl;
378  }
379 }
380 
382  kalman = _kalman;
383  sensor = _sensor;
384  kalman_ext = _kalman;
385  sensor_ext = _sensor;
386  Init();
387 }
388 
390  kalman = _kalman;
391  sensor = _sensor;
392  kalman_ext = NULL;
393  sensor_ext = NULL;
394  Init();
395 }
396 
398  cvReleaseImage(&img);
399 }
400 
402  img_matrix(kalman->x, 1, 1); // 1
403  if (kalman_ext && sensor_ext) {
404  int y = std::max(2+n, 3+m+m);
405  img_matrix(kalman_ext->P, 1, y); // n
406  }
407 }
408 
410  img_matrix(kalman->F, 3, 1); // n
411  img_matrix(kalman->x_pred, 4+n, 1); // 1
412  img_matrix(sensor->H, 6+n, 1); // n
413  img_matrix(sensor->z_pred, 7+n+n, 1); // 1
414  img_matrix(sensor->z, 7+n+n, 2 + m);
415  img_matrix(sensor->z_residual, 9+n+n, 1); // 1
416  img_matrix(sensor->K, 11+n+n, 1); // m
417  img_matrix(sensor->x_gain, 12+n+n+m, 1); // 1
418  img_matrix(kalman->x, 14+n+n+m, 1); // 1
419  if (kalman_ext && sensor_ext) {
420  int y = std::max(2+n, 3+m+m);
421  img_matrix(kalman_ext->Q, 2+n, y); // n
422  img_matrix(kalman_ext->P_pred, 3+n+n, y); // n
423  img_matrix(sensor_ext->R, 4+n+n+n, y); // m
424  img_matrix(kalman_ext->P, img->width - 1 - n, y); // n
425  }
426  if (img_legend) {
427  cvSetImageROI(img_show, cvRect(0, img_legend->height, img->width * img_scale, img->height * img_scale));
428  cvResize(img, img_show, CV_INTER_NN);
429  cvResetImageROI(img_show);
430  } else {
431  cvResize(img, img_show, CV_INTER_NN);
432  }
433 }
434 
436  //out_matrix(sensor->K, "K");
437  cvShowImage("KalmanVisualize", img_show);
438 }
439 
440 } // namespace alvar
KalmanVisualize(Kalman *_kalman, KalmanSensor *_sensor)
Constructor for full Kalman implementation.
Definition: Kalman.cpp:381
Extended Kalman Filter (EKF) sensor implementation.
Definition: Kalman.h:225
Main ALVAR namespace.
Definition: Alvar.h:174
CvMat * x_tmp1
Definition: Kalman.h:252
Kalman(int _n)
Constructor.
Definition: Kalman.cpp:161
f
~KalmanSensorCore()
Destructor.
Definition: Kalman.cpp:59
virtual void update_K(CvMat *P_pred)
Method for updating the Kalman gain. This is called from predict_update() of Kalman.
Definition: Kalman.cpp:134
KalmanCore(const KalmanCore &s)
Copy constructor.
Definition: Kalman.cpp:82
Core implementation for Kalman.
Definition: Kalman.h:80
virtual void predict_x(unsigned long tick)
Definition: Kalman.cpp:276
int prev_tick
Definition: Kalman.h:180
CvMat * x_plus
Definition: Kalman.h:250
KalmanSensorEkf(const KalmanSensorEkf &k)
Definition: Kalman.cpp:229
virtual void update_H(CvMat *x_pred)
Method for updating how the Kalman state vector is mapped into this sensor&#39;s measurements vector...
Definition: Kalman.cpp:200
CvMat * predict_update(KalmanSensor *sensor, unsigned long tick)
Predict the Kalman state vector for the given time step and update the state using the Kalman gain...
Definition: Kalman.cpp:185
virtual void update_x(CvMat *x_pred, CvMat *x)
Method for updating the state estimate x This is called from predict_update() of Kalman. In KalmanSensorCore and in KalmanSensor this update is made linearly but KalmanSensorEkf will override this method to use unlinear estimation.
Definition: Kalman.cpp:69
CvMat * K
The matrix (n*m) containing Kalman gain (something between 0 and H^-1). In this core-implementation w...
Definition: Kalman.h:56
CvMat * predict_update(KalmanSensorCore *sensor)
Predict the Kalman state vector and update the state using the constant Kalman gain. x = x_pred + K* ( z - H*x_pred)
Definition: Kalman.cpp:110
~KalmanVisualize()
Destructor.
Definition: Kalman.cpp:397
CvMat * P
The error covariance matrix describing the accuracy of the state estimate.
Definition: Kalman.h:184
TFSIMD_FORCE_INLINE const tfScalar & y() const
virtual CvMat * predict()
Predict the Kalman state vector for the given time step . x_pred = F * x.
Definition: Kalman.cpp:105
CvMat * P_pred
The predicted error covariance matrix.
Definition: Kalman.h:188
Core implementation for Kalman sensor.
Definition: Kalman.h:39
CvMat * R
The covariance matrix for the observation noise.
Definition: Kalman.h:124
CvMat * delta
Definition: Kalman.h:249
virtual void update_F(unsigned long tick)
Definition: Kalman.cpp:253
Kalman sensor implementation.
Definition: Kalman.h:118
KalmanSensorCore(const KalmanSensorCore &k)
Copy constructor.
Definition: Kalman.cpp:35
~Kalman()
Destructor.
Definition: Kalman.cpp:168
virtual void update_F(unsigned long tick)
Definition: Kalman.cpp:174
This file implements a versatile Kalman filter.
virtual void update_P(CvMat *P_pred, CvMat *P)
Method for updating the error covariance matrix describing the accuracy of the state estimate...
Definition: Kalman.cpp:145
CvMat * F_trans
Definition: Kalman.h:85
CvMat * x_minus
Definition: Kalman.h:251
void show()
Show the genrated visualization image.
Definition: Kalman.cpp:435
TFSIMD_FORCE_INLINE const tfScalar & x() const
~KalmanCore()
Destructor.
Definition: Kalman.cpp:98
Drawable d[32]
CvMat * H
The matrix (m*n) mapping Kalman state vector into this sensor&#39;s measurements vector.
Definition: Kalman.h:52
CvMat * x_tmp2
Definition: Kalman.h:253
CvMat * F
The matrix (n*n) containing the transition model for the internal state.
Definition: Kalman.h:91
This file implements generic utility functions and a serialization interface.
TFSIMD_FORCE_INLINE const tfScalar & z() const
void update_pre()
Update the visualization image - call this before the Kalman&#39;s predict_update.
Definition: Kalman.cpp:401
KalmanSensor(const KalmanSensor &k)
Copy constructor.
Definition: Kalman.cpp:116
void img_matrix(CvMat *mat, int top, int left)
Add matrix to the image.
Definition: Kalman.cpp:297
void update_post()
Update the visualization image - call this after the Kalman&#39;s predict_update.
Definition: Kalman.cpp:409
unsigned int step
void Init()
Init everything. Called from constructors.
Definition: Kalman.cpp:328
CvMat * x_pred
Predicted state, TODO: should be protected?!
Definition: Kalman.h:113
CvMat * Q
The covariance matrix for the process noise.
Definition: Kalman.h:186
CvMat * z
Latest measurement vector (m*1)
Definition: Kalman.h:50
static void out_matrix(CvMat *m, char *name)
Helper method for outputting matrices (for debug purposes)
Definition: Kalman.cpp:356
CvMat * x
The Kalman state vector (n*1)
Definition: Kalman.h:89
~KalmanSensor()
Destructor.
Definition: Kalman.cpp:128
void predict_P()
Definition: Kalman.cpp:153
virtual void update_x(CvMat *x_pred, CvMat *x)
Method for updating the state estimate x This is called from predict_update() of Kalman. In KalmanSensorCore and in KalmanSensor this update is made linearly but KalmanSensorEkf will override this method to use unlinear estimation.
Definition: Kalman.cpp:221
virtual void update_H(CvMat *x_pred)
Method for updating how the Kalman state vector is mapped into this sensor&#39;s measurements vector...
Definition: Kalman.h:139
double seconds_since_update(unsigned long tick)
Helper method.
Definition: Kalman.cpp:195
This file defines library export definitions, version numbers and build information.
Kalman implementation.
Definition: Kalman.h:178
KalmanEkf(int _n)
Definition: Kalman.cpp:281
virtual void predict_x(unsigned long tick)
Definition: Kalman.cpp:77


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Jun 6 2019 19:27:23