git.maemo.org Git - opencv/blob - tests/cv/src/akalman.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "cvtest.h"
  43
  44 class CV_KalmanTest : public CvTest
  45 {
  46 public:
  47     CV_KalmanTest();
  48 protected:
  49     void run(int);
  50 };
  51
  52
  53 CV_KalmanTest::CV_KalmanTest():
  54     CvTest( "kalman", "cvKalmanPredict, cvKalmanCorrect" )
  55 {
  56     support_testing_modes = CvTS::CORRECTNESS_CHECK_MODE;
  57 }
  58
  59 void CV_KalmanTest::run( int )
  60 {
  61     int code = CvTS::OK;
  62     const int Dim = 7;
  63     const int Steps = 100;
  64     const double max_init = 1;
  65     const double max_noise = 0.1;
  66
  67     const double EPSILON = 1.000;
  68     CvRNG* rng = ts->get_rng();
  69     CvKalman* Kalm;
  70     int i, j;
  71
  72     CvMat* Sample = cvCreateMat(Dim,1,CV_32F);
  73     CvMat* Temp = cvCreateMat(Dim,1,CV_32F);
  74
  75     Kalm = cvCreateKalman(Dim, Dim);
  76     CvMat Dyn = cvMat(Dim,Dim,CV_32F,Kalm->DynamMatr);
  77     CvMat Mes = cvMat(Dim,Dim,CV_32F,Kalm->MeasurementMatr);
  78     CvMat PNC = cvMat(Dim,Dim,CV_32F,Kalm->PNCovariance);
  79     CvMat MNC = cvMat(Dim,Dim,CV_32F,Kalm->MNCovariance);
  80     CvMat PriErr = cvMat(Dim,Dim,CV_32F,Kalm->PriorErrorCovariance);
  81     CvMat PostErr = cvMat(Dim,Dim,CV_32F,Kalm->PosterErrorCovariance);
  82     CvMat PriState = cvMat(Dim,1,CV_32F,Kalm->PriorState);
  83     CvMat PostState = cvMat(Dim,1,CV_32F,Kalm->PosterState);
  84     cvSetIdentity(&PNC);
  85     cvSetIdentity(&PriErr);
  86     cvSetIdentity(&PostErr);
  87     cvSetZero(&MNC);
  88     cvSetZero(&PriState);
  89     cvSetZero(&PostState);
  90     cvSetIdentity(&Mes);
  91     cvSetIdentity(&Dyn);
  92     cvRandArr(rng, Sample, CV_RAND_UNI, cvScalarAll(-max_init), cvScalarAll(max_init));
  93     cvKalmanCorrect(Kalm, Sample);
  94     for(i = 0; i<Steps; i++)
  95     {
  96         cvKalmanPredict(Kalm);
  97         for(j = 0; j<Dim; j++)
  98         {
  99             float t = 0;
 100             for(int k=0; k<Dim; k++)
 101             {
 102                 t += Dyn.data.fl[j*Dim+k]*Sample->data.fl[k];
 103             }
 104             Temp->data.fl[j]= (float)(t+(cvTsRandReal(rng)*2-1)*max_noise);
 105         }
 106         cvCopy( Temp, Sample );
 107         cvKalmanCorrect(Kalm,Temp);
 108     }
 109
 110     code = cvTsCmpEps2( ts, Sample, Kalm->state_post, EPSILON, false, "The final estimated state" );
 111
 112     cvReleaseMat(&Sample);
 113     cvReleaseMat(&Temp);
 114     cvReleaseKalman(&Kalm);
 115
 116     if( code < 0 )
 117         ts->set_failed_test_info( code );
 118 }
 119
 120 CV_KalmanTest kalman_test;
 121
 122
 123 /* End of file. */