卡尔曼滤波原理及实现

前一段时间，做项目研究了一下卡尔曼滤波，并且在项目当中实现了一个物体跟踪的功能，所以，借着新鲜劲儿，本次博客对卡尔曼滤波进行一次整理。

卡尔曼滤波是什么
卡尔曼滤波能做什么
卡尔曼滤波的工作原理
举个栗子

卡尔曼滤波是什么

卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声，观测值不准确，卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导，会让人很头疼，难以把握其中的主线和思想。所以我参考了国外一位学者的文章，讲述卡尔曼滤波的工作原理，然后编写了一个基于OpenCV的小程序给大家做一下说明。下面的这个视频请大家先直观地看看热闹吧~
角度跟踪视频

卡尔曼滤波能做什么

假设我们手头有一辆DIY的移动小车。这辆车的外形是这样的：
这里写图片描述
这辆车可以在荒野移动，为了便于对它进行控制，需要知道它的位置以及移动速度。所以，建立一个向量，用来存储小车的位置和速度

x k - \to = (p \to, v \to)

其实，一个系统的状态有很多，选择最关心的状态来建立这个状态向量是很重要的。例如，状态还有水库里面水位的高低、炼钢厂高炉内的温度、平板电脑上面指尖触碰屏幕的位置等等这些需要持续跟踪的物理量。好了，回归到正题，小车上面安装了GPS传感器，这个传感器的精度是10米。但是如果小车行驶的荒野上面有河流和悬崖的话，10米的范围就太大，很容易掉进去进而无法继续工作。所以，单纯靠GPS的定位是无法满足需求的。另外，如果有人说小车本身接收操控着发送的运动指令，根据车轮所转动过的圈数时能够知道它走了多远，但是方向未知，并且在路上小车打滑车轮空转的现象绝对是不可避免。所以，GPS以及车轮上面电机的码盘等传感器是间接地为我们提供了小车的信息，这些信息包含了很多的和不确定性。如果将所有这些信息综合起来，是否能够通过计算得到我们更想要的准确信息呢？答案是可以的！

这里写图片描述

卡尔曼滤波的工作原理

1.先验状态估计

以之前我们创建的状态变量为例，

x \to = [p v]

下图表示的是一个状态空间图，为了研究方便，假如小车在一条绝对笔直的线路上面行驶，其位置和速度的方向是确定的，不确定的是大小。
这里写图片描述
卡尔曼滤波器发生作用的前提是小车的速度和位置量在其定义域内具有正态的高斯分布规律。每一个变量都具有一个平均值

