终于到了opencv这一章,有关KF的理论推导和matlab例程见点击打开链接。opencv中提供了封装好的KF类实现滤波流程,这里以opencv3.1.0版本中的源码为例进行详细分析
还是先简要回顾下KF的流程,如下图所示。
整体来说KF分为两大步:step2-step3为预测阶段,计算得到新的预测值;step4-step6为校正阶段,使用观测值校正预测值得到估计值。这里提前把它们分为两个阶段,是对应着后面opencv中KF类的两大成员函数predict和correct
首先抠出类的声明部分,位于opencv-3.1.0\modules\video\include\opencv2\video\tracking.hpp中,注释已经十分详细了。
class CV_EXPORTS_W KalmanFilter
{
public:
/** @brief The constructors.
@note In C API when CvKalman\* kalmanFilter structure is not needed anymore, it should be released
with cvReleaseKalman(&kalmanFilter)
*/
CV_WRAP KalmanFilter();
/** @overload
@param dynamParams Dimensionality of the state.
@param measureParams Dimensionality of the measurement.
@param controlParams Dimensionality of the control vector.
@param type Type of the created matrices that should be CV_32F or CV_64F.
*/
CV_WRAP KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );
/** @brief Re-initializes Kalman filter. The previous content is destroyed.
@param dynamParams Dimensionality of the state.
@param measureParams Dimensionality of the measurement.
@param controlParams Dimensionality of the control vector.
@param type Type of the created matrices that should be CV_32F or CV_64F.
*/
void init( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );
/** @brief Computes a predicted state.
@param control The optional input control
*/
CV_WRAP const Mat& predict( const Mat& control = Mat() );
/** @brief Updates the predicted state from the measurement.
@param measurement The measured system parameters
*/
CV_WRAP const Mat& correct( const Mat& measurement );
CV_PROP_RW Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
CV_PROP_RW Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
CV_PROP_RW Mat transitionMatrix; //!< state transition matrix (A)
CV_PROP_RW Mat controlMatrix; //!< control matrix (B) (not used if there is no control)
CV_PROP_RW Mat measurementMatrix; //!< measurement matrix (H)
CV_PROP_RW Mat processNoiseCov; //!< process noise covariance matrix (Q)
CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
CV_PROP_RW Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
CV_PROP_RW Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
CV_PROP_RW Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
// temporary matrices
Mat temp1;
Mat temp2;
Mat temp3;
Mat temp4;
Mat temp5;
};
namespace cv
{
KalmanFilter::KalmanFilter() {}
/* dynamParams(DP):状态向量中的元素种类数
* measureParams(MP):观测的状态向量中的元素个数
* controlParams(CP):控制矩阵的列数
*/
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是状态向量中的元素种类数,即状态矩阵的行数
statePost = Mat::zeros(DP, 1, type);
transitionMatrix = Mat::eye(DP, DP, type);// 状态转移矩阵A的大小为DP X DP
processNoiseCov = Mat::eye(DP, DP, type);// 系统噪声协方差矩阵的大小为DP X DP
measurementMatrix = Mat::zeros(MP, DP, type);// MP为观测的状态向量中的元素个数(MP <= DP),观测矩阵的大小为MP X DP
measurementNoiseCov = Mat::eye(MP, MP, type);// 观测噪声协方差矩阵大小为MP X MP
errorCovPre = Mat::zeros(DP, DP, type); // 预测值和真实值之间的误差协方差矩阵大小为DP X DP
errorCovPost = Mat::zeros(DP, DP, type);// 估计值和真实值之间的误差协方差矩阵大小为DP X DP
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.empty() )
// 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
// errorCovPre = 1*temp1*transitionMatrix_t+1*processNoiseCov
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)
// 求解 temp3 * temp4 = temp2的问题,即temp4 = inv(temp3)*temp2
// 但是此时temp4不是卡尔曼增益K,而是它的转置
solve(temp3, temp2, temp4, DECOMP_SVD);
// K(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;
}
}
(1)gemm()实现几个矩阵的加权乘法后相加(还可以选择对某个输入矩阵进行转置)
(2)solve()解决AX=B的问题,等价于求X=inv(A)*B
(3)randn()用于生成指定均值和方差的高斯分布结果
(4)setIdentity()用于初始化矩阵元素
结合KF的流程,opencv中使用KF类的大致流程如下:
step1.初始化KF类对象
step2.KF.predict()得到新的预测值
step3.KF.correct()得到新的估计值
之后重复step2-step3即可,下面以opencv自带的一个卡尔曼滤波例子进行分析,位于opencv-3.1.0\samples\cpp\kalman.cpp。例子中我们建立一个绕某一圆心做匀速圆周运动的小球,但是实际中它会受到系统噪声影响从而其角度和角速度有所变化,我们通过带有噪声的观测值(真实值+观测噪声)和匀速运动模型的预测值为输入使用KF得到估计值,具体代码如下。
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include
using namespace cv;
// 根据圆心和夹角计算点的二维坐标
static inline Point calcPoint(Point2f center, double R, double angle)
{
return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;// 图像坐标系中y轴向下
}
static void help()
{
printf( "\nExample of c calls to OpenCV's Kalman filter.\n"
" Tracking of rotating point.\n"
" Rotation speed is constant.\n"
" Both state and measurements vectors are 1D (a point angle),\n"
" Measurement is the real point angle + gaussian noise.\n"
" The real and the estimated points are connected with yellow line segment,\n"
" the real and the measured points are connected with red line segment.\n"
" (if Kalman filter works correctly,\n"
" the yellow segment should be shorter than the red one).\n"
"\n"
" Pressing any key (except ESC) will reset the tracking with a different speed.\n"
" Pressing ESC will stop the program.\n"
);
}
int main(int, char**)
{
help();
Mat img(500, 500, CV_8UC3);
KalmanFilter KF(2, 1, 0);// 系统状态矩阵大小为2X1, 观测矩阵大小为1X2,卡尔曼增益矩阵大小为2X1
Mat state(2, 1, CV_32F); /* (phi, delta_phi),系统状态为[角度,角速度] */
Mat processNoise(2, 1, CV_32F);// 系统状态噪声矩阵
Mat measurement = Mat::zeros(1, 1, CV_32F);// 观测矩阵,这里只观测角度
char code = (char)-1;
for(;;)
{
randn( state, Scalar::all(0), Scalar::all(0.1) );// 初始化系统状态真实值
KF.transitionMatrix = (Mat_(2, 2) << 1, 1, 0, 1);// 匀速运动模型中的状态转移矩阵A
// 初始化矩阵
setIdentity(KF.measurementMatrix); // A
setIdentity(KF.processNoiseCov, Scalar::all(1e-5));// Q
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));// R
setIdentity(KF.errorCovPost, Scalar::all(1));// P
randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));// 估计值
for(;;)
{
Point2f center(img.cols*0.5f, img.rows*0.5f);
float R = img.cols/3.f;
double stateAngle = state.at(0);
Point statePt = calcPoint(center, R, stateAngle);// 真实位置
Mat prediction = KF.predict();
double predictAngle = prediction.at(0);
Point predictPt = calcPoint(center, R, predictAngle);// 预测位置
randn( measurement, Scalar::all(0), Scalar::all(sqrt(KF.measurementNoiseCov.at(0)));
// generate measurement
measurement += KF.measurementMatrix*state;// 观测位置 = 真实位置+观测位置噪声
double measAngle = measurement.at(0);
Point measPt = calcPoint(center, R, measAngle);// 观测位置坐标
// plot points
#define drawCross( center, color, d ) \
line( img, Point( center.x - d, center.y - d ), \
Point( center.x + d, center.y + d ), color, 1, LINE_AA, 0); \
line( img, Point( center.x + d, center.y - d ), \
Point( center.x - d, center.y + d ), color, 1, LINE_AA, 0 )
// 实时更新三个位置
img = Scalar::all(0);
drawCross( statePt, Scalar(255,255,255), 3 );
drawCross( measPt, Scalar(0,0,255), 3 );
drawCross( predictPt, Scalar(0,255,0), 3 );
line( img, statePt, measPt, Scalar(0,0,255), 3, LINE_AA, 0 );
line( img, statePt, predictPt, Scalar(0,255,255), 3, LINE_AA, 0 );
if(theRNG().uniform(0,4) != 0)
KF.correct(measurement);// 使用观测值更新估计值
randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at(0, 0))));
state = KF.transitionMatrix*state + processNoise;
imshow( "Kalman", img );
code = (char)waitKey(100);
if( code > 0 )
break;
}
if( code == 27 || code == 'q' || code == 'Q' )
break;
}
return 0;
}
唯一感到奇怪的是,例程中比较的是预测值和真实值的偏差,没有比较估计值(KF.correct()的返回值)和真实值的偏差,尚不清楚这样做的目的。程序运行效果如下图,其中黄线代表预测值和真实值的偏差,红线代表测量值和真实值的偏差。