KF算法学习（三）：opencv中的KF源码分析

最新推荐文章于 2024-01-28 12:00:00 发布

Mega_Li

最新推荐文章于 2024-01-28 12:00:00 发布

阅读量6.3k

点赞数 6

文章标签：算法 opencv

本文链接：https://blog.csdn.net/lwx309025167/article/details/78339163

版权

0.前言

终于到了opencv这一章，有关KF的理论推导和matlab例程见点击打开链接。opencv中提供了封装好的KF类实现滤波流程，这里以opencv3.1.0版本中的源码为例进行详细分析

1.KF流程

还是先简要回顾下KF的流程，如下图所示。

整体来说KF分为两大步：step2-step3为预测阶段，计算得到新的预测值；step4-step6为校正阶段，使用观测值校正预测值得到估计值。这里提前把它们分为两个阶段，是对应着后面opencv中KF类的两大成员函数predict和correct

2.opencv中的KF类

首先抠出类的声明部分，位于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;
};

接着来看KF类的实现部分，为方便加深理解自己加了一些中文注释。

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;
}

}

其中用到一些opencv对矩阵的数学操作，记录如下：

（1）gemm()实现几个矩阵的加权乘法后相加（还可以选择对某个输入矩阵进行转置）

（2）solve()解决AX=B的问题，等价于求X=inv(A)*B

（3）randn()用于生成指定均值和方差的高斯分布结果

（4）setIdentity()用于初始化矩阵元素

3.opencv中使用KF

结合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 <stdio.h>

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_<float>(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<float>(0);
            Point statePt = calcPoint(center, R, stateAngle);// 真实位置

            Mat prediction = KF.predict();
            double predictAngle = prediction.at<float>(0);
            Point predictPt = calcPoint(center, R, predictAngle);// 预测位置

            randn( measurement, Scalar::all(0), Scalar::all(sqrt(KF.measurementNoiseCov.at<float>(0)));

            // generate measurement
            measurement += KF.measurementMatrix*state;// 观测位置 = 真实位置+观测位置噪声

            double measAngle = measurement.at<float>(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<float>(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()的返回值）和真实值的偏差，尚不清楚这样做的目的。程序运行效果如下图，其中黄线代表预测值和真实值的偏差，红线代表测量值和真实值的偏差。