namespace cv{class CV_EXPORTS_W KalmanFilter
{
public://缺省構造函數：無論構造函數是自動生成的還是用戶定義的，它都是可以調用的構造函數，無需提供任何參數。CV_WRAP KalmanFilter();//完整構造函數CV_WRAP KalmanFilter(int dynamparams,int measureparamsint, int controlParams=0, int type=CV_32F); //dynamparams:狀態維度//measureparamsint：觀測維度//controlParams：控制維度void init(int dynamParams, int measureParams, int controlParams=0, int type=CV_32F);//計算預測狀態CV_WRAP const Mat& predict(const Mat& control=Mat()); //依據觀測值更新預測狀態 CV_WRAP const Mat& correct(const Mat& measurement); Mat statePre; //!< 預測狀態 (x'(k)): x(k)=A*x(k-1)+B*u(k) Mat statePost; //!< 糾正狀態 (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) Mat transitionMatrix; //!< 狀態轉換矩陣 (F) Mat controlMatrix; //!< 控制矩陣(B) (not used if there is no control) Mat measurementMatrix; //!< 觀測矩陣(H) Mat processNoiseCov; //!< 處理過程噪聲協方差矩陣 (Q) Mat measurementNoiseCov;//!< 觀測噪聲協方差矩陣 (R) Mat errorCovPre; //!< 先驗誤差估計協方差矩陣 (P'(k)): P'(k)=A*P(k-1)*At + Q)*/ Mat gain; //!< 卡爾曼增益矩陣 (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R) Mat errorCovPost; //!< 后驗誤差估計協方差矩陣 (P(k)): P(k)=(I-K(k)*H)*P'(k) // temporary matrices Mat temp1; Mat temp2; Mat temp3; Mat temp4; Mat temp5;
};
}
kalaman類的實現
namespace cv
{
KalmanFilter::KalmanFilter() {}//dynamparams:狀態維度//measureparamsint：觀測維度//controlParams：控制維度
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);//狀態向量維度DP X 1statePost = Mat::zeros(DP, 1, type);transitionMatrix = Mat::eye(DP, DP, type);// 狀態轉移矩陣F的大小為DP X DPprocessNoiseCov = Mat::eye(DP, DP, type);// 處理過程噪聲協方差矩陣Q的大小為DP X DPmeasurementMatrix = Mat::zeros(MP, DP, type);//觀測矩陣H的大小為MP X DPmeasurementNoiseCov = Mat::eye(MP, MP, type);// 觀測噪聲協方差矩陣R大小為MP X MPerrorCovPre = Mat::zeros(DP, DP, type); // 先驗誤差估計協方差矩陣P'大小為DP X DPerrorCovPost = Mat::zeros(DP, DP, type);// 后驗誤差估計協方差矩陣P大小為DP X DPgain = Mat::zeros(DP, MP, type); // 卡爾曼增益矩陣if( CP > 0 )controlMatrix = Mat::zeros(DP, CP, type);// 控制矩陣BelsecontrolMatrix.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)
{// 更新預測狀態 e: x'(k) = F*x(k)statePre = transitionMatrix*statePost;//判斷是否添加控制，是否添加控制矩陣影響更新的預測狀態if( !control.empty() )// x'(k) = x'(k) + B*u(k)statePre += controlMatrix*control;// temp1 = F*P(k)temp1 = transitionMatrix*errorCovPost;// 更新先驗誤差估計協方差矩陣P'(k) = temp1*F_T + Q// gemm(a,b,n,c,m,ans,flags): ans = n*a*b+m*c// flags=GEMM_2_T:b transposesgemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);// handle the case when there will be measurement before the next predict.//a.copyTo(b):得到與a一樣的b，兩者可進行互不相關的操作。statePre.copyTo(statePost);errorCovPre.copyTo(errorCovPost);return statePre;
}const Mat& KalmanFilter::correct(const Mat& measurement)
{// K(k) = p'(k)*H_T*inv(H*p'(k)*H_T+R)// temp2 = F*P'(k)temp2 = measurementMatrix * errorCovPre;// temp3 = temp2*H_T + Rgemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);//// solve(MA,MB,MX,CV_LU) 可以求AX=B for X 等價于X=A-1 *B// temp4 = inv(temp3)*temp2 = K(k)_Tsolve(temp3, temp2, temp4, DECOMP_SVD);// K(k)// 轉置過來得到真正的Kgain = temp4.t();// temp5 = z(k) - H*x'(k)temp5 = measurement - measurementMatrix*statePre;// x(k) = x'(k) + K(k)*temp5statePost = statePre + gain*temp5;// P(k) = P'(k) - K(k)*temp2errorCovPost = errorCovPre - gain*temp2;return statePost;
}
}
