--- /dev/null
+/*M///////////////////////////////////////////////////////////////////////////////////////\r
+//\r
+// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.\r
+//\r
+// By downloading, copying, installing or using the software you agree to this license.\r
+// If you do not agree to this license, do not download, install,\r
+// copy or use the software.\r
+//\r
+//\r
+// Intel License Agreement\r
+// For Open Source Computer Vision Library\r
+//\r
+// Copyright (C) 2000, Intel Corporation, all rights reserved.\r
+// Third party copyrights are property of their respective owners.\r
+//\r
+// Redistribution and use in source and binary forms, with or without modification,\r
+// are permitted provided that the following conditions are met:\r
+//\r
+// * Redistribution's of source code must retain the above copyright notice,\r
+// this list of conditions and the following disclaimer.\r
+//\r
+// * Redistribution's in binary form must reproduce the above copyright notice,\r
+// this list of conditions and the following disclaimer in the documentation\r
+// and/or other materials provided with the distribution.\r
+//\r
+// * The name of Intel Corporation may not be used to endorse or promote products\r
+// derived from this software without specific prior written permission.\r
+//\r
+// This software is provided by the copyright holders and contributors "as is" and\r
+// any express or implied warranties, including, but not limited to, the implied\r
+// warranties of merchantability and fitness for a particular purpose are disclaimed.\r
+// In no event shall the Intel Corporation or contributors be liable for any direct,\r
+// indirect, incidental, special, exemplary, or consequential damages\r
+// (including, but not limited to, procurement of substitute goods or services;\r
+// loss of use, data, or profits; or business interruption) however caused\r
+// and on any theory of liability, whether in contract, strict liability,\r
+// or tort (including negligence or otherwise) arising in any way out of\r
+// the use of this software, even if advised of the possibility of such damage.\r
+//\r
+//M*/\r
+#include "_cv.h"\r
+\r
+\r
+CV_IMPL CvKalman*\r
+cvCreateKalman( int DP, int MP, int CP )\r
+{\r
+ CvKalman *kalman = 0;\r
+\r
+ CV_FUNCNAME( "cvCreateKalman" );\r
+ \r
+ __BEGIN__;\r
+\r
+ if( DP <= 0 || MP <= 0 )\r
+ CV_ERROR( CV_StsOutOfRange,\r
+ "state and measurement vectors must have positive number of dimensions" );\r
+\r
+ if( CP < 0 )\r
+ CP = DP;\r
+ \r
+ /* allocating memory for the structure */\r
+ CV_CALL( kalman = (CvKalman *)cvAlloc( sizeof( CvKalman )));\r
+ memset( kalman, 0, sizeof(*kalman));\r
+ \r
+ kalman->DP = DP;\r
+ kalman->MP = MP;\r
+ kalman->CP = CP;\r
+\r
+ CV_CALL( kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 ));\r
+ cvZero( kalman->state_pre );\r
+ \r
+ CV_CALL( kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 ));\r
+ cvZero( kalman->state_post );\r
+ \r
+ CV_CALL( kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 ));\r
+ cvSetIdentity( kalman->transition_matrix );\r
+\r
+ CV_CALL( kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 ));\r
+ cvSetIdentity( kalman->process_noise_cov );\r
+ \r
+ CV_CALL( kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 ));\r
+ cvZero( kalman->measurement_matrix );\r
+\r
+ CV_CALL( kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 ));\r
+ cvSetIdentity( kalman->measurement_noise_cov );\r
+\r
+ CV_CALL( kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 ));\r
+ \r
+ CV_CALL( kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 ));\r
+ cvZero( kalman->error_cov_post );\r
+\r
+ CV_CALL( kalman->gain = cvCreateMat( DP, MP, CV_32FC1 ));\r
+\r
+ if( CP > 0 )\r
+ {\r
+ CV_CALL( kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 ));\r
+ cvZero( kalman->control_matrix );\r
+ }\r
+\r
+ CV_CALL( kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 ));\r
+ CV_CALL( kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 ));\r
+ CV_CALL( kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 ));\r
+ CV_CALL( kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 ));\r
+ CV_CALL( kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 ));\r
+\r
+#if 1\r
+ kalman->PosterState = kalman->state_pre->data.fl;\r
+ kalman->PriorState = kalman->state_post->data.fl;\r
+ kalman->DynamMatr = kalman->transition_matrix->data.fl;\r
+ kalman->MeasurementMatr = kalman->measurement_matrix->data.fl;\r
+ kalman->MNCovariance = kalman->measurement_noise_cov->data.fl;\r
+ kalman->PNCovariance = kalman->process_noise_cov->data.fl;\r
+ kalman->KalmGainMatr = kalman->gain->data.fl;\r
+ kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl;\r
+ kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl;\r
+#endif \r
+\r
+ __END__;\r
+\r
+ if( cvGetErrStatus() < 0 )\r
+ cvReleaseKalman( &kalman );\r
+\r
+ return kalman;\r
+}\r
+\r
+\r
+CV_IMPL void\r
+cvReleaseKalman( CvKalman** _kalman )\r
+{\r
+ CvKalman *kalman;\r
+\r
+ CV_FUNCNAME( "cvReleaseKalman" );\r
+ __BEGIN__;\r
+ \r
+ if( !_kalman )\r
+ CV_ERROR( CV_StsNullPtr, "" );\r
+ \r
+ kalman = *_kalman;\r
+ \r
+ /* freeing the memory */\r
+ cvReleaseMat( &kalman->state_pre );\r
+ cvReleaseMat( &kalman->state_post );\r
+ cvReleaseMat( &kalman->transition_matrix );\r
+ cvReleaseMat( &kalman->control_matrix );\r
+ cvReleaseMat( &kalman->measurement_matrix );\r
+ cvReleaseMat( &kalman->process_noise_cov );\r
+ cvReleaseMat( &kalman->measurement_noise_cov );\r
+ cvReleaseMat( &kalman->error_cov_pre );\r
+ cvReleaseMat( &kalman->gain );\r
+ cvReleaseMat( &kalman->error_cov_post );\r
+ cvReleaseMat( &kalman->temp1 );\r
+ cvReleaseMat( &kalman->temp2 );\r
+ cvReleaseMat( &kalman->temp3 );\r
+ cvReleaseMat( &kalman->temp4 );\r
+ cvReleaseMat( &kalman->temp5 );\r
+\r
+ memset( kalman, 0, sizeof(*kalman));\r
+\r
+ /* deallocating the structure */\r
+ cvFree( _kalman );\r
+\r
+ __END__;\r
+}\r
+\r
+\r
+CV_IMPL const CvMat*\r
+cvKalmanPredict( CvKalman* kalman, const CvMat* control )\r
+{\r
+ CvMat* result = 0;\r
+ \r
+ CV_FUNCNAME( "cvKalmanPredict" );\r
+\r
+ __BEGIN__;\r
+ \r
+ if( !kalman )\r
+ CV_ERROR( CV_StsNullPtr, "" );\r
+\r
+ /* update the state */\r
+ /* x'(k) = A*x(k) */\r
+ CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre ));\r
+\r
+ if( control && kalman->CP > 0 )\r
+ /* x'(k) = x'(k) + B*u(k) */\r
+ CV_CALL( cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre ));\r
+ \r
+ /* update error covariance matrices */\r
+ /* temp1 = A*P(k) */\r
+ CV_CALL( cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 ));\r
+ \r
+ /* P'(k) = temp1*At + Q */\r
+ CV_CALL( cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1,\r
+ kalman->error_cov_pre, CV_GEMM_B_T ));\r
+\r
+ result = kalman->state_pre;\r
+\r
+ __END__;\r
+\r
+ return result;\r
+}\r
+\r
+\r
+CV_IMPL const CvMat*\r
+cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement )\r
+{\r
+ CvMat* result = 0;\r
+\r
+ CV_FUNCNAME( "cvKalmanCorrect" );\r
+\r
+ __BEGIN__;\r
+ \r
+ if( !kalman || !measurement )\r
+ CV_ERROR( CV_StsNullPtr, "" );\r
+\r
+ /* temp2 = H*P'(k) */\r
+ CV_CALL( cvMatMulAdd( kalman->measurement_matrix,\r
+ kalman->error_cov_pre, 0, kalman->temp2 ));\r
+ /* temp3 = temp2*Ht + R */\r
+ CV_CALL( cvGEMM( kalman->temp2, kalman->measurement_matrix, 1,\r
+ kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T ));\r
+\r
+ /* temp4 = inv(temp3)*temp2 = Kt(k) */\r
+ CV_CALL( cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD ));\r
+\r
+ /* K(k) */\r
+ CV_CALL( cvTranspose( kalman->temp4, kalman->gain ));\r
+ \r
+ /* temp5 = z(k) - H*x'(k) */\r
+ CV_CALL( cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 ));\r
+\r
+ /* x(k) = x'(k) + K(k)*temp5 */\r
+ CV_CALL( cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post ));\r
+\r
+ /* P(k) = P'(k) - K(k)*temp2 */\r
+ CV_CALL( cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1,\r
+ kalman->error_cov_post, 0 ));\r
+\r
+ result = kalman->state_post;\r
+\r
+ __END__;\r
+\r
+ return result;\r
+}\r
+\r
+namespace cv\r
+{\r
+\r
+KalmanFilter::KalmanFilter() {}\r
+KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams)\r
+{\r
+ init(dynamParams, measureParams, controlParams);\r
+}\r
+\r
+void KalmanFilter::init(int DP, int MP, int CP)\r
+{\r
+ CV_Assert( DP > 0 && MP > 0 );\r
+ CP = std::max(CP, 0);\r
+\r
+ statePre = Mat::zeros(DP, 1, CV_32F);\r
+ statePost = Mat::zeros(DP, 1, CV_32F);\r
+ transitionMatrix = Mat::eye(DP, DP, CV_32F);\r
+\r
+ processNoiseCov = Mat::eye(DP, DP, CV_32F);\r
+ measurementMatrix = Mat::zeros(MP, DP, CV_32F);\r
+ measurementNoiseCov = Mat::eye(MP, MP, CV_32F);\r
+\r
+ errorCovPre = Mat::zeros(DP, DP, CV_32F);\r
+ errorCovPost = Mat::zeros(DP, DP, CV_32F);\r
+ gain = Mat::zeros(DP, MP, CV_32F);\r
+\r
+ if( CP > 0 )\r
+ controlMatrix = Mat::zeros(DP, CP, CV_32F);\r
+ else\r
+ controlMatrix.release();\r
+\r
+ temp1.create(DP, DP, CV_32F);\r
+ temp2.create(MP, DP, CV_32F);\r
+ temp3.create(MP, MP, CV_32F);\r
+ temp4.create(MP, DP, CV_32F);\r
+ temp5.create(MP, 1, CV_32F);\r
+}\r
+\r
+const Mat& KalmanFilter::predict(const Mat& control)\r
+{\r
+ // update the state: x'(k) = A*x(k)\r
+ statePre = transitionMatrix*statePost;\r
+\r
+ if( control.data )\r
+ // x'(k) = x'(k) + B*u(k)\r
+ statePre += controlMatrix*control;\r
+\r
+ // update error covariance matrices: temp1 = A*P(k)\r
+ temp1 = transitionMatrix*errorCovPost;\r
+\r
+ // P'(k) = temp1*At + Q\r
+ gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);\r
+\r
+ return statePre;\r
+}\r
+\r
+const Mat& KalmanFilter::correct(const Mat& measurement)\r
+{\r
+ // temp2 = H*P'(k)\r
+ temp2 = measurementMatrix * errorCovPre;\r
+\r
+ // temp3 = temp2*Ht + R\r
+ gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T); \r
+\r
+ // temp4 = inv(temp3)*temp2 = Kt(k)\r
+ solve(temp3, temp2, temp4, DECOMP_SVD);\r
+\r
+ // K(k)\r
+ gain = temp4.t();\r
+ \r
+ // temp5 = z(k) - H*x'(k)\r
+ temp5 = measurement - measurementMatrix*statePre;\r
+\r
+ // x(k) = x'(k) + K(k)*temp5\r
+ statePost = statePre + gain*temp5;\r
+\r
+ // P(k) = P'(k) - K(k)*temp2\r
+ errorCovPost = errorCovPre - gain*temp2;\r
+\r
+ return statePost;\r
+}\r
+ \r
+};\r
projects / opencv / blobdiff