OpenCV4入门教程056：kalman（卡尔曼）滤波

索引地址：系列索引

卡尔曼滤波内容详见：图解卡尔曼滤波工作原理（翻译）

编程步骤

定义KalmanFilter类并初始化

• KalmanFilter KF(DP, MP, 0);构造KF对象，初始化相关参数
• KF.transitionMatrix 转移矩阵 A
• KF.measurementMatrix 测量矩阵 H
• KF.processNoiseCov 过程噪声 Q
• KF.measurementNoiseCov 测量噪声 R
• KF.errorCovPost 最小均方误差 P
• KF.statePost 系统初始状态 x(0)
• Mat measurement 定义初始测量值 z(0)

预测

• KF.predict();返回的是下一时刻的状态值KF.statePost (k+1)

更新

更新measurement; //注意measurement不能通过观测方程进行计算得到，要自己定义！

更新KF KF.correct(measurement)

最终的结果应该是更新后的statePost.

相关参数的确定

对于系统状态方程，简记为Y=AX+B，X和Y是表示系统状态的列向量，A是转移矩阵，B是其他项。

状态值（向量）只要能表示系统的状态即可，状态值的维数决定了转移矩阵A的维数，比如X和Y是N×1的，则A是N×N的。

A的确定跟X有关，只要保证方程中不相干项的系数为0即可，看下面例子

X和Y是二维的，

$\left[ \begin{matrix} c \\ d \end{matrix} \right] = \left[ \begin{matrix} 1 && 0 \\ 0 && 1 \end{matrix} \right] \cdot \left[ \begin{matrix} c \\ d \end{matrix} \right] + B$

X和Y是三维的，

$\left[ \begin{matrix} c \\ d \\ e \end{matrix} \right] = \left[ \begin{matrix} 1 && 0 && 0 \\ 0 && 1 && 0 \\ 0 && 0 && 1 \end{matrix} \right] \cdot \left[ \begin{matrix} c \\ d \\ e \end{matrix} \right] + B$

X和Y是三维的，但c和$\Delta c$是相关项

$\left[ \begin{matrix} c \\ d \\ \Delta c \end{matrix} \right] = \left[ \begin{matrix} 1 && 0 && 1 \\ 0 && 1 && 0 \\ 0 && 0 && 1 \end{matrix} \right] \cdot \left[ \begin{matrix} c \\ d \\ \Delta c \end{matrix} \right] + B$

$\left[ \begin{matrix} c \\ \Delta c \\ d \end{matrix} \right] = \left[ \begin{matrix} 1 && 1 && 0 \\ 0 && 1 && 0 \\ 0 && 0 && 1 \end{matrix} \right] \cdot \left[ \begin{matrix} c \\ \Delta c \\ d \end{matrix} \right] + B$

上面的1也可以是其他值。

下面对predict( ) 和correct( )函数介绍下，可以不用看，不影响编程。

CV_EXPORTS const oclMat& KalmanFilter::predict(const oclMat& control){
    gemm(transitionMatrix, statePost, 1, oclMat(), 0, statePre);
    oclMat temp;

    if(control.data)
        gemm(controlMatrix, control, 1, statePre, 1, statePre);
    gemm(transitionMatrix, errorCovPost, 1, oclMat(), 0, temp1);
    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
    statePre.copyTo(statePost);
    return statePre;
}

CV_EXPORTS const oclMat& KalmanFilter::predict(const oclMat& control){
    gemm(transitionMatrix, statePost, 1, oclMat(), 0, statePre);
    oclMat temp;

    if(control.data)
        gemm(controlMatrix, control, 1, statePre, 1, statePre);
    gemm(transitionMatrix, errorCovPost, 1, oclMat(), 0, temp1);
    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);
    statePre.copyTo(statePost);
    return statePre;
}

gemm( )是矩阵的广义乘法

void gemm(const GpuMat& src1,
          constGpuMat& src2, 
          double alpha, 
          const GpuMat& src3, 
          double beta,
          GpuMat& dst, 
          int flags=0, 
          Stream& stream=Stream::Null())

$ dst = alpha · src1 · src2 +beta· src3 $

上面，oclMat()其实是$u_k$，只不过默认为0，所以没赋值。整个过程就计算了x’和P’。（用x’代表x的预测值，用P’代表P的预测值）。GEMM_2_T表示对第2个参数转置。

$x'(k)=1·A·x(k-1)$

如果B非空，

x’(k) = 1·B·u + 1·x’(k-1)
temp1 = 1·A·P(k-1) + 0·u(k)
P’(k) = 1· temp1·AT + 1· Qk= A·P(k-1)·AT + 1· Qk

可见，和第一部分的理论介绍完全一致。

CV_EXPORTS const oclMat& KalmanFilter::correct(const oclMat& measurement)
{
    CV_Assert(measurement.empty() == false);
    gemm(measurementMatrix, errorCovPre, 1, oclMat(), 0, temp2);
    gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);
    Mat temp;
    solve(Mat(temp3), Mat(temp2), temp, DECOMP_SVD);
    temp4.upload(temp);
    gain = temp4.t();
    gemm(measurementMatrix, statePre, -1, measurement, 1, temp5);
    gemm(gain, temp5, 1, statePre, 1, statePost);
    gemm(gain, temp2, -1, errorCovPre, 1, errorCovPost);
    return statePost;
}

bool solve(InputArray src1,
           InputArray src2, 
           OutputArray dst, 
           int flags=DECOMP_LU)

求解线型最小二乘估计

temp2 = 1· H·P’ + 0·u(k)
temp3 = 1· temp2·HT + 1·R = H·P’·HT+ 1· R 也就是上面的Sk
temp = argmin||tem2- temp3||
K=temp
temp5 = -1· H·x’ + 1·zk 就是上面的y’。
x = 1·K·temp5 + 1·x’ = KT·y’ +x’
P =-1·K·temp2 + 1·P’ = -K·H·P’+P’ = (I- K·H) P’

也和第一部分的理论完全一致。

通过深入函数内部，学到了两个实用的函数哦。矩阵广义乘法gemm()、最小二乘估计solve()

测试代码：

//---------------------------------【头文件、命名空间包含部分】----------------------------
//      描述：包含程序所使用的头文件和命名空间
//------------------------------------------------------------------------------------------------
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/video/tracking.hpp"
#include 
using namespace cv;

//---------------------------------【calcPoint( )函数】--------------------------------------
//       描述：计算点坐标
//----------------------------------------------------------------------------------------------
static inline Point calcPoint(Point2f center, double R, double angle) {
    return center + Point2f((float)cos(angle), (float)-sin(angle)) * (float)R;
}

//--------------------------------------【help( )函数】--------------------------------------
//       描述：输出一些帮助信息
//----------------------------------------------------------------------------------------------
static void help() {
    printf("\n\t用OpenCV动态绘制卡尔曼滤波。\n");
    printf("\n\t任意按键重置轨迹，并更新速度\n");
    printf("\n\tESC键-程序结束\n");
}

//-----------------------------------【main( )函数】--------------------------------------------
//      描述：控制台应用程序的入口函数，我们的程序从这里开始
//-------------------------------------------------------------------------------------------------
int main(int, char **) {
    help();
    Mat          img(500, 500, CV_8UC3);
    KalmanFilter KF(2, 1, 0);
    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;

        setIdentity(KF.measurementMatrix);
        setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
        setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
        setIdentity(KF.errorCovPost, Scalar::all(1));

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

效果：