UnscentedKalman.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 __UNSCENTED_KALMAN__
00025 #define __UNSCENTED_KALMAN__
00026 
00027 #include "Alvar.h"
00028 #include "cxcore.h"
00029 
00036 namespace alvar {
00037 
00038   class UnscentedProcess;
00039   class UnscentedObservation;
00040 
00096   class ALVAR_EXPORT UnscentedKalman {
00097   private:
00098     int state_n;
00099     int state_k;
00100     int obs_n;
00101     int sigma_n;
00102     bool sigmasUpdated;
00103         double lambda, lambda2;
00104 
00105     CvMat *state;
00106     CvMat *stateCovariance;
00107     CvMat *sqrtStateCovariance;
00108     CvMat *stateD;
00109     CvMat *stateU;
00110     CvMat *stateV;
00111     CvMat *stateTmp;
00112     CvMat *stateDiff;
00113 
00114     CvMat *predObs;
00115     CvMat *predObsCovariance;
00116     CvMat *invPredObsCovariance;
00117     CvMat *predObsDiff;
00118 
00119     CvMat *statePredObsCrossCorrelation;
00120     CvMat *kalmanGain;
00121     CvMat *kalmanTmp;
00122 
00123     CvMat **sigma_state;
00124     CvMat **sigma_predObs;
00125 
00126     // possess state mean and co-variance (as a list of sigma points).
00127     // generate sigma points from state mean vector and co-variance matrix.
00128     // compute state mean vector and co-variance matrix from sigma points.
00129 
00130     // predict: 
00131     //  - map sigma points thru process model f.
00132 
00133     // update:
00134     //  - map sigma points thru h.
00135     //  - from current sigma points and sigma observations:
00136     //   - compute state estimate x and co-variance P.
00137     //   - compute predicted observation z and innocation co-variance Z
00138     //   - compute cross correlation XZ
00139     //  - compute new state mean and co-variance.
00140     //  - generate new sigma points.
00141   public:
00142 
00160     UnscentedKalman(int state_n, int obs_n, int state_k = 0, double alpha = 0.001, double beta = 2.0);
00161     ~UnscentedKalman();
00162 
00173     CvMat *getState() { return state; }
00174 
00184     CvMat *getStateCovariance() { return stateCovariance; }
00185 
00191     void initialize();
00192 
00201     void predict(UnscentedProcess *process_model);
00202 
00213     void update(UnscentedObservation *observation);
00214   };
00215 
00221   class ALVAR_EXPORT UnscentedProcess {
00222   public:
00231     virtual void f(CvMat *state) = 0;
00232 
00242     virtual CvMat *getProcessNoise() = 0;
00243   };
00244 
00252   class ALVAR_EXPORT UnscentedObservation {
00253   public:
00261     virtual void h(CvMat *z, CvMat *state) = 0;
00262 
00272     virtual CvMat *getObservation() = 0;
00273 
00284     virtual CvMat *getObservationNoise() = 0;
00285   };
00286 
00287 } // namespace alvar
00288 
00289 #endif // __UNSCENTED_KALMAN__


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Sun Oct 5 2014 22:16:26