x^k = [p o s i t i o n v e l o c

P k = [Σ p p

C o v (x) = Σ

C o v (A x) = A Σ A T

2.后验估计（量测更新）

到此，利用和能够对系统进行粗略的跟踪，但是还不完善。因为安装在系统上面的传感器会对系统的状态进行测量反馈，纠正估计。下面看看传感器的量测更新是怎么样对最终的估计产生影响的。下图说明的是状态空间向观测空间的映射。
在这里需要说明一下，卡尔曼滤波器的观测系统的维数小于等于动态系统的维数，即观测量可以少于动态系统中状态向量所包含的元素个数。
这里写图片描述
注意，传感器的输出值不一定就是我们创建的状态向量当中的元素，有时候需要进行一下简单的换算。即使是，有可能单位也不对应，所以，需要一个转换。这个转换就是矩阵

N (x, μ, σ) = 1 σ 2 π - -

N (x, μ 0, σ 0) \cdot N (x,

K = Σ 0 (Σ 0 + Σ 1)

μ \to' = μ \to

Σ' = Σ 0 - K Σ 0

(μ 0, Σ 0) = (H k

(μ 1, Σ 1) = (z k, R

H k x^' k

H k P' k H

举个栗子

这部分重点讲解一下利用OpenCV如何实现一个对三维空间内物体的二维平面跟踪。
背景：跟踪一个移动速度大小和方向大致保持不变的物体，检测该物体的传感器是通过对物体拍摄连续帧图像，经过逐个像素的分析计算，得到x、y方向的速度，将两个速度构成一个二维的列向量作为观测向量。
下面看一下OpenCV当中对卡尔曼滤波的支持和提供的接口说明。
OpenCV2.4.13-KalmanFilter
下面是参与到卡尔曼滤波的一些数据结构，它们代表的意义在其后面用英文进行了描述。
OpenCV将以下的成员封装在了KalmanFilter当中，我们使用时候，可以直接实例化一个对象，例如：

KalmanFilter m_KF;

//CV_PROP_RW Mat statePre;           //!< predicted state (x\'(k)): x(k)=A*x(k-1)+B*u(k)
//CV_PROP_RW Mat statePost;          //!< corrected state (x(k)): x(k)=x\'(k)+K(k)*(z(k)-H*x\'(k))
//CV_PROP_RW Mat transitionMatrix;   //!< state transition matrix (A)
//CV_PROP_RW Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)
//CV_PROP_RW Mat measurementMatrix;  //!< measurement matrix (H)
//CV_PROP_RW Mat processNoiseCov;    //!< process noise covariance matrix (Q)
//CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
//CV_PROP_RW Mat errorCovPre;        //!< priori error estimate covariance matrix (P\'(k)): P\'(k)=A*P(k-1)*At + Q)*/
//CV_PROP_RW Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P\'(k)*Ht*inv(H*P\'(k)*Ht+R)
//CV_PROP_RW Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P\'(k)

KalmanFilter类的成员函数具有如下几个：

KalmanFilter
init
predict
correct

方法很简单，但是需要知道如何使用：
程序当中，我单独写了一个类，在我的计算线程（就是获取到量测结果的线程）当中对该类进行实例化并且调用其中的方法。量测结果存放在

m_dispacementVector[0] = m_xSpeed;
m_dispacementVector[1] = m_ySpeed;

当中。
步骤一：

CV_WRAP KalmanFilter( int dynamParams, int measureParams, int controlParams = 0, int type = CV_32F );

    /** @brief Re-initializes Kalman filter. The previous content is destroyed.

    @param dynamParams Dimensionality of the state.
    @param measureParams Dimensionality of the measurement.
    @param controlParams Dimensionality of the control vector.
    @param type Type of the created matrices that should be CV_32F or CV_64F.
     */

在卡尔曼滤波类的构造函数成员列表当中首先告知OpenCV过程状态向量的维度和观测向量的维度：

m_KF(4,2,0)

对m_KF成员的初始化：

1状态向量初始化

我想对物体的位置信息和速度信息进行跟踪，由于是二维的，所以位置信息x、y方向两个变量，速度信息x、y方向两个变量，从而m_KF.statePre和m_KF.statePost是一个四维列向量。

s t a t e = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ x y v

2状态转移矩阵初始化

在计算机屏幕上面，我自定义了一个该物体的运动空间，具有横纵坐标，后面会看到这个空间。由于相机的帧率是30fps，所以相邻帧时间间隔

F k = ⎡ ⎣ ⎢ ⎢ ⎢ 100

m_KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 0.03, 0,0,1,0,0.03,0,0,1,0,0,0,0,1);

3过程噪声激励协方差矩阵

setIdentity(m_KF.processNoiseCov, Scalar::all(1e-5));//Q

认为过程激励噪声比较弱，并且每个分量相互之间不存在相关系。

4观测矩阵

setIdentity(m_KF.measurementMatrix);

初始化得到：

H k = [000

5预测估计协方差矩阵

setIdentity(m_KF.errorCovPost, Scalar::all(1));

初始化为单位阵。

6测量噪声协方差矩阵

setIdentity(m_KF.measurementNoiseCov, Scalar::all(1e-1));//R

步骤二：
先验估计：

m_prediction = m_KF.predict();

步骤三：
后验估计：
首先需要告知卡尔曼滤波器最新的传感器数据：

    m_dispacementVector[0] = m_xSpeed;
    m_dispacementVector[1] = m_ySpeed;

m_pKalman->updateMeasurements(m_dispacementVector);

void kalmanFilter::updateMeasurements(double p[])
{
    m_measurement = (Mat_<float>(2, 1) << p[0], p[1]);
}

接着完成后验估计：

m_KF.correct(m_measurement);

KalmanFilter.h:

#include <QObject>
#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"
using namespace cv;
class kalmanFilter:public QObject
{
    Q_OBJECT
public:
    kalmanFilter();
    ~kalmanFilter();
    void initKalman();
    void kalmanPredict();
    void updateMeasurements(double p[]);
    void kalmamCorrect();
    double *returnResult();
    void drawArrow(Point start, Point end, Scalar color, int alpha, int len);
private:
    KalmanFilter m_KF;
    Mat m_state;
    Mat m_postCorrectionState;
    Mat m_processNoise;
    Mat m_measurement;
    Mat m_img;
    Mat m_prediction;
    Point2f m_PointCenter;
};

KalmanFilter.cpp:

#include "kalmanFilter.h"
#include <iostream>
#include <fstream>
//CV_PROP_RW Mat statePre;           //!< predicted state (x\'(k)): x(k)=A*x(k-1)+B*u(k)
//CV_PROP_RW Mat statePost;          //!< corrected state (x(k)): x(k)=x\'(k)+K(k)*(z(k)-H*x\'(k))
//CV_PROP_RW Mat transitionMatrix;   //!< state transition matrix (A)
//CV_PROP_RW Mat controlMatrix;      //!< control matrix (B) (not used if there is no control)
//CV_PROP_RW Mat measurementMatrix;  //!< measurement matrix (H)
//CV_PROP_RW Mat processNoiseCov;    //!< process noise covariance matrix (Q)
//CV_PROP_RW Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
//CV_PROP_RW Mat errorCovPre;        //!< priori error estimate covariance matrix (P\'(k)): P\'(k)=A*P(k-1)*At + Q)*/
//CV_PROP_RW Mat gain;               //!< Kalman gain matrix (K(k)): K(k)=P\'(k)*Ht*inv(H*P\'(k)*Ht+R)
//CV_PROP_RW Mat errorCovPost;       //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P\'(k)

using namespace cv;
using namespace std;
kalmanFilter::kalmanFilter()
:m_KF(4,2,0)
, m_state(4,1,CV_32F)
, m_processNoise(4, 1, CV_32F)
, m_measurement(2,1,CV_32F)
, m_img(300, 300, CV_8UC3)
, m_PointCenter(m_img.cols*0.5f, m_img.rows)
{
    m_KF.transitionMatrix = (Mat_<float>(4, 4) << 1, 0, 0.03, 0,0,1,0,0.03,0,0,1,0,0,0,0,1);//A
    setIdentity(m_KF.measurementMatrix);
    setIdentity(m_KF.processNoiseCov, Scalar::all(1e-5));//Q
    setIdentity(m_KF.measurementNoiseCov, Scalar::all(1e-1));//R
    setIdentity(m_KF.errorCovPost, Scalar::all(1));
}
kalmanFilter::~kalmanFilter()
{
}

void kalmanFilter::initKalman()
{
    m_state = (Mat_<float>(4, 1) << 0,0,0,0);
    m_KF.statePre = (Mat_<float>(4, 1) << m_PointCenter.x, m_PointCenter.y, 0, 0);
    m_KF.statePost = (Mat_<float>(4, 1) << m_PointCenter.x, m_PointCenter.y, 0, 0);
    m_KF.measurementMatrix = (Mat_<float>(2, 4) << 0,0, 1, 0,0,0,0,1);
}

void kalmanFilter::updateMeasurements(double p[])
{
    m_measurement = (Mat_<float>(2, 1) << p[0], p[1]);
}

void kalmanFilter::kalmanPredict()
{
    m_prediction = m_KF.predict();
}

void kalmanFilter::kalmamCorrect()
{
    m_KF.correct(m_measurement);
     m_postCorrectionState = m_KF.statePost;
     //show the result
     m_img = Scalar::all(0);
     //measured result
     drawArrow(Point(m_KF.statePost.at<float>(0, 0), m_KF.statePost.at<float>(1, 0)),
         Point(m_KF.statePost.at<float>(0, 0) + m_measurement.at<float>(0, 0), m_KF.statePost.at<float>(1, 0) + m_measurement.at<float>(1, 0)),
         Scalar(0, 0, 255), 20, 15);//B G R
     putText(m_img, "measurement", Point(m_KF.statePost.at<float>(0, 0) + m_measurement.at<float>(0, 0), m_KF.statePost.at<float>(1, 0) + m_measurement.at<float>(1, 0)-5), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 2);
     //predicted result
     drawArrow(Point(m_KF.statePost.at<float>(0, 0), m_KF.statePost.at<float>(1, 0)),
         Point(m_KF.statePost.at<float>(0, 0) + m_KF.statePost.at<float>(2, 0), m_KF.statePost.at<float>(1, 0) + m_KF.statePost.at<float>(3, 0)),
         Scalar(0,255, 0), 20, 15);
     putText(m_img, "Kalman", Point(m_KF.statePost.at<float>(0, 0) + m_KF.statePost.at<float>(2, 0)-10, m_KF.statePost.at<float>(1, 0) + m_KF.statePost.at<float>(3, 0)-5), FONT_HERSHEY_PLAIN, 1, Scalar(0, 255, 0), 2);
     imshow("Kalman", m_img);
     ofstream myfile("example.txt", ios::app);
     myfile << "measure" << "   "<<m_measurement.at<float>(0, 0) << "   " << m_measurement.at<float>(1, 0)<<"   ";
     myfile << "kalman" << "    " << m_KF.statePost.at<float>(2, 0) << "    " << m_KF.statePost.at<float>(3, 0) << endl;
}

double * kalmanFilter::returnResult()
{
    double result[4];
    for (int i = 0; i < 4; i++)
    {
        result[i] = m_postCorrectionState.at<double>(i, 1);
    }
    return result;
}

void kalmanFilter::drawArrow(Point start, Point end, Scalar color,int alpha,int len)
{
    const double PI = 3.1415926;
    Point arrow; 
    double angle = atan2((double)(start.y - end.y), (double)(start.x - end.x));
    line(m_img, start, end, color,2);
    arrow.x = end.x + len * cos(angle + PI * alpha / 180);
    arrow.y = end.y + len * sin(angle + PI * alpha / 180);
    line(m_img, end, arrow, color,1);
    arrow.x = end.x + len * cos(angle - PI * alpha / 180);
    arrow.y = end.y + len * sin(angle - PI * alpha / 180);
    line(m_img, end, arrow, color,1);
}

下面是录制的程序运行效果图(抱歉物体未显示)：
这里写图片描述
参考资料：
卡尔曼滤波器OpenCV自带例子
 教程
 卡尔曼滤波的直觉理解

本文链接：https://www.cnblogs.com/zhoug2020/archive/2004/01/13/7623134.html

卡尔曼滤波原理及实现