卡尔曼滤波（Kalman Filter）理解及实现

文档下载链接https://download.csdn.net/download/OEMT_301/12065734

卡尔曼滤波，个人理解类似于一种信息融合算法，它是将预测值和测量值进行融合，得到一个估计值。因此需要一个预测（状态）方程和一个观测方程。即：

基本模型
这里用一个小车在一维坐标系中做匀加速直线运动来说明参数含义。

预测方程：
预测方程是利用线性差分方程对下一状态进行估计。例如匀速运动问题，室温问题等。其方程表达式如下：

参数含义：假设一个小车，其初始状态为，其加速度为，那么可以得到如下方程：

转换为矩阵形式如下：

测量方程
测量方程是由当前状态估计值转换为测量值得一种线性函数，该过程获取的到不是实际传感器测量的结果，可以理解为是一种预测测量值。可表示为：

其中预测噪声w和测量噪声v是相互独立的，且是高斯白噪声，服从高斯分布：

推到过程忽略，直接到结论

实例
下面是利用opencv中的卡尔曼滤波函数实现鼠标跟踪，其中绿点为每次测量值，黄点为估计值。

#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
#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));
        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) 
        {
            break;
        }
    }
}

结果：


参考：
https://blog.csdn.net/honyniu/article/details/88697520
https://blog.csdn.net/gdfsg/article/details/50904811