##Kalman濾波算法 在視頻跟蹤處理中，預測目標運動軌跡是一項基本任務。目標運動狀態估計的目的有三個：一是對目標過去的狀態進行平滑；二是對目標現在的運動狀態進行濾波；三是對目標未來的運動狀態進行預測。物體的運動狀態一般包括目標位置、速度、加速度等。著名的Kalman濾波技術就是其中一種，這是一種線性系統估計技術。 OpenCV中自帶了kalman濾波的代碼和例程，可參照kalman.cpp，它存在于類KalmanFilter中。kalman濾波算法的調用比較方便，主要的難點是了解多個參數和矩陣計算公式。一個總體的思路是，需要了解前一時刻的狀態估計值x和當前的觀測值y，然后建立狀態方程和觀測方程。經過一些運算后即可預測下一步的狀態。 **一、離散時間線性動態系統的狀態方程** Kalman濾波利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。由于觀測數據中包括系統中的噪聲和干擾的影響，所以最優估計也可看作是濾波過程。一個**線性系統**是采用狀態方程、觀測方程及其初始條件來描述。線性離散時間系統的一般狀態方程可描述為： ? 其中，?是狀態轉移矩陣，?是過程噪聲增益矩陣。是k時刻目標的狀態向量，?是過程噪聲，它是具有均值為零、方差矩陣為Q(k)的高斯噪聲向量，即： ? **二、傳感器的觀測方程** 傳感器的通用觀測方程為： ? 這里， 是傳感器在?時刻的觀測向量，觀測噪聲?是具有零均值和正定協方差矩陣R(k)的高斯分布測量噪聲向量，即： ? **三、初始狀態的描述**? 初始狀態?是高斯的，具有均值?和協方差?，即： ? 以上的描述比較抽象，因此記錄一個例子加以說明： 例：目標沿x軸作勻速直線運動，過程噪聲為速度噪聲，試寫出目標的狀態方程。 解：由題意知，目標的狀態為：  用T表示時間間隔，ux表速度噪聲，則有：  寫成矩陣形式為：  令：  則有：  其中：  為均值等于0，方差為q的高斯噪聲。 在OpenCV中自帶的例程里面描述了一個一維的運動跟蹤，該點在一個圓弧上運動，只有一個自由度即角度。因此只需建立勻速運動模型即可。 例程的路徑：C:\opencv\sources\samples\cpp\kalman.cpp ~~~ 在代碼中各變量的對應情況如下： 狀態估計值X對應：state 當前觀測值Z對應：measurement KalmanFilter類內成員變量transitionMatrix即為狀態轉移方程中的矩陣A KalmanFilter類內成員變量measurementMatrix即為量測方程中矩陣C Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k) Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) Mat transitionMatrix; //!< state transition matrix (A) Mat controlMatrix; //!< control matrix (B) (not used if there is no control) Mat measurementMatrix; //!< measurement matrix (H) Mat processNoiseCov; //!< process noise covariance matrix (Q) Mat measurementNoiseCov;//!< measurement noise covariance matrix (R) Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/ Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R) Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k) ~~~ 以下是OpenCV/modules/video/src/Kalman.cpp的源代碼，后續需繼續分析這些代碼： ~~~ /*M/////////////////////////////////////////////////////////////////////////////////////// // // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. // // By downloading, copying, installing or using the software you agree to this license. // If you do not agree to this license, do not download, install, // copy or use the software. // // // Intel License Agreement // For Open Source Computer Vision Library // // Copyright (C) 2000, Intel Corporation, all rights reserved. // Third party copyrights are property of their respective owners. // // Redistribution and use in source and binary forms, with or without modification, // are permitted provided that the following conditions are met: // // * Redistribution's of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // // * Redistribution's in binary form must reproduce the above copyright notice, // this list of conditions and the following disclaimer in the documentation // and/or other materials provided with the distribution. // // * The name of Intel Corporation may not be used to endorse or promote products // derived from this software without specific prior written permission. // // This software is provided by the copyright holders and contributors "as is" and // any express or implied warranties, including, but not limited to, the implied // warranties of merchantability and fitness for a particular purpose are disclaimed. // In no event shall the Intel Corporation or contributors be liable for any direct, // indirect, incidental, special, exemplary, or consequential damages // (including, but not limited to, procurement of substitute goods or services; // loss of use, data, or profits; or business interruption) however caused // and on any theory of liability, whether in contract, strict liability, // or tort (including negligence or otherwise) arising in any way out of // the use of this software, even if advised of the possibility of such damage. // //M*/ #include "precomp.hpp" CV_IMPL CvKalman* cvCreateKalman( int DP, int MP, int CP ) { CvKalman *kalman = 0; if( DP <= 0 || MP <= 0 ) CV_Error( CV_StsOutOfRange, "state and measurement vectors must have positive number of dimensions" ); if( CP < 0 ) CP = DP; /* allocating memory for the structure */ kalman = (CvKalman *)cvAlloc( sizeof( CvKalman )); memset( kalman, 0, sizeof(*kalman)); kalman->DP = DP; kalman->MP = MP; kalman->CP = CP; kalman->state_pre = cvCreateMat( DP, 1, CV_32FC1 ); cvZero( kalman->state_pre ); kalman->state_post = cvCreateMat( DP, 1, CV_32FC1 ); cvZero( kalman->state_post ); kalman->transition_matrix = cvCreateMat( DP, DP, CV_32FC1 ); cvSetIdentity( kalman->transition_matrix ); kalman->process_noise_cov = cvCreateMat( DP, DP, CV_32FC1 ); cvSetIdentity( kalman->process_noise_cov ); kalman->measurement_matrix = cvCreateMat( MP, DP, CV_32FC1 ); cvZero( kalman->measurement_matrix ); kalman->measurement_noise_cov = cvCreateMat( MP, MP, CV_32FC1 ); cvSetIdentity( kalman->measurement_noise_cov ); kalman->error_cov_pre = cvCreateMat( DP, DP, CV_32FC1 ); kalman->error_cov_post = cvCreateMat( DP, DP, CV_32FC1 ); cvZero( kalman->error_cov_post ); kalman->gain = cvCreateMat( DP, MP, CV_32FC1 ); if( CP > 0 ) { kalman->control_matrix = cvCreateMat( DP, CP, CV_32FC1 ); cvZero( kalman->control_matrix ); } kalman->temp1 = cvCreateMat( DP, DP, CV_32FC1 ); kalman->temp2 = cvCreateMat( MP, DP, CV_32FC1 ); kalman->temp3 = cvCreateMat( MP, MP, CV_32FC1 ); kalman->temp4 = cvCreateMat( MP, DP, CV_32FC1 ); kalman->temp5 = cvCreateMat( MP, 1, CV_32FC1 ); #if 1 kalman->PosterState = kalman->state_pre->data.fl; kalman->PriorState = kalman->state_post->data.fl; kalman->DynamMatr = kalman->transition_matrix->data.fl; kalman->MeasurementMatr = kalman->measurement_matrix->data.fl; kalman->MNCovariance = kalman->measurement_noise_cov->data.fl; kalman->PNCovariance = kalman->process_noise_cov->data.fl; kalman->KalmGainMatr = kalman->gain->data.fl; kalman->PriorErrorCovariance = kalman->error_cov_pre->data.fl; kalman->PosterErrorCovariance = kalman->error_cov_post->data.fl; #endif return kalman; } CV_IMPL void cvReleaseKalman( CvKalman** _kalman ) { CvKalman *kalman; if( !_kalman ) CV_Error( CV_StsNullPtr, "" ); kalman = *_kalman; if( !kalman ) return; /* freeing the memory */ cvReleaseMat( &kalman->state_pre ); cvReleaseMat( &kalman->state_post ); cvReleaseMat( &kalman->transition_matrix ); cvReleaseMat( &kalman->control_matrix ); cvReleaseMat( &kalman->measurement_matrix ); cvReleaseMat( &kalman->process_noise_cov ); cvReleaseMat( &kalman->measurement_noise_cov ); cvReleaseMat( &kalman->error_cov_pre ); cvReleaseMat( &kalman->gain ); cvReleaseMat( &kalman->error_cov_post ); cvReleaseMat( &kalman->temp1 ); cvReleaseMat( &kalman->temp2 ); cvReleaseMat( &kalman->temp3 ); cvReleaseMat( &kalman->temp4 ); cvReleaseMat( &kalman->temp5 ); memset( kalman, 0, sizeof(*kalman)); /* deallocating the structure */ cvFree( _kalman ); } CV_IMPL const CvMat* cvKalmanPredict( CvKalman* kalman, const CvMat* control ) { if( !kalman ) CV_Error( CV_StsNullPtr, "" ); /* update the state */ /* x'(k) = A*x(k) */ cvMatMulAdd( kalman->transition_matrix, kalman->state_post, 0, kalman->state_pre ); if( control && kalman->CP > 0 ) /* x'(k) = x'(k) + B*u(k) */ cvMatMulAdd( kalman->control_matrix, control, kalman->state_pre, kalman->state_pre ); /* update error covariance matrices */ /* temp1 = A*P(k) */ cvMatMulAdd( kalman->transition_matrix, kalman->error_cov_post, 0, kalman->temp1 ); /* P'(k) = temp1*At + Q */ cvGEMM( kalman->temp1, kalman->transition_matrix, 1, kalman->process_noise_cov, 1, kalman->error_cov_pre, CV_GEMM_B_T ); /* handle the case when there will be measurement before the next predict */ cvCopy(kalman->state_pre, kalman->state_post); return kalman->state_pre; } CV_IMPL const CvMat* cvKalmanCorrect( CvKalman* kalman, const CvMat* measurement ) { if( !kalman || !measurement ) CV_Error( CV_StsNullPtr, "" ); /* temp2 = H*P'(k) */ cvMatMulAdd( kalman->measurement_matrix, kalman->error_cov_pre, 0, kalman->temp2 ); /* temp3 = temp2*Ht + R */ cvGEMM( kalman->temp2, kalman->measurement_matrix, 1, kalman->measurement_noise_cov, 1, kalman->temp3, CV_GEMM_B_T ); /* temp4 = inv(temp3)*temp2 = Kt(k) */ cvSolve( kalman->temp3, kalman->temp2, kalman->temp4, CV_SVD ); /* K(k) */ cvTranspose( kalman->temp4, kalman->gain ); /* temp5 = z(k) - H*x'(k) */ cvGEMM( kalman->measurement_matrix, kalman->state_pre, -1, measurement, 1, kalman->temp5 ); /* x(k) = x'(k) + K(k)*temp5 */ cvMatMulAdd( kalman->gain, kalman->temp5, kalman->state_pre, kalman->state_post ); /* P(k) = P'(k) - K(k)*temp2 */ cvGEMM( kalman->gain, kalman->temp2, -1, kalman->error_cov_pre, 1, kalman->error_cov_post, 0 ); return kalman->state_post; } namespace cv { KalmanFilter::KalmanFilter() {} KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type) { init(dynamParams, measureParams, controlParams, type); } void KalmanFilter::init(int DP, int MP, int CP, int type) { CV_Assert( DP > 0 && MP > 0 ); CV_Assert( type == CV_32F || type == CV_64F ); CP = std::max(CP, 0); statePre = Mat::zeros(DP, 1, type); statePost = Mat::zeros(DP, 1, type); transitionMatrix = Mat::eye(DP, DP, type); processNoiseCov = Mat::eye(DP, DP, type); measurementMatrix = Mat::zeros(MP, DP, type); measurementNoiseCov = Mat::eye(MP, MP, type); errorCovPre = Mat::zeros(DP, DP, type); errorCovPost = Mat::zeros(DP, DP, type); gain = Mat::zeros(DP, MP, type); if( CP > 0 ) controlMatrix = Mat::zeros(DP, CP, type); else controlMatrix.release(); temp1.create(DP, DP, type); temp2.create(MP, DP, type); temp3.create(MP, MP, type); temp4.create(MP, DP, type); temp5.create(MP, 1, type); } const Mat& KalmanFilter::predict(const Mat& control) { // update the state: x'(k) = A*x(k) statePre = transitionMatrix*statePost; if( control.data ) // x'(k) = x'(k) + B*u(k) statePre += controlMatrix*control; // update error covariance matrices: temp1 = A*P(k) temp1 = transitionMatrix*errorCovPost; // P'(k) = temp1*At + Q gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T); // handle the case when there will be measurement before the next predict. statePre.copyTo(statePost); errorCovPre.copyTo(errorCovPost); return statePre; } const Mat& KalmanFilter::correct(const Mat& measurement) { // temp2 = H*P'(k) temp2 = measurementMatrix * errorCovPre; // temp3 = temp2*Ht + R gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T); // temp4 = inv(temp3)*temp2 = Kt(k) solve(temp3, temp2, temp4, DECOMP_SVD); // K(k) gain = temp4.t(); // temp5 = z(k) - H*x'(k) temp5 = measurement - measurementMatrix*statePre; // x(k) = x'(k) + K(k)*temp5 statePost = statePre + gain*temp5; // P(k) = P'(k) - K(k)*temp2 errorCovPost = errorCovPre - gain*temp2; return statePost; } } ~~~ 參考鏈接：[http://blog.csdn.net/yang_xian521/article/details/7050398](http://blog.csdn.net/yang_xian521/article/details/7050398)