My Project
Kalman.h
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 #ifndef KALMAN_H
25 #define KALMAN_H
26 
33 #include "Alvar.h"
34 
35 #include <opencv2/core.hpp>
36 
37 namespace alvar {
38 
40 class ALVAR_EXPORT KalmanSensorCore
41 {
42  friend class KalmanVisualize;
43 
44 protected:
45  int n;
46  int m;
47  cv::Mat H_trans;
48  cv::Mat z_pred;
49  cv::Mat z_residual;
50  cv::Mat x_gain;
51 
52 public:
54  cv::Mat z;
56  cv::Mat H;
60  cv::Mat K;
68  KalmanSensorCore(int _n, int _m);
72  int
74  {
75  return n;
76  }
78  int
80  {
81  return m;
82  }
88  virtual void update_x(const cv::Mat &x_pred, cv::Mat &x);
89 };
90 
92 class ALVAR_EXPORT KalmanCore
93 {
94  friend class KalmanVisualize;
95 
96 protected:
97  int n;
98  //cv::Mat x_pred;
99  cv::Mat F_trans;
100  virtual void predict_x(unsigned long tick);
101 
102 public:
104  cv::Mat x;
106  cv::Mat F;
113  KalmanCore(int _n);
117  int
119  {
120  return n;
121  }
125  virtual cv::Mat &predict();
129  cv::Mat &predict_update(KalmanSensorCore *sensor);
130 
132  cv::Mat x_pred;
133 };
134 
136 class ALVAR_EXPORT KalmanSensor : public KalmanSensorCore
137 {
138 protected:
139  cv::Mat R_tmp;
140  cv::Mat P_tmp;
141 
142 public:
144  cv::Mat R;
152  KalmanSensor(int n, int _m);
159  virtual void
160  update_H(const cv::Mat &x_pred)
161  {
162  }
166  virtual void update_K(const cv::Mat &P_pred);
170  virtual void update_P(const cv::Mat &P_pred, cv::Mat &P);
171 };
172 
201 class ALVAR_EXPORT Kalman : public KalmanCore
202 {
203 protected:
204  int prev_tick;
205  void predict_P();
206 
207 public:
209  cv::Mat P;
211  cv::Mat Q;
213  cv::Mat P_pred;
219  Kalman(int _n);
225  virtual void update_F(unsigned long tick);
232  cv::Mat &predict(unsigned long tick);
239  cv::Mat &predict_update(KalmanSensor *sensor, unsigned long tick);
241  double seconds_since_update(unsigned long tick);
242 };
243 
250 class ALVAR_EXPORT KalmanSensorEkf : public KalmanSensor
251 {
252 protected:
253  cv::Mat delta;
254  cv::Mat x_plus;
255  cv::Mat x_minus;
256  cv::Mat z_tmp1;
257  cv::Mat z_tmp2;
258  virtual void h(const cv::Mat &x_pred, cv::Mat &_z_pred) = 0;
259  virtual void update_H(const cv::Mat &x_pred);
260  virtual void update_x(const cv::Mat &x_pred, cv::Mat &x);
261 
262 public:
264  KalmanSensorEkf(int _n, int _m);
265  ~KalmanSensorEkf();
266 };
267 
274 class ALVAR_EXPORT KalmanEkf : public Kalman
275 {
276 protected:
277  cv::Mat delta;
278  cv::Mat x_plus;
279  cv::Mat x_minus;
280  cv::Mat x_tmp1;
281  cv::Mat x_tmp2;
282  virtual void f(const cv::Mat &_x, cv::Mat &_x_pred, double dt) = 0;
283  virtual void update_F(unsigned long tick);
284  virtual void predict_x(unsigned long tick);
285 
286 public:
287  KalmanEkf(int _n);
288  ~KalmanEkf();
289 };
290 
303 class ALVAR_EXPORT KalmanVisualize
304 {
305  int n;
306  int m;
307  KalmanCore * kalman;
308  KalmanSensorCore *sensor;
309  Kalman * kalman_ext;
310  KalmanSensor * sensor_ext;
312  cv::Mat img;
314  cv::Mat img_legend;
316  cv::Mat img_show;
318  int img_scale;
320  void img_matrix(const cv::Mat &mat, int top, int left);
322  void Init();
323 
324 public:
326  static void out_matrix(const cv::Mat &m, char *name);
328  KalmanVisualize(Kalman *_kalman, KalmanSensor *_sensor);
334  void update_pre();
336  void update_post();
338  void show();
339 };
340 
341 } // namespace alvar
342 
343 #endif
This file defines library export definitions, version numbers and build information.
Core implementation for Kalman.
Definition: Kalman.h:93
cv::Mat x_pred
Predicted state, TODO: should be protected?!
Definition: Kalman.h:132
virtual cv::Mat & predict()
Predict the Kalman state vector for the given time step . x_pred = F * x.
KalmanCore(int _n)
Constructor.
cv::Mat x
The Kalman state vector (n*1)
Definition: Kalman.h:104
KalmanCore(const KalmanCore &s)
Copy constructor.
~KalmanCore()
Destructor.
cv::Mat F
The matrix (n*n) containing the transition model for the internal state.
Definition: Kalman.h:106
cv::Mat & predict_update(KalmanSensorCore *sensor)
Predict the Kalman state vector and update the state using the constant Kalman gain....
int get_n()
Accessor for n.
Definition: Kalman.h:118
Extended Kalman Filter (EKF) implementation.
Definition: Kalman.h:275
virtual void update_F(unsigned long tick)
Kalman implementation.
Definition: Kalman.h:202
cv::Mat & 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.
double seconds_since_update(unsigned long tick)
Helper method.
cv::Mat P
The error covariance matrix describing the accuracy of the state estimate.
Definition: Kalman.h:209
~Kalman()
Destructor.
cv::Mat P_pred
The predicted error covariance matrix.
Definition: Kalman.h:213
Kalman(int _n)
Constructor.
cv::Mat & predict(unsigned long tick)
Predict the Kalman state vector for the given time step This calls updateF for updating the transitio...
cv::Mat Q
The covariance matrix for the process noise.
Definition: Kalman.h:211
virtual void update_F(unsigned long tick)
Core implementation for Kalman sensor.
Definition: Kalman.h:41
KalmanSensorCore(int _n, int _m)
Constructor.
KalmanSensorCore(const KalmanSensorCore &k)
Copy constructor.
cv::Mat z
Latest measurement vector (m*1)
Definition: Kalman.h:54
cv::Mat H
The matrix (m*n) mapping Kalman state vector into this sensor's measurements vector.
Definition: Kalman.h:56
cv::Mat K
The matrix (n*m) containing Kalman gain (something between 0 and H^-1). In this core-implementation w...
Definition: Kalman.h:60
virtual void update_x(const cv::Mat &x_pred, cv::Mat &x)
Method for updating the state estimate x This is called from predict_update() of Kalman....
~KalmanSensorCore()
Destructor.
int get_m()
Accessor for m.
Definition: Kalman.h:79
int get_n()
Accessor for n.
Definition: Kalman.h:73
Extended Kalman Filter (EKF) sensor implementation.
Definition: Kalman.h:251
virtual void update_H(const cv::Mat &x_pred)
Method for updating how the Kalman state vector is mapped into this sensor's measurements vector....
virtual void update_x(const cv::Mat &x_pred, cv::Mat &x)
Method for updating the state estimate x This is called from predict_update() of Kalman....
Kalman sensor implementation.
Definition: Kalman.h:137
~KalmanSensor()
Destructor.
cv::Mat R
The covariance matrix for the observation noise.
Definition: Kalman.h:144
virtual void update_H(const cv::Mat &x_pred)
Method for updating how the Kalman state vector is mapped into this sensor's measurements vector....
Definition: Kalman.h:160
virtual void update_P(const cv::Mat &P_pred, cv::Mat &P)
Method for updating the error covariance matrix describing the accuracy of the state estimate....
KalmanSensor(int n, int _m)
Constructor.
KalmanSensor(const KalmanSensor &k)
Copy constructor.
virtual void update_K(const cv::Mat &P_pred)
Method for updating the Kalman gain. This is called from predict_update() of Kalman.
Class for visualizing Kalman filter.
Definition: Kalman.h:304
void update_post()
Update the visualization image - call this after the Kalman's predict_update.
void show()
Show the genrated visualization image.
~KalmanVisualize()
Destructor.
KalmanVisualize(KalmanCore *_kalman, KalmanSensorCore *_sensor)
Constructor for core Kalman implementation (not all visualizations possible)
void update_pre()
Update the visualization image - call this before the Kalman's predict_update.
KalmanVisualize(Kalman *_kalman, KalmanSensor *_sensor)
Constructor for full Kalman implementation.
static void out_matrix(const cv::Mat &m, char *name)
Helper method for outputting matrices (for debug purposes)
Main ALVAR namespace.
Definition: Alvar.h:174