|
|
|
@ -211,38 +211,39 @@ namespace cv |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
KalmanFilter::KalmanFilter() {} |
|
|
|
|
KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams) |
|
|
|
|
KalmanFilter::KalmanFilter(int dynamParams, int measureParams, int controlParams, int type) |
|
|
|
|
{ |
|
|
|
|
init(dynamParams, measureParams, controlParams); |
|
|
|
|
init(dynamParams, measureParams, controlParams, type); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void KalmanFilter::init(int DP, int MP, int CP) |
|
|
|
|
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, CV_32F); |
|
|
|
|
statePost = Mat::zeros(DP, 1, CV_32F); |
|
|
|
|
transitionMatrix = Mat::eye(DP, DP, CV_32F); |
|
|
|
|
statePre = Mat::zeros(DP, 1, type); |
|
|
|
|
statePost = Mat::zeros(DP, 1, type); |
|
|
|
|
transitionMatrix = Mat::eye(DP, DP, type); |
|
|
|
|
|
|
|
|
|
processNoiseCov = Mat::eye(DP, DP, CV_32F); |
|
|
|
|
measurementMatrix = Mat::zeros(MP, DP, CV_32F); |
|
|
|
|
measurementNoiseCov = Mat::eye(MP, MP, CV_32F); |
|
|
|
|
processNoiseCov = Mat::eye(DP, DP, type); |
|
|
|
|
measurementMatrix = Mat::zeros(MP, DP, type); |
|
|
|
|
measurementNoiseCov = Mat::eye(MP, MP, type); |
|
|
|
|
|
|
|
|
|
errorCovPre = Mat::zeros(DP, DP, CV_32F); |
|
|
|
|
errorCovPost = Mat::zeros(DP, DP, CV_32F); |
|
|
|
|
gain = Mat::zeros(DP, MP, CV_32F); |
|
|
|
|
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, CV_32F); |
|
|
|
|
controlMatrix = Mat::zeros(DP, CP, type); |
|
|
|
|
else |
|
|
|
|
controlMatrix.release(); |
|
|
|
|
|
|
|
|
|
temp1.create(DP, DP, CV_32F); |
|
|
|
|
temp2.create(MP, DP, CV_32F); |
|
|
|
|
temp3.create(MP, MP, CV_32F); |
|
|
|
|
temp4.create(MP, DP, CV_32F); |
|
|
|
|
temp5.create(MP, 1, CV_32F); |
|
|
|
|
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) |
|
|
|
|