OpenCV3之卡尔曼滤波KalmanFilter例子魔改代码

---

OpenCV3之卡尔曼滤波KalmanFilter原理

魔改例子

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <stdio.h>
using namespace std;
using namespace cv;
 
/** @brief 计算圆周上的点相对窗口的坐标值 = 中心点 + 圆周上的点相对圆心

@param center 中心点
@param R 半径
@param angle 角度，因为坐标原点在左上角，角度是逆时针的（和通常的极坐标系顺时针的相反），所以sin前有个负号
 */ 
Point calcPoint(Point2f center, double R, double angle)
{
    angle = angle / 180.0 * 3.1415926;
    return center + Point2f( (float)cos(angle), (float)-sin(angle) ) *(float)R;
}

/** @brief 画X形十字线，通过中心点和半长计算四个顶点的坐标

@param img 要作画的图像.
@param center 中心点.
@param d 一半长.
@param color 颜色.
 */ 
void drawCross(const Mat & img, Point center, int d ,const Scalar & color){
    line( img, Point( center.x - d, center.y - d ),Point( center.x + d, center.y + d ), color, 4);
    line( img, Point( center.x + d, center.y - d ),Point( center.x - d, center.y + d ), color, 5);
}


 
int main(int, char**)
{
    Mat img(800, 800, CV_8UC3);
    
    /** @brief 创建卡尔曼滤波器对象KF.
    
    @param dynamParams 2，状态向量的维度，二维列向量（角度，△角度）
    @param measureParams 1，测量向量的维度，一维列向量（角度）
    @param controlParams 0，控制向量的维度  
     */ 
    KalmanFilter KF(2, 1, 0);



    /* 定义三个列向量：状态向量Xk（state）、测量向量Zk（measurement）和控制向量Uk（control） */

    // 定义状态向量Xk（state）：不用它的值，所以不用初始化

    // 定义测量向量Zk（measurement）
    // for循环中需要传入其测量值，所以初始化
    Mat measurement = Mat::zeros(1, 1, CV_32F);

    // 控制向量Uk（control）简单实例就不考虑了



    /* 初始化三个矩阵：状态转移矩阵Ak（transitionMatrix），控制矩阵Bk（controlMatrix）和测量矩阵Hk（measurementMatrix） */
    
    // 初始化状态转移矩阵Ak（transitionMatrix）：定值
    KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);

    // 初始化测量矩阵Hk（measurementMatrix）：单位矩阵化
    setIdentity(KF.measurementMatrix);
    
    // 控制向量都没有，控制矩阵Bk（controlMatrix）更没有。


    
    /* 两个估计的状态向量：先验估计的状态向量（statePre）和后验估计的状态向量P(k-1|k-1)(statePost) */

    // 先验估计的状态向量（statePre）第一次循环完在函数predict中就自己初始化了

    // 初始化后验估计的状态向量P(statePost)：
    randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));



	/* 定义两个噪声：系统噪声Wk和测量噪声Vk */
    
    // 定义系统噪声Wk：二维列向量 
    Mat processNoise(2, 1, CV_32F);

    // 测量噪声Vk之后再说

    

    /* 两个噪声的协方差矩阵： 系统噪声的协方差矩阵Qk（processNoiseCov）和测量噪声的协方差矩阵Rk（measurementNoiseCov）*/

    // 系统噪声的协方差矩阵Qk（processNoiseCov）
    setIdentity(KF.processNoiseCov, Scalar::all(1e-5));

    // 测量噪声的协方差矩阵Rk（measurementNoiseCov）
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-5));



    /** @brief 初始化系统噪声
    不加噪声的话就是匀速圆周运动，加了点噪声类似匀速圆周运动，因为噪声的原因，运动方向可能会改变
    */ 
    randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));



    /* 两个错误估计的协方差矩阵： 后验错误估计的协方差矩阵P（errorCovPost）和*/ 
    
    //  errorCovPost 后验错误估计的协方差矩阵P
    setIdentity(KF.errorCovPost, Scalar::all(1));

    // errorCovPre 先验错误估计的协方差矩阵P之后再说

    

    /* 圆周运动的设定 */
    
    // center图像中心点
    Point2f center(img.cols*0.5f, img.rows*0.5f);

    // 半径
    float R = img.cols/3.f;

    
    
    // 退出waitKey()的返回值
    char code = (char)-1;


    
    for(;;)
    {
        // 清零画布
        img = Scalar::all(0);



        /* 上一次的点 */

        // 上一次的点的角度stateAngle：取状态向量state的第一个值，即角度
        double lastAngle = KF.statePost.at<float>(0);

        // 上一次的点的坐标statePt：（在圆周上）相对Mat画布的点。 
        Point lastPt = calcPoint(center, R, lastAngle);



        // 计算预测值
        KF.predict();
        


        /* 预测点 */

        // 预测点的角度predictAngle：取预测值prediction的第一个值，即角度
        double predictAngle = KF.statePre.at<float>(0);

        // 预测点的坐标predictPt：（在圆周上）相对Mat画布的点。
        Point predictPt = calcPoint(center, R, predictAngle);


        /* 测量点 */

        // 初始化测量向量Zk（measurement）:随机值；然后和上一次的测量值加起来
        // 表示每次的测量值都不一样，物体在移动
        measurement += Scalar(theRNG().uniform(10.0, 40.0));

        // 测量点的角度measAngle：取预测值measurement的第一个值，即角度
        double measAngle = measurement.at<float>(0);

        // 测量点的坐标measPt：（在圆周上）相对Mat画布的点。
        Point measPt = calcPoint(center, R, measAngle);
        


        // 融合更新
        KF.correct(measurement);

        
        
        // 最优点的角度measAngle：取预测值measurement的第一个值，即角度
        double optimalAngle = KF.statePost.at<float>(0);

        // 测量点的坐标measPt：（在圆周上）相对Mat画布的点。
        Point optimalPt = calcPoint(center, R, optimalAngle);



        /* 画 */
        // 轨道
        circle(img, center, R, Scalar::all(70), 1);
        // 上一个的点（白色）
        drawCross( img , lastPt,  3 ,Scalar(255,255,255));
        // 测量点（红色）
        drawCross( img , measPt, 3 ,Scalar(0,0,255));
        // 预测点（绿色）
        drawCross( img , predictPt, 10 , Scalar(0,255,0));
        // 最优点（蓝色）
        drawCross( img , optimalPt, 10 , Scalar(255,0,0));
        // 测量点到预测点
        line( img, measPt, predictPt, Scalar(0,255,0), 2);
        // 测量点到最优点
        line( img, measPt, optimalPt, Scalar(255,0,0), 2);
        

        // cout << "角度:\n";
        // cout << lastAngle << endl;
        // cout << measAngle << endl;
        // cout << predictAngle << endl;
        // cout << optimalAngle << endl;


        imshow( "Kalman", img );
        code = (char)waitKey(400);

        
        if( code == 27 || code == 'q' || code == 'Q' )
            break;
    }
 
    return 0;
}