一、概述

卡尔曼滤波是综合系统预测值和观测值，以得到更准确的估计值。与频域上的高通、低通、带通的滤波概念不同，卡尔曼滤波是依据已有的状态转移矩阵（对动力系统的知识），再加上过程噪声和观测噪声的概率分布，进而对观测值进行最优估计。

卡尔曼滤波就是“猜-测”的过程，先“猜”，然后根据“测”的结果矫正“猜”的结果，从而获得比“猜”和“测”都更准确的结果。大概过程如下：

Predict过程： 在预测过程中，利用状态转移矩阵从上一时刻的状态预测当前的状态，得到先验估计；
Correct过程：利用状态转移矩阵、预测值的协方差、观测矩阵、观测值的协方差计算卡尔曼增益(用来决定更相信预测值还是更相信观测值多一些），并综合当前的预测值和当前的观测值，更新预测值，测到后验估计。

二、动力系统

假设线性动力系统满足：

$x_{k}=F_{k}x_{k-1}+ B_{k}u_{k}+w_{k}$ （1）

$z_{k}=H_{k}x_{k}+v_{k}$ （2）

式中，

$x_{k}$ 表示k时刻的系统状态。

$F_{k}$ 是状态转移矩阵，由上一时刻的状态 $x_{k-1}$ 推导下一时刻的状态 $x_{k}$ ；

$B_{k}$ 是控制矩阵， $u_{k}$ 是控制向量（若没有控制，则无此项）；

$H_{k}$ 是观测矩阵，由状态 $x_{k}$ 推导观测量 $z_{k}$ ；

$w_{k}$ 过程噪音，假设满足均值为0的正态分布 $w_{k} \sim N(0,Q_{k})$ ，其中 $Q_{k}$ 为协方差。

$v_{ k}$ 是观测噪音，假设满足均值为0的正态分布 $w_{k} \sim N(0,R_{k})$ ，其中 $R_{k}$ 为协方差。

三、算法

迭代过程：a) Predict过程： 利用状态转移矩阵（先验）预测-> b) Correct过程：计算卡尔曼增益-> 结合观测值修正预测值。

卡尔曼滤波5个核心方程

a) Predict过程（先验预测）：

根据状态转移矩阵估计状态

$\hat{x}_{k|k-1} =F_{k}\hat{x}_{k-1|k-1}+ B_{k}u_{k-1}$ （1）

$\hat{x}_{m|n}$ 表示根据n时刻及其之前的状态和观测计算的状态估计；

$\hat{x}_{k|k-1}$ 为先验估计，即根据上一时刻状态获得的预测结果，未考虑k时刻观察的估计；

$\hat{x}_{k-1|k-1}$ 为上一时刻的最优结果。

估计协方差

$P_{k|k-1}=F_{k}P_{k-1|k-1}F_{k}^{T}+Q_{k}$ （2）

协方差给出了结果的可信度，先验估计增加了过程噪音的协方差 $Q_{k}$ ，使得协方差 $P_{k|k-1}$ 变大，即先验估计增加了不确定度。

b) Correct过程：

计算卡尔曼增益

$K_{k}=P_{k|k-1}H_{k}^{T}(H_{k}P_{k|k-1}H_{k}^{T}+R_{k})^{-1}$ （3）

根据过程噪声 $Q_{k}$ （隐式地包含在 $P_{k|k-1}$ 中）和观测噪声 $R_{k}$ ，以及状态转移矩阵 $F_{k}$ 和观测矩阵 $H_{k}$ 计算卡尔曼增益，以确定如何结合先验预测值和观测值（决定相信预测值多一些还是观测值多一些！！）。

根据观测值、估计值和卡尔曼增益更新估计值

$\hat{x}_{k|k} =\hat{x}_{k|k-1}+K_{k}(z_{k}-H_{k}\hat{x}_{k|k-1})$ （4）

$\hat{x}_{k|k}$ 为后验估计，即利用卡尔曼增益综合考虑了先验估计和观察的最优估计，是当前时刻的最优结果。

$K_{k}$ 越小，越接近先验预测值； $K_{k}$ 越大，越接近测量值。

