vcs.maemo.org Git - opencv/blob - src/cv/cvkalman.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////\r
   2 //\r
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
   4 //\r
   5 //  By downloading, copying, installing or using the software you agree to this license.\r
   6 //  If you do not agree to this license, do not download, install,\r
   7 //  copy or use the software.\r
   8 //\r
   9 //\r
  10 //                        Intel License Agreement\r
  11 //                For Open Source Computer Vision Library\r
  12 //\r
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.\r
  14 // Third party copyrights are property of their respective owners.\r
  15 //\r
  16 // Redistribution and use in source and binary forms, with or without modification,\r
  17 // are permitted provided that the following conditions are met:\r
  18 //\r
  19 //   * Redistribution's of source code must retain the above copyright notice,\r
  20 //     this list of conditions and the following disclaimer.\r
  21 //\r
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,\r
  23 //     this list of conditions and the following disclaimer in the documentation\r
  24 //     and/or other materials provided with the distribution.\r
  25 //\r
  26 //   * The name of Intel Corporation may not be used to endorse or promote products\r
  27 //     derived from this software without specific prior written permission.\r
  28 //\r
  29 // This software is provided by the copyright holders and contributors "as is" and\r
  30 // any express or implied warranties, including, but not limited to, the implied\r
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.\r
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,\r
  33 // indirect, incidental, special, exemplary, or consequential damages\r
  34 // (including, but not limited to, procurement of substitute goods or services;\r
  35 // loss of use, data, or profits; or business interruption) however caused\r
  36 // and on any theory of liability, whether in contract, strict liability,\r
  37 // or tort (including negligence or otherwise) arising in any way out of\r
  38 // the use of this software, even if advised of the possibility of such damage.\r
  39 //\r
  40 //M*/\r
  41 #include "_cv.h"\r
  42 \r
  43 \r
  44 CV_IMPL CvKalman*\r
  45 cvCreateKalman( int DP, int MP, int CP )\r
  46 {\r
  47     CvKalman *kalman = 0;\r
  48 \r
  49     CV_FUNCNAME( "cvCreateKalman" );\r
  50     \r
  51     __BEGIN__;\r
  52 \r
  53     if( DP <= 0 || MP <= 0 )\r
  54         CV_ERROR( CV_StsOutOfRange,\r
  55         "state and measurement vectors must have positive number of dimensions" );\r
  56 \r
  57     if( CP < 0 )\r
  58         CP = DP;\r
  59     \r
  60     /* allocating memory for the structure */\r
  61     CV_CALL( kalman = (CvKalman *)cvAlloc( sizeof( CvKalman )));\r
  62     memset( kalman, 0, sizeof(*kalman));\r
  63     \r
  64     kalman->DP = DP;\r
  65     kalman->MP = MP;\r
  66     kalman->CP = CP;\r
  67 \r
  68     CV_CALL( kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 ));\r
  69     cvZero( kalman->state_pre );\r
  70     \r
  71     CV_CALL( kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 ));\r
  72     cvZero( kalman->state_post );\r
  73     \r
  74     CV_CALL( kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 ));\r
  75     cvSetIdentity( kalman->transition_matrix );\r
  76 \r
  77     CV_CALL( kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 ));\r
  78     cvSetIdentity( kalman->process_noise_cov );\r
  79     \r
  80     CV_CALL( kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 ));\r
  81     cvZero( kalman->measurement_matrix );\r
  82 \r
  83     CV_CALL( kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 ));\r
  84     cvSetIdentity( kalman->measurement_noise_cov );\r
  85 \r
  86     CV_CALL( kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 ));\r
  87     \r
  88     CV_CALL( kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 ));\r
  89     cvZero( kalman->error_cov_post );\r
  90 \r
  91     CV_CALL( kalman->gain = cvCreateMat( DP, MP, CV_32FC1 ));\r
  92 \r
  93     if( CP > 0 )\r
  94     {\r
  95         CV_CALL( kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 ));\r
  96         cvZero( kalman->control_matrix );\r
  97     }\r
  98 \r
  99     CV_CALL( kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 ));\r
 100     CV_CALL( kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 ));\r
 101     CV_CALL( kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 ));\r
 102     CV_CALL( kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 ));\r
 103     CV_CALL( kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 ));\r
 104 \r
 105 #if 1\r
 106     kalman->PosterState = kalman->state_pre->data.fl;\r
 107     kalman->PriorState = kalman->state_post->data.fl;\r
 108     kalman->DynamMatr = kalman->transition_matrix->data.fl;\r
 109     kalman->MeasurementMatr = kalman->measurement_matrix->data.fl;\r
 110     kalman->MNCovariance = kalman->measurement_noise_cov->data.fl;\r
 111     kalman->PNCovariance = kalman->process_noise_cov->data.fl;\r
 112     kalman->KalmGainMatr = kalman->gain->data.fl;\r
 113     kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl;\r
 114     kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl;\r
 115 #endif    \r
 116 \r
 117     __END__;\r
 118 \r
 119     if( cvGetErrStatus() < 0 )\r
 120         cvReleaseKalman( &kalman );\r
 121 \r
 122     return kalman;\r
 123 }\r
 124 \r
 125 \r
 126 CV_IMPL void\r
 127 cvReleaseKalman( CvKalman** _kalman )\r
 128 {\r
 129     CvKalman *kalman;\r
 130 \r
 131     CV_FUNCNAME( "cvReleaseKalman" );\r
 132     __BEGIN__;\r
 133     \r
 134     if( !_kalman )\r
 135         CV_ERROR( CV_StsNullPtr, "" );\r
 136     \r
 137     kalman = *_kalman;\r
 138     \r
 139     /* freeing the memory */\r
 140     cvReleaseMat( &kalman->state_pre );\r
 141     cvReleaseMat( &kalman->state_post );\r
 142     cvReleaseMat( &kalman->transition_matrix );\r
 143     cvReleaseMat( &kalman->control_matrix );\r
 144     cvReleaseMat( &kalman->measurement_matrix );\r
 145     cvReleaseMat( &kalman->process_noise_cov );\r
 146     cvReleaseMat( &kalman->measurement_noise_cov );\r
 147     cvReleaseMat( &kalman->error_cov_pre );\r
 148     cvReleaseMat( &kalman->gain );\r
 149     cvReleaseMat( &kalman->error_cov_post );\r
 150     cvReleaseMat( &kalman->temp1 );\r
 151     cvReleaseMat( &kalman->temp2 );\r
 152     cvReleaseMat( &kalman->temp3 );\r
 153     cvReleaseMat( &kalman->temp4 );\r
 154     cvReleaseMat( &kalman->temp5 );\r
 155 \r
 156     memset( kalman, 0, sizeof(*kalman));\r
 157 \r
 158     /* deallocating the structure */\r
 159     cvFree( _kalman );\r
 160 \r
 161     __END__;\r
 162 }\r
 163 \r
 164 \r
 165 CV_IMPL const CvMat*\r
 166 cvKalmanPredict( CvKalman* kalman, const CvMat* control )\r
 167 {\r
 168     CvMat* result = 0;\r
 169     \r
 170     CV_FUNCNAME( "cvKalmanPredict" );\r
 171 \r
 172     __BEGIN__;\r
 173     \r
 174     if( !kalman )\r
 175         CV_ERROR( CV_StsNullPtr, "" );\r
 176 \r
 177     /* update the state */\r
 178     /* x'(k) = A*x(k) */\r
 179     CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre ));\r
 180 \r
 181     if( control && kalman->CP > 0 )\r
 182         /* x'(k) = x'(k) + B*u(k) */\r
 183         CV_CALL( cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre ));\r
 184     \r
 185     /* update error covariance matrices */\r
 186     /* temp1 = A*P(k) */\r
 187     CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 ));\r
 188     \r
 189     /* P'(k) = temp1*At + Q */\r
 190     CV_CALL( cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1,\r
 191                      kalman->error_cov_pre, CV_GEMM_B_T ));\r
 192 \r
 193     result = kalman->state_pre;\r
 194 \r
 195     __END__;\r
 196 \r
 197     return result;\r
 198 }\r
 199 \r
 200 \r
 201 CV_IMPL const CvMat*\r
 202 cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement )\r
 203 {\r
 204     CvMat* result = 0;\r
 205 \r
 206     CV_FUNCNAME( "cvKalmanCorrect" );\r
 207 \r
 208     __BEGIN__;\r
 209     \r
 210     if( !kalman || !measurement )\r
 211         CV_ERROR( CV_StsNullPtr, "" );\r
 212 \r
 213     /* temp2 = H*P'(k) */\r
 214     CV_CALL( cvMatMulAdd( kalman->measurement_matrix,\r
 215                           kalman->error_cov_pre, 0, kalman->temp2 ));\r
 216     /* temp3 = temp2*Ht + R */\r
 217     CV_CALL( cvGEMM( kalman->temp2, kalman->measurement_matrix, 1,\r
 218                      kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T ));\r
 219 \r
 220     /* temp4 = inv(temp3)*temp2 = Kt(k) */\r
 221     CV_CALL( cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD ));\r
 222 \r
 223     /* K(k) */\r
 224     CV_CALL( cvTranspose( kalman->temp4, kalman->gain ));\r
 225     \r
 226     /* temp5 = z(k) - H*x'(k) */\r
 227     CV_CALL( cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 ));\r
 228 \r
 229     /* x(k) = x'(k) + K(k)*temp5 */\r
 230     CV_CALL( cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post ));\r
 231 \r
 232     /* P(k) = P'(k) - K(k)*temp2 */\r
 233     CV_CALL( cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1,\r
 234                      kalman->error_cov_post, 0 ));\r
 235 \r
 236     result = kalman->state_post;\r
 237 \r
 238     __END__;\r
 239 \r
 240     return result;\r
 241 }\r
 242 \r
 243 namespace cv\r
 244 {\r
 245 \r
 246 KalmanFilter::KalmanFilter() {}\r
 247 KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams)\r
 248 {\r
 249     init(dynamParams, measureParams, controlParams);\r
 250 }\r
 251 \r
 252 void KalmanFilter::init(int DP, int MP, int CP)\r
 253 {\r
 254     CV_Assert( DP > 0 && MP > 0 );\r
 255     CP = std::max(CP, 0);\r
 256 \r
 257     statePre = Mat::zeros(DP, 1, CV_32F);\r
 258     statePost = Mat::zeros(DP, 1, CV_32F);\r
 259     transitionMatrix = Mat::eye(DP, DP, CV_32F);\r
 260 \r
 261     processNoiseCov = Mat::eye(DP, DP, CV_32F);\r
 262     measurementMatrix = Mat::zeros(MP, DP, CV_32F);\r
 263     measurementNoiseCov = Mat::eye(MP, MP, CV_32F);\r
 264 \r
 265     errorCovPre = Mat::zeros(DP, DP, CV_32F);\r
 266     errorCovPost = Mat::zeros(DP, DP, CV_32F);\r
 267     gain = Mat::zeros(DP, MP, CV_32F);\r
 268 \r
 269     if( CP > 0 )\r
 270         controlMatrix = Mat::zeros(DP, CP, CV_32F);\r
 271     else\r
 272         controlMatrix.release();\r
 273 \r
 274     temp1.create(DP, DP, CV_32F);\r
 275     temp2.create(MP, DP, CV_32F);\r
 276     temp3.create(MP, MP, CV_32F);\r
 277     temp4.create(MP, DP, CV_32F);\r
 278     temp5.create(MP, 1, CV_32F);\r
 279 }\r
 280 \r
 281 const Mat& KalmanFilter::predict(const Mat& control)\r
 282 {\r
 283     // update the state: x'(k) = A*x(k)\r
 284     statePre = transitionMatrix*statePost;\r
 285 \r
 286     if( control.data )\r
 287         // x'(k) = x'(k) + B*u(k)\r
 288         statePre += controlMatrix*control;\r
 289 \r
 290     // update error covariance matrices: temp1 = A*P(k)\r
 291     temp1 = transitionMatrix*errorCovPost;\r
 292 \r
 293     // P'(k) = temp1*At + Q\r
 294     gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);\r
 295 \r
 296     return statePre;\r
 297 }\r
 298 \r
 299 const Mat& KalmanFilter::correct(const Mat& measurement)\r
 300 {\r
 301     // temp2 = H*P'(k)\r
 302     temp2 = measurementMatrix * errorCovPre;\r
 303 \r
 304     // temp3 = temp2*Ht + R\r
 305     gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T); \r
 306 \r
 307     // temp4 = inv(temp3)*temp2 = Kt(k)\r
 308     solve(temp3, temp2, temp4, DECOMP_SVD);\r
 309 \r
 310     // K(k)\r
 311     gain = temp4.t();\r
 312     \r
 313     // temp5 = z(k) - H*x'(k)\r
 314     temp5 = measurement - measurementMatrix*statePre;\r
 315 \r
 316     // x(k) = x'(k) + K(k)*temp5\r
 317     statePost = statePre + gain*temp5;\r
 318 \r
 319     // P(k) = P'(k) - K(k)*temp2\r
 320     errorCovPost = errorCovPre - gain*temp2;\r
 321 \r
 322     return statePost;\r
 323 }\r
 324     \r
 325 };\r