卡尔曼滤波

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <stdio.h>
#include<iostream>
using namespace cv;
using namespace std;

const int winHeight = 600;
const int winWidth = 800;

Point mousePosition = Point(winWidth >> 1, winHeight >> 1);

//mouse event callback
void mouseEvent(int event, int x, int y, int flags, void *param)
{
    if (event == CV_EVENT_MOUSEMOVE) {
        mousePosition = Point(x, y);
    }
}

int main(void)
{
    Mat processNoise(2, 1, CV_32F);//新加程序
    RNG rng;
    //1.kalman filter setup
    const int stateNum = 4;                                      //状态值4×1向量(x,y,△x,△y)
    const int measureNum = 2;                                    //测量值2×1向量(x,y)
    KalmanFilter KF(stateNum, measureNum, 0);

    KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);  //转移矩阵A
    setIdentity(KF.measurementMatrix);                                             //测量矩阵H
    setIdentity(KF.processNoiseCov, Scalar::all(5));                            //系统噪声方差矩阵Q
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));                        //测量噪声方差矩阵R
    setIdentity(KF.errorCovPost, Scalar::all(1));                                  //后验错误估计协方差矩阵P
    rng.fill(KF.statePost, RNG::UNIFORM, 0, winHeight>winWidth ? winWidth : winHeight);   //初始状态值x(0)
    Mat measurement = Mat::zeros(measureNum, 1, CV_32F);                           //初始测量值x'(0)，因为后面要更新这个值，所以必须先定义

    // 初始化值
    cout << "Time Update:" << endl;
    cout << "X(k)_ = A*X(k-1)_" << endl;
    cout << "P(k)_ = A*P(k-1)*AT + Q" << endl << endl;
    cout << "Measurement Update:" << endl;
    cout << "K(k) = P(k)_ * HT * inv(H * P(k)_ * HT + R)" << endl;  // K(k)为卡尔曼增益
    cout << "X(k) = X(k)_ + K(k) * (Z(k) - H * X(k)_)" << endl;
    cout << "P(k) = (I - K(k) * H) * P(k)_" << endl << endl;

    cout << "初始估计值X: " << endl << KF.statePost << endl;
    cout << "转移矩阵A: " << endl << KF.transitionMatrix << endl;
    cout << "后验错误估计协方差矩阵P: " << endl << KF.errorCovPost << endl;
    cout << "系统噪声方差矩阵Q: " << endl << KF.processNoiseCov << endl << endl;
    cout << "测量值Z: " << endl << measurement << endl;
    cout << "测量矩阵H: " << endl << KF.measurementMatrix << endl;
    cout << "测量噪声方差矩阵R: " << endl << KF.measurementNoiseCov << endl;

    namedWindow("kalman");
    setMouseCallback("kalman", mouseEvent);

    Mat image(winHeight, winWidth, CV_8UC3, Scalar(0));

    while (1)
    {
        //2.kalman prediction
        Mat prediction = KF.predict();
        Point predict_pt = Point(prediction.at<float>(0), prediction.at<float>(1));   //预测值(x',y')

        //3.update measurement
//        randn(processNoise, Scalar::all(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));//新加程序
//        measurement.at<float>(0) = (float)(mousePosition.x+processNoise.at<float>(0,0));
//        measurement.at<float>(1) = (float)(mousePosition.y+processNoise.at<float>(1,0));

        measurement.at<float>(0) = (float)mousePosition.x;
        measurement.at<float>(1) = (float)mousePosition.y;

        Point p;
        p.x = measurement.at<float>(0);
        p.y = measurement.at<float>(1);
        //4.update
        KF.correct(measurement);

        //draw
        image.setTo(Scalar(255, 255, 255, 0));
        circle(image, predict_pt, 5, Scalar(0, 255, 0), 3);    //predicted point with green
        circle(image, mousePosition, 5, Scalar(255, 0, 0), 3); //current position with red

        putText(image, "predicted position:" + to_string(predict_pt.x) + "," + to_string(predict_pt.y), Point(10, 30), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8);
        putText(image, " current position:" + to_string(mousePosition.x) + "," + to_string(mousePosition.y), cvPoint(10, 60), CV_FONT_HERSHEY_SCRIPT_COMPLEX, 1, Scalar(0, 0, 0), 1, 8);

        imshow("kalman", image);
        int key = waitKey(100);
        if (key == 27) {//esc
            break;
        }
    }
}