Kalman.h

Go to the documentation of this file.

/*
 * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
 *
 * Copyright 2007-2012 VTT Technical Research Centre of Finland
 *
 * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
 *          <http://www.vtt.fi/multimedia/alvar.html>
 *
 * ALVAR is free software; you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your option)
 * any later version.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with ALVAR; if not, see
 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
 */

#ifndef KALMAN_H
#define KALMAN_H

#include "Alvar.h"
#include "cxcore.h"

namespace alvar {

class ALVAR_EXPORT KalmanSensorCore {
        friend class KalmanVisualize;
protected:
        int n;
        int m; 
        CvMat *H_trans;
        CvMat *z_pred;
        CvMat *z_residual;
        CvMat *x_gain;
public:
        CvMat *z; 
        CvMat *H;
        CvMat *K;
        KalmanSensorCore(const KalmanSensorCore &k);
        KalmanSensorCore(int _n, int _m);
        ~KalmanSensorCore();
        int get_n() { return n; }
        int get_m() { return m; }
        virtual void update_x(CvMat *x_pred, CvMat *x);
};

class ALVAR_EXPORT KalmanCore {
        friend class KalmanVisualize;
protected:
        int n; 
        //CvMat *x_pred;
        CvMat *F_trans;
        virtual void predict_x(unsigned long tick);
public:
        CvMat *x;
        CvMat *F;
        KalmanCore(const KalmanCore &s);
        KalmanCore(int _n);
        ~KalmanCore();
        int get_n() { return n; }
        virtual CvMat *predict();
        CvMat *predict_update(KalmanSensorCore *sensor);

        CvMat *x_pred;

};

class ALVAR_EXPORT KalmanSensor : public KalmanSensorCore {
protected:
        CvMat *R_tmp;
        CvMat *P_tmp;
public:
        CvMat *R;
        KalmanSensor(const KalmanSensor &k);
        KalmanSensor(int n, int _m);
        ~KalmanSensor();
        virtual void update_H(CvMat *x_pred) {}
        virtual void update_K(CvMat *P_pred);
        virtual void update_P(CvMat *P_pred, CvMat *P);
};

class ALVAR_EXPORT Kalman : public KalmanCore {
protected:
        int prev_tick; 
        void predict_P();
public:
        CvMat *P;
        CvMat *Q;
        CvMat *P_pred;
        Kalman(int _n);
        ~Kalman();
        virtual void update_F(unsigned long tick);
        CvMat *predict(unsigned long tick);
        CvMat *predict_update(KalmanSensor *sensor, unsigned long tick);
        double seconds_since_update(unsigned long tick);
};

class ALVAR_EXPORT KalmanSensorEkf : public KalmanSensor {
protected:
        CvMat *delta;
        CvMat *x_plus;
        CvMat *x_minus;
        CvMat *z_tmp1;
        CvMat *z_tmp2;
        virtual void h(CvMat *x_pred, CvMat *_z_pred) = 0;
        virtual void update_H(CvMat *x_pred);
        virtual void update_x(CvMat *x_pred, CvMat *x);
public:
        KalmanSensorEkf(const KalmanSensorEkf &k);
        KalmanSensorEkf(int _n, int _m);
        ~KalmanSensorEkf();
};

class ALVAR_EXPORT KalmanEkf : public Kalman {
protected:
        CvMat *delta;
        CvMat *x_plus;
        CvMat *x_minus;
        CvMat *x_tmp1;
        CvMat *x_tmp2;
        virtual void f(CvMat *_x, CvMat *_x_pred, double dt) = 0;
        virtual void update_F(unsigned long tick);
        virtual void predict_x(unsigned long tick);
public:
        KalmanEkf(int _n);
        ~KalmanEkf();
};

class ALVAR_EXPORT KalmanVisualize {
        int n;
        int m;
        KalmanCore *kalman;
        KalmanSensorCore *sensor;
        Kalman *kalman_ext;
        KalmanSensor *sensor_ext;
        IplImage *img;
        IplImage *img_legend;
        IplImage *img_show;
        int img_scale;
        void img_matrix(CvMat *mat, int top, int left);
        void Init();
public:
        static void out_matrix(CvMat *m, char *name);
        KalmanVisualize(Kalman *_kalman, KalmanSensor *_sensor);
        KalmanVisualize(KalmanCore *_kalman, KalmanSensorCore *_sensor);
        ~KalmanVisualize();
        void update_pre();
        void update_post();
        void show();
};

} // namespace alvar 

#endif