git.maemo.org Git - opencv/blob - cv/src/cvkalman.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 #include "_cv.h"
  42
  43
  44 CV_IMPL CvKalman*
  45 cvCreateKalman( int DP, int MP, int CP )
  46 {
  47     CvKalman *kalman = 0;
  48
  49     CV_FUNCNAME( "cvCreateKalman" );
  50
  51     __BEGIN__;
  52
  53     if( DP <= 0 || MP <= 0 )
  54         CV_ERROR( CV_StsOutOfRange,
  55         "state and measurement vectors must have positive number of dimensions" );
  56
  57     if( CP < 0 )
  58         CP = DP;
  59
  60     /* allocating memory for the structure */
  61     CV_CALL( kalman = (CvKalman *)cvAlloc( sizeof( CvKalman )));
  62     memset( kalman, 0, sizeof(*kalman));
  63
  64     kalman->DP = DP;
  65     kalman->MP = MP;
  66     kalman->CP = CP;
  67
  68     CV_CALL( kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 ));
  69     cvZero( kalman->state_pre );
  70
  71     CV_CALL( kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 ));
  72     cvZero( kalman->state_post );
  73
  74     CV_CALL( kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 ));
  75     cvSetIdentity( kalman->transition_matrix );
  76
  77     CV_CALL( kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 ));
  78     cvSetIdentity( kalman->process_noise_cov );
  79
  80     CV_CALL( kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 ));
  81     cvZero( kalman->measurement_matrix );
  82
  83     CV_CALL( kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 ));
  84     cvSetIdentity( kalman->measurement_noise_cov );
  85
  86     CV_CALL( kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 ));
  87
  88     CV_CALL( kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 ));
  89     cvZero( kalman->error_cov_post );
  90
  91     CV_CALL( kalman->gain = cvCreateMat( DP, MP, CV_32FC1 ));
  92
  93     if( CP > 0 )
  94     {
  95         CV_CALL( kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 ));
  96         cvZero( kalman->control_matrix );
  97     }
  98
  99     CV_CALL( kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 ));
 100     CV_CALL( kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 ));
 101     CV_CALL( kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 ));
 102     CV_CALL( kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 ));
 103     CV_CALL( kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 ));
 104
 105 #if 1
 106     kalman->PosterState = kalman->state_pre->data.fl;
 107     kalman->PriorState = kalman->state_post->data.fl;
 108     kalman->DynamMatr = kalman->transition_matrix->data.fl;
 109     kalman->MeasurementMatr = kalman->measurement_matrix->data.fl;
 110     kalman->MNCovariance = kalman->measurement_noise_cov->data.fl;
 111     kalman->PNCovariance = kalman->process_noise_cov->data.fl;
 112     kalman->KalmGainMatr = kalman->gain->data.fl;
 113     kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl;
 114     kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl;
 115 #endif
 116
 117     __END__;
 118
 119     if( cvGetErrStatus() < 0 )
 120         cvReleaseKalman( &kalman );
 121
 122     return kalman;
 123 }
 124
 125
 126 CV_IMPL void
 127 cvReleaseKalman( CvKalman** _kalman )
 128 {
 129     CvKalman *kalman;
 130
 131     CV_FUNCNAME( "cvReleaseKalman" );
 132     __BEGIN__;
 133
 134     if( !_kalman )
 135         CV_ERROR( CV_StsNullPtr, "" );
 136
 137     kalman = *_kalman;
 138
 139     /* freeing the memory */
 140     cvReleaseMat( &kalman->state_pre );
 141     cvReleaseMat( &kalman->state_post );
 142     cvReleaseMat( &kalman->transition_matrix );
 143     cvReleaseMat( &kalman->control_matrix );
 144     cvReleaseMat( &kalman->measurement_matrix );
 145     cvReleaseMat( &kalman->process_noise_cov );
 146     cvReleaseMat( &kalman->measurement_noise_cov );
 147     cvReleaseMat( &kalman->error_cov_pre );
 148     cvReleaseMat( &kalman->gain );
 149     cvReleaseMat( &kalman->error_cov_post );
 150     cvReleaseMat( &kalman->temp1 );
 151     cvReleaseMat( &kalman->temp2 );
 152     cvReleaseMat( &kalman->temp3 );
 153     cvReleaseMat( &kalman->temp4 );
 154     cvReleaseMat( &kalman->temp5 );
 155
 156     memset( kalman, 0, sizeof(*kalman));
 157
 158     /* deallocating the structure */
 159     cvFree( _kalman );
 160
 161     __END__;
 162 }
 163
 164
 165 CV_IMPL const CvMat*
 166 cvKalmanPredict( CvKalman* kalman, const CvMat* control )
 167 {
 168     CvMat* result = 0;
 169
 170     CV_FUNCNAME( "cvKalmanPredict" );
 171
 172     __BEGIN__;
 173
 174     if( !kalman )
 175         CV_ERROR( CV_StsNullPtr, "" );
 176
 177     /* update the state */
 178     /* x'(k) = A*x(k) */
 179     CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre ));
 180
 181     if( control && kalman->CP > 0 )
 182         /* x'(k) = x'(k) + B*u(k) */
 183         CV_CALL( cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre ));
 184
 185     /* update error covariance matrices */
 186     /* temp1 = A*P(k) */
 187     CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 ));
 188
 189     /* P'(k) = temp1*At + Q */
 190     CV_CALL( cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1,
 191                      kalman->error_cov_pre, CV_GEMM_B_T ));
 192
 193     result = kalman->state_pre;
 194
 195     __END__;
 196
 197     return result;
 198 }
 199
 200
 201 CV_IMPL const CvMat*
 202 cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement )
 203 {
 204     CvMat* result = 0;
 205
 206     CV_FUNCNAME( "cvKalmanCorrect" );
 207
 208     __BEGIN__;
 209
 210     if( !kalman || !measurement )
 211         CV_ERROR( CV_StsNullPtr, "" );
 212
 213     /* temp2 = H*P'(k) */
 214     CV_CALL( cvMatMulAdd( kalman->measurement_matrix,
 215                           kalman->error_cov_pre, 0, kalman->temp2 ));
 216     /* temp3 = temp2*Ht + R */
 217     CV_CALL( cvGEMM( kalman->temp2, kalman->measurement_matrix, 1,
 218                      kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T ));
 219
 220     /* temp4 = inv(temp3)*temp2 = Kt(k) */
 221     CV_CALL( cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD ));
 222
 223     /* K(k) */
 224     CV_CALL( cvTranspose( kalman->temp4, kalman->gain ));
 225
 226     /* temp5 = z(k) - H*x'(k) */
 227     CV_CALL( cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 ));
 228
 229     /* x(k) = x'(k) + K(k)*temp5 */
 230     CV_CALL( cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post ));
 231
 232     /* P(k) = P'(k) - K(k)*temp2 */
 233     CV_CALL( cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1,
 234                      kalman->error_cov_post, 0 ));
 235
 236     result = kalman->state_post;
 237
 238     __END__;
 239
 240     return result;
 241 }