更新协方差

$P_{k|k}=(I-K_{k}H_{k})P_{k|k-1}$ （5）

总结：

卡尔曼滤波需要输入的已知量（不考虑控制的情况下）为：

状态转移矩阵 transitionMatrix $F_{k}$
观测矩阵 measurementMatrix $H_{k}$
过程噪声协方差 processNoiseCov $Q_{k}$
测量噪声协方差 measurementNoiseCov $R_{k}$

每次测量更新的输入：

测量结果 measure $z_{k}$

产生的中间变量包括：

协方差 $P_{k}$ （predict过程计算一次 $P_{k|k-1}$ 、Correct过程更新一次 $P_{k|k}$ ）、
卡尔曼增益

输出结果：

系统状态估计 $\hat{x}_{k}$ （predict过程计算一次 $\hat{x}_{k|k-1}$ 、Correct过程更新一次 $\hat{x}_{k|k}$ ）

备注：
KalmanFilter中各矩阵和向量的维度：输入参数dynamParams（state变量数）和measureParams（measure变量数）决定了

state维度:                         $x_{k}$ [dynamParams*1]

transitionMatrix维度    $F_{k}$ [dynamParams*dynamParams]

processNoiseCov维度 $Q_{k}$ [dynamParams*dynamParams]

processNoise维度                 $w_{k}$ [dynamParams*1]

measurementMatrix维度     $H_{k}$ [measureParams*dynamParams]

measurementNoiseCov维度 $R_{k}$ [measureParams*measureParams]

measureNoise维度                $v_{ k}$    [measureParams*1]

measure维度                          $z_{k}$    [measureParams*1]

variance维度                          $P_{k}$ [dynamParams*dynamParams]

kalman_gain维度                    $K_{k}$ [dynamParams*measureParams]

对于已经封装好的算法，如OpenCV，只需要将需要输入的量初始化好，并调用predict和correct两个方法就可以循环更新系统状态，

隐藏了中间变量和计算过程。

举例：建立一个点绕圆心旋转的动力系统，指定状态转移矩阵、观测矩阵、过程噪声协方差、测量噪声协方差后，调用predict和correct两个方法，即获得系统状态估计。

$x_{k}=F_{k}x_{k-1}+ B_{k}u_{k}+w_{k}$ (1)

$\begin{pmatrix} \theta \\ \Delta \theta \end{pmatrix}_{k}=\begin{bmatrix} 1 & 1\\ 0& 1 \end{bmatrix}\begin{pmatrix} \theta \\ \Delta \theta \end{pmatrix}_{k-1}+w_{k}$

$z_{k}=H_{k}x_{k}+v_{k}$ （2）

$z _{k}=\begin{bmatrix} 1 & 0 \end{bmatrix}\begin{pmatrix} \theta \\ \Delta \theta \end{pmatrix}_{k}+v_{k}$

