Kalman.h
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #ifndef KALMAN_H
00025 #define KALMAN_H
00026 
00033 #include "Alvar.h"
00034 #include "cxcore.h"
00035 
00036 namespace alvar {
00037 
00039 class ALVAR_EXPORT KalmanSensorCore {
00040         friend class KalmanVisualize;
00041 protected:
00042         int n;
00043         int m; 
00044         CvMat *H_trans;
00045         CvMat *z_pred;
00046         CvMat *z_residual;
00047         CvMat *x_gain;
00048 public:
00050         CvMat *z; 
00052         CvMat *H;
00056         CvMat *K;
00058         KalmanSensorCore(const KalmanSensorCore &k);
00064         KalmanSensorCore(int _n, int _m);
00066         ~KalmanSensorCore();
00068         int get_n() { return n; }
00070         int get_m() { return m; }
00076         virtual void update_x(CvMat *x_pred, CvMat *x);
00077 };
00078 
00080 class ALVAR_EXPORT KalmanCore {
00081         friend class KalmanVisualize;
00082 protected:
00083         int n; 
00084         //CvMat *x_pred;
00085         CvMat *F_trans;
00086         virtual void predict_x(unsigned long tick);
00087 public:
00089         CvMat *x;
00091         CvMat *F;
00093         KalmanCore(const KalmanCore &s);
00098         KalmanCore(int _n);
00100         ~KalmanCore();
00102         int get_n() { return n; }
00106         virtual CvMat *predict();
00110         CvMat *predict_update(KalmanSensorCore *sensor);
00111 
00113         CvMat *x_pred;
00114 
00115 };
00116 
00118 class ALVAR_EXPORT KalmanSensor : public KalmanSensorCore {
00119 protected:
00120         CvMat *R_tmp;
00121         CvMat *P_tmp;
00122 public:
00124         CvMat *R;
00126         KalmanSensor(const KalmanSensor &k);
00132         KalmanSensor(int n, int _m);
00134         ~KalmanSensor();
00139         virtual void update_H(CvMat *x_pred) {}
00143         virtual void update_K(CvMat *P_pred);
00147         virtual void update_P(CvMat *P_pred, CvMat *P);
00148 };
00149 
00178 class ALVAR_EXPORT Kalman : public KalmanCore {
00179 protected:
00180         int prev_tick; 
00181         void predict_P();
00182 public:
00184         CvMat *P;
00186         CvMat *Q;
00188         CvMat *P_pred;
00194         Kalman(int _n);
00196         ~Kalman();
00200         virtual void update_F(unsigned long tick);
00207         CvMat *predict(unsigned long tick);
00214         CvMat *predict_update(KalmanSensor *sensor, unsigned long tick);
00216         double seconds_since_update(unsigned long tick);
00217 };
00218 
00225 class ALVAR_EXPORT KalmanSensorEkf : public KalmanSensor {
00226 protected:
00227         CvMat *delta;
00228         CvMat *x_plus;
00229         CvMat *x_minus;
00230         CvMat *z_tmp1;
00231         CvMat *z_tmp2;
00232         virtual void h(CvMat *x_pred, CvMat *_z_pred) = 0;
00233         virtual void update_H(CvMat *x_pred);
00234         virtual void update_x(CvMat *x_pred, CvMat *x);
00235 public:
00236         KalmanSensorEkf(const KalmanSensorEkf &k);
00237         KalmanSensorEkf(int _n, int _m);
00238         ~KalmanSensorEkf();
00239 };
00240 
00247 class ALVAR_EXPORT KalmanEkf : public Kalman {
00248 protected:
00249         CvMat *delta;
00250         CvMat *x_plus;
00251         CvMat *x_minus;
00252         CvMat *x_tmp1;
00253         CvMat *x_tmp2;
00254         virtual void f(CvMat *_x, CvMat *_x_pred, double dt) = 0;
00255         virtual void update_F(unsigned long tick);
00256         virtual void predict_x(unsigned long tick);
00257 public:
00258         KalmanEkf(int _n);
00259         ~KalmanEkf();
00260 };
00261 
00274 class ALVAR_EXPORT KalmanVisualize {
00275         int n;
00276         int m;
00277         KalmanCore *kalman;
00278         KalmanSensorCore *sensor;
00279         Kalman *kalman_ext;
00280         KalmanSensor *sensor_ext;
00282         IplImage *img;
00284         IplImage *img_legend;
00286         IplImage *img_show;
00288         int img_scale;
00290         void img_matrix(CvMat *mat, int top, int left);
00292         void Init();
00293 public:
00295         static void out_matrix(CvMat *m, char *name);
00297         KalmanVisualize(Kalman *_kalman, KalmanSensor *_sensor);
00299         KalmanVisualize(KalmanCore *_kalman, KalmanSensorCore *_sensor);
00301         ~KalmanVisualize();
00303         void update_pre();
00305         void update_post();
00307         void show();
00308 };
00309 
00310 } // namespace alvar 
00311 
00312 #endif


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Sat Dec 28 2013 16:46:15