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 } 242