//改编自OpenCV: samples/cpp/kalman.cpp
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui.hpp"
#include <stdio.h>
#include <iostream>
using namespace cv;
static inline Point calcPoint(Point2f center, double R, double angle)
{
    return center + Point2f((float)cos(angle), (float)-sin(angle))*(float)R;
}
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); 
//输入参数dynamParams和measureParams决定了KalmanFilter中各矩阵和向量的维度
//state维度[dynamParams*1]
//transitionMatrix维度[dynamParams*dynamParams]
//processNoiseCov维度[dynamParams*dynamParams]
//processNoise维度[dynamParams*1]
//measurementMatrix维度[measureParams*dynamParams]
//measurementNoiseCov维度[measureParams*measureParams]
//measureNoise维度[measureParams*1]
//variance维度[dynamParams*dynamParams]
//kalman_gain维度[dynamParams*measureParams]

    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) );
	//std::cout<< state<<std::endl;

	//********初始化卡尔曼滤波：transitionMatrix、measurementMatrix、processNoiseCov、measurementNoiseCov*************************
        KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1); //状态转移矩阵
        setIdentity(KF.measurementMatrix);  //观测矩阵
        setIdentity(KF.processNoiseCov, Scalar::all(1e-5));  //过程噪声
	//std::cout<< "KF.processNoiseCov"<<KF.processNoiseCov<<std::endl;
        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;

	    //***************建立动力系统********************************
	    //模拟真值
            randn( processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));//过程噪音满足高斯分布
            state = KF.transitionMatrix*state + processNoise; //状态转移方程
	    //模拟测量值
            randn( measurement, Scalar::all(0), Scalar::all(KF.measurementNoiseCov.at<float>(0))); //测量噪音满足高斯分布
            measurement += KF.measurementMatrix*state;   //观测方程

	    //***********Predict过程**********************
	    Mat prediction = KF.predict(); //先验预测, 返回值即为KF.statePost
 
	    //************Correct过程******************** 
            if(theRNG().uniform(0,4) != 0)
                KF.correct(measurement);  //Updates the predicted state from the measurement.

            //*************将真值\测量值\最优估计结果转化为绘制的点********************
            double stateAngle = state.at<float>(0);
            Point statePt = calcPoint(center, R, stateAngle);
            double measAngle = measurement.at<float>(0);
            Point measPt = calcPoint(center, R, measAngle); 
	    double correctAngle = KF.statePost.at<float>(0);
            Point correctPt = calcPoint(center, R, correctAngle);
            // 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( correctPt, Scalar(0,255,0), 3 );
            line( img, statePt, measPt, Scalar(0,0,255), 3, LINE_AA, 0 );
            line( img, statePt, correctPt, Scalar(0,255,255), 3, LINE_AA, 0 );
            imshow( "Kalman", img );
	    waitKey(0);
        }
        if( code == 27 || code == 'q' || code == 'Q' )
            break;
    }
    return 0;
}

terminal编译后运行可视化效果，编译命令：

g++ -o test KalmanFilter.cpp `pkg-config --cflags --libs opencv`

暴露更多细节的例子：

Predict(float delta_t) {
  if (inited_) {
    state_transition_matrix_(0, 2) = delta_t;
    state_transition_matrix_(1, 3) = delta_t;
    state_ = state_transition_matrix_ * state_;
    predict_state_ = state_;
    variance_ = state_transition_matrix_ * variance_ *
                    state_transition_matrix_.transpose() +
                process_noise_;
  }
}


Correct(const Eigen::VectorXd &z) {
  if (inited_) {
    Eigen::Vector2d measure;
    measure << z[0], z[1];
    // measurement covariance: S = H*P*H^T + R
    Eigen::Matrix2d cov =
        measure_matrix_ * variance_ * measure_matrix_.transpose() +
        measure_noise_;

    kalman_gain_ = variance_ * measure_matrix_.transpose() * cov.inverse();
    variance_ = variance_ - kalman_gain_ * measure_matrix_ * variance_;
    state_ = state_ + kalman_gain_ * (measure - measure_matrix_ * state_);

    // compute likelihood
    auto residual = measure - predict_state_.head(2);
    likelihood_ =
        std::exp(-0.5 * residual.transpose() * cov.inverse() * residual) /
        std::sqrt(2 * M_PI * cov.determinant());
  } else {
    Init(z);
  }
}

四、应用

滤波：若只有一种测量值，则处理结果为结合了系统转移矩阵的预测值和测量值后的最优估计，实际效果相当于滤波。

多传感器融合：若有多种通道源的观测值，在滤波的基础上实现多传感器融合的效果。以两传感器通道源为例，

若两通道同时获得观测数据，则通过预测->利用通道1观测值更新->利用通道2观测值更新的迭代形式实现两传感器信息融合；
若两通道观测数据不同时获得，则通过预测->利用通道1观测值更新->预测->利用通道2观测值更新的迭代形式实现两传感器信息融合。多传感器的情况依此类推。

参考代码：https://github.com/JunshengFu/tracking-with-Extended-Kalman-Filter

使用OpenCV库进行卡尔曼滤波：

https://docs.opencv.org/master/dd/d6a/classcv_1_1KalmanFilter.html

示例：https://docs.opencv.org/master/de/d70/samples_2cpp_2kalman_8cpp-example.html#a28

卡尔曼滤波 Kalman filter