QVision: Qt's Image, Video and Computer Vision Library

00001 /*
00002  *      Copyright (C) 2010, 2011, 2012. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00024 
00025 #ifndef QVUKF_H
00026 #define QVUKF_H
00027 
00028 #include <iostream>
00029 #include <QVMatrix>
00030 #include <qvmatrixalgebra.h>
00031 #include <QVFunction>
00032 
00041 struct QVUKFstate
00042 {
00044         QVMatrix mean;
00046         QVMatrix covariance;
00047 };
00048 
00057 struct QVUKFweights
00058 {
00060         QVMatrix Wm;
00062         QVMatrix Wc;
00063 };
00064 
00073 class QVUKF
00074         {
00075         private:
00076                 int n;
00077                 double k;
00078                 double alpha;
00079                 double beta;
00080                 struct QVUKFstate currentState;
00081                 struct QVUKFweights weights;
00082                 double discrepance;
00083 
00090                 QVMatrix sigmaPoints(const QVMatrix &X, const QVMatrix &P) const;
00091 
00095                 void computeWeights();
00096 
00102                 QVMatrix computeMean(const QVMatrix &M) const;
00103 
00110                 QVMatrix computeCovariance(const QVMatrix &mean, const QVMatrix &FdeSigma) const;
00111 
00120                 QVMatrix CrossCovariance (const QVMatrix &mean, const QVMatrix &FdeSigma, const QVMatrix &zt, const QVMatrix &Zt) const;
00121 
00122         public:
00129                 QVUKF(double k=0.0, double alpha=0.5, double beta=2.0);
00130 
00135                 QVUKF(const QVUKFstate &state);
00136 
00141                 void setState(const QVUKFstate &state);
00142 
00151                 void update(const QVMatrix &obs, QVFunction<QVVector, QVVector> &g, QVFunction<QVVector, QVVector> &h, const QVMatrix &Rt, const QVMatrix &Qt);
00152 
00157                 QVMatrix getState() const;
00158 
00163                 double getDiscrepance() const;
00164         };
00165 
00166 #endif // QVUKF_H
00167
src/qvmath/qvukf.h
