PARP Research Group University of Murcia, Spain


examples/OpenCV/kalmanExample/kalmanExample.cpp

Go to the documentation of this file.
00001 /*
00002  *      Copyright (C) 2009. 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 
00039 #include <stdio.h>
00040 #include <stdlib.h>
00041 #include <iostream>
00042 
00043 #include <QDebug>
00044 #include <QVApplication>
00045 #include <QVDefaultGUI>
00046 
00047 #include <QVNumericPlot>
00048 #include <QVVector>
00049 #include <QVMatrix>
00050 
00051 #include <qvmath.h>
00052 
00053 #ifndef DOXYGEN_IGNORE_THIS
00054 
00055 #define MEASUREMENT_NOISE_COV   0.5
00056 #define PROCESS_NOISE_COV       0.1
00057 
00058 class QVKalmanFilter
00059         {
00060         private:
00061                 CvKalman* kalman;
00062 
00063         public:
00064                 QVKalmanFilter( const int dynamParams, const int measureParams, const QVMatrix &transitionMatrix,
00065                                 const double processNoiseCovariance, const double measurementNoiseCovariance):
00066                         kalman(cvCreateKalman(dynamParams, measureParams, 0))
00067                         {
00068                         CvMat *transitionCvMatrix = transitionMatrix.toCvMat(CV_32F);
00069                         cvCopy(transitionCvMatrix, kalman->transition_matrix);
00070 
00071                         cvReleaseMat(&transitionCvMatrix);
00072                         
00073                         cvSetIdentity(kalman->process_noise_cov,        cvRealScalar(processNoiseCovariance));
00074                         cvSetIdentity(kalman->measurement_noise_cov,    cvRealScalar(measurementNoiseCovariance));
00075                         cvSetIdentity(kalman->measurement_matrix,       cvRealScalar(1));
00076                         cvSetIdentity(kalman->error_cov_post,           cvRealScalar(1));
00077 
00078                         cvZero( kalman->state_post );
00079                         }
00080 
00081                 const QVVector predict() const
00082                         {
00083                         return QVMatrix(cvKalmanPredict( kalman, 0 ));
00084                         }
00085 
00086                 void correct(const QVVector &z)
00087                         {
00088                         CvMat* z_k = z.toColumnMatrix().toCvMat(CV_32F);
00089                         cvKalmanCorrect( kalman, z_k );
00090                         cvReleaseMat(&z_k);
00091                         }
00092 
00093         static const QVMatrix linearMotionTransitionMatrix(const int size = 1, const double step = 0.1)
00094                 {
00095                 QVMatrix transitionMatrix = QVMatrix::identity(2*size);
00096                 for (int i = 0; i < size; i++)
00097                         transitionMatrix(i,size+i) = step;
00098         
00099                 return transitionMatrix;
00100                 }
00101         };
00102 
00103 
00104 class KalmanFilterWorker: public QVWorker
00105         {
00106         private:
00107                 QVKalmanFilter kalmanFilter;
00108         
00109         public:
00110                 KalmanFilterWorker(const QString name): QVWorker(name),
00111                         kalmanFilter(2, 1, QVKalmanFilter::linearMotionTransitionMatrix(), PROCESS_NOISE_COV, MEASUREMENT_NOISE_COV)
00112                         {
00113                         addProperty<double>("Kalman[sin(x) + noise]", outputFlag);
00114                         addProperty<double>("sin(x)", outputFlag);
00115                         addProperty<double>("sin(x) + noise", outputFlag);
00116 
00117                         addProperty<double>("Kalman error", outputFlag);
00118                         addProperty<double>("Measurement error", outputFlag);
00119                         }
00120 
00121                 void iterate()
00122                         {
00123                         // Read sensor, actualize real state.
00124                         const double    realState = 1 - cos((double) getIteration() / 512.0),
00125                                         measurementNoise = ((double)(rand()%2000)/1000.0 - 1.0) * sqrt(MEASUREMENT_NOISE_COV),
00126                                         processNoise = ((double)(rand()%2000)/1000.0 - 1.0) * sqrt(PROCESS_NOISE_COV);
00127 
00128                         // Predict and correct Kalman filter state
00129                         const QVVector y = kalmanFilter.predict();
00130                         kalmanFilter.correct(QVVector(1, realState + processNoise + measurementNoise));
00131 
00132                         // Publish results
00133                         setPropertyValue<double>("Kalman[sin(x) + noise]", y[0]);
00134                         setPropertyValue<double>("sin(x)", realState + processNoise);
00135                         setPropertyValue<double>("sin(x) + noise", realState + processNoise + measurementNoise);
00136                         setPropertyValue<double>("Kalman error", ABS(realState - y[0]));
00137                         setPropertyValue<double>("Measurement error", ABS(processNoise + measurementNoise));
00138                         }
00139         };
00140 
00141 #include <QVYUV4MPEG2Recorder>
00142 #include <QVNumericPlot>
00143 
00144 int main(int argc, char *argv[])
00145         {
00146         QVApplication app(argc, argv,
00147                 "Example program for QVision library. Applies Kalman filtering to an input function."
00148                 );
00149 
00150         KalmanFilterWorker kalmanFilterWorker("Corners Worker");
00151 
00152         QVDefaultGUI interface;
00153 
00154         QVNumericPlot measurePlot("Obtained values");
00155         kalmanFilterWorker.linkProperty("Kalman[sin(x) + noise]", measurePlot);
00156         kalmanFilterWorker.linkProperty("sin(x)", measurePlot);
00157         kalmanFilterWorker.linkProperty("sin(x) + noise", measurePlot);
00158 
00159         QVNumericPlot errorPlot("Estimation errors");
00160         kalmanFilterWorker.linkProperty("Kalman error", errorPlot);
00161         kalmanFilterWorker.linkProperty("Measurement error", errorPlot);
00162 
00163         return app.exec();
00164 
00165         }
00166 
00167 #endif
00168 



QVision framework. PARP research group, copyright 2007, 2008.