说说卡尔曼滤波

更新2024/1/15：

个人认为从贝叶斯公式的角度来理解卡尔曼滤波更加直观，为此专门重新写了新的文章来尽量读懂卡尔曼滤波。

==============更新结束

这是我在知乎的第一篇文章。前一段时间，做项目研究了一下卡尔曼滤波，并且在项目当中实现了一个物体跟踪的功能，所以，借着新鲜劲儿，本次专栏对卡尔曼滤波进行一次整理。

卡尔曼滤波是什么

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

角度跟踪视频

卡尔曼滤波能做什么

假设我们手头有一辆DIY的移动小车。这辆车的外形是这样的：

这辆车可以在荒野移动，为了便于对它进行控制，需要知道它的位置以及移动速度。所以，建立一个向量，用来存储小车的位置和速度 ：

\overrightarrow{x_k}=(\overrightarrow{p},\overrightarrow{v})

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

卡尔曼滤波的工作原理

卡尔曼滤波的工作原理主要包括先验估计和后验估计。什么意思，请看下文：

1.先验状态估计

为了简化我们的例子，假如小车在一条绝对笔直的线路上面行驶，其运动轨迹是确定的，不确定的是小车的速度大小和位置。于是，套路来了：

创建状态变量：\overrightarrow{x}=\begin{bmatrix}p\\v\\ \end{bmatrix}

更直观地，下图表示的是一个状态空间，横坐标是速度，纵坐标是位置，平面上面的任意一个点就唯一地描述出这个小车的运动状态。

卡尔曼滤波器发生作用的前提是小车的速度和位置量在其定义域内具有正态的高斯分布规律。用数学语言来讲，就是每一个变量都是具有一个平均值\mu（这个值在变量的概率密度函数分布图的最中心位置，代表该数值是最可能发生的）和\sigma^2（这个数值代表方差，表示变量的不确定性程度）。那么，一谈到概率统计，我们马上可以想到：相互独立，或者互不相关。即已知其中一个变量的变换规律，我们无法推断出另外一个变量的变化规律。就像下图所示的这样（越亮的区域，表示发生的可能性越高）：

但是，相互独立的反面就是相关。下图所示的规律一看就明白，速度越大，位移也越大，这种情况下，两个变量相关（其实这种情况是很有可能发生的，因为速度越大的话，可能小车就远离我们，速度越小，表明靠近我们）。那么，通过协方差矩阵\Sigma 就能够将几个变量的相关程度描述清楚。矩阵当中的某一个元素\Sigma_{ij} 表示的是状态向量的第i个元素和第j个元素之间的相关程度，\Sigma_{ij}=Cov(x_i,x_j)=E[(x_i-\mu_i)(x_j-\mu_j)]。

要注意的是协方差矩阵是一个对称阵。感兴趣的童鞋可以深入研究一下，协方差矩阵的特征值和特征向量所具有的几何意义：the directions in which the data varies the most.啥意思，就是哪个方向变化快，特征向量指哪儿。

下面的两个链接供大家参考，有兴趣的要好好看看，知识点融会贯通了才觉得有意思！

协方差的特征向量是什么

协方差的几何意义

下面是重头戏：数学描述部分：

定义： \hat{x}_k=\begin{bmatrix}position\\velocity\\ \end{bmatrix}，P_k=\begin{bmatrix}\Sigma_{pp}&&\Sigma_{pv}\\\Sigma_{vp}&&\Sigma_{vv}\\\end{bmatrix}，\overrightarrow x_k是状态向量，P_k是协方差。

下图描述了一个k-1时刻\overrightarrow{x}_{k-1}和\overrightarrow{x}_k时刻的状态：我们其实是完全有理由根据前一时刻的状态来预测下一时刻的状态，这就是状态更新方程。

p_k=p_{k-1}+\delta tv_{k-1}

v_k=v_{k-1}

于是，习惯性地要写成矩阵的形式：

\hat{x}_k=\begin{bmatrix}1&&\delta t\\0&&1 \end{bmatrix}\hat{x}_{k-1}=F_k\hat{x}_{k-1}<br> (1)

状态向量的更新有了，但是还缺少状态向量之间相关性的更新，也就是协方差矩阵。

Cov(x)=\Sigma <br>Cov(Ax)=A\Sigma A^T

结合（1）得到：

\hat{x}_k=F_k\hat{x}_{k-1}（先验状态估计向量）

{P}_k=F_k P_{k-1}F_k^T（先验状态估计协方差矩阵）

外部确定性影响

到目前为止，我们只是关心系统内部状态的更新，但是如果在外部对系统产生了确定影响，比如：小车的操控者发出一条刹车的指令，我们通过建模该指令，假如小车的加速度为a，根据运动学的公式，得到：

\hat{p}_k=p_{k-1}+\delta tv_{k-1}+\frac{1}{2}a\delta t^2

v_k=v_{k-1}+a\delta t

写成矩阵的形式就是：

\hat{x}_k=F_kx_{k-1}+\begin{bmatrix}\frac{\delta t^2}{2}\\\delta t \end{bmatrix}a=F_kx_{k-1}+B_k\overrightarrow{u}_k其中，B_k是控制矩阵，\overrightarrow{u_k}是控制向量。由于本例子当中的控制实际上只包含了加速度，所以该向量包含元素的个数为1。
外部不确定性影响

现实世界往往是不那么好描述清楚的，就是存在不确定的外部影响，会对系统产生不确定的干扰。我们是无法对这些干扰进行准确的跟踪和量化的。所以，除了外界的确定项，还需要考虑不确定干扰项w_k。

从而，得到最终先验估计的更新方程：

\hat{x}_k=F_kx_{k-1}+B_k\overrightarrow{u}_k+w_k

每一次的状态更新，就是在原来的最优估计的基础上面，下一次的状态落在一个新的高斯分布区域，从坐标系上面看就像是一团云状的集合，最优的估计就在这个云团当中的某一处。所以，我们首先要弄清楚这个云团具有什么样的性质，也就是使用过程激励噪声协方差Q_k来描述不确定干扰。注意，E(w_k)=0。

到此，先验估计的过程结束，总结一下，形成如下的公式：

\hat{x}_k=F_k\hat{x}_{k-1}+B_k\overrightarrow{u}_k+w_k （2）

\hat{P}_k=F_k\hat{P}_{k-1}F_k^T+Q_k （3）

重点来了：
先验估计\hat{x}_k取决于如下三部分：一部分是上一次的最优估计值（也就是上一轮卡尔曼滤波的结果），一部分是确定性的外界影响值，另一部分是环境当中不确定的干扰。先验估计协方差矩阵P_k<br>，首先是依据第k-1<br>次卡尔曼估计（后验估计）的协方差矩阵进行递推，再与外界在这次更新中可能对系统造成的不确定的影响求和得到。

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

到此，利用\hat{x}_k和P_k<br>能够对系统进行粗略的跟踪，但是还不完善，因为人们总是不能够完全信任自身的经验，也需要另外的一条途径来纠正潜在发生的错误或者是误差。所以，我们很自然的想到在车身安装各类的传感器，比如速度传感器、位移传感器等等，以这些传感器的反馈作为纠正我们推断的依据。下面看看传感器的量测更新是怎么样对最终的估计产生影响的。下图说明的是状态空间向观测空间的映射。
在这里需要说明一下，卡尔曼滤波器的观测系统的维数小于等于动态系统的维数，即观测量可以少于动态系统中状态向量所包含的元素个数。

注意，传感器的输出值不一定就是我们创建的状态向量当中的元素，有时候需要进行一下简单的换算。即使是，有可能单位也不对应，所以，需要一个转换。这个转换就是矩阵H_k，在一些文献当中也被称作状态空间到观测空间的映射矩阵。

通过空间映射矩阵，依据我们先验估计值，在量测空间当中，传感器的测量值理想情况下应该是这样的：

\overrightarrow \mu_{expected}=H_k\hat x_k<br>

\Sigma_{expected}=H_kP_kH_k^T

但是，传感器对系统某些状态的测量真的准确吗？是不是也会有偏差呢？答案是肯定的。系统在某一个状态下，会推断出一组理想值，在另一个状态下，会有另外一组理想值，而对应时刻传感器的测量值一定是无法和理想值保持完全吻合的。由于测量噪声的存在，不同的系统状态下，测量具有一定误差，呈现高斯分布，但是这个高斯分布的最中心还是当前的测量值。所以，还需要一个观测噪声向量以及观测噪声协方差来衡量测量水平，我们将它们分别命名为v_k<br>和R_k<br>。下面的两张图说明这一点。

\overrightarrow z_t=H_k\hat x_k+\overrightarrow v_k

观测向量\overrightarrow {z_k}服从高斯分布，并且其平均值认为就是本次的量测值(z_1,z_2)。
下图看到的是两个云团，一个是状态预测值，另一个是观测值。那么到底哪一个具体的结果才是最好的呢？现在需要做的是对这两个结果进行合理的取舍（本质就是加权滤波），通过一种方法完成最终的卡尔曼预测。即：从图中粉红色云团和绿色云团当中找到一个最合适的点。

问题来了，怎么找？
首先来直观理解一下：考察观测向量\overrightarrow {z_k}和先验估计\hat x_k：存在两个事件：事件1传感器的输出是对系统状态真实值的完美测量，丝毫不差；事件2先验状态估计的结果就是系统状态真实值的完美预测，也是丝毫不差。但是大家读到这里心里非常清楚的一点是：两个事件的发生都是概率性的，不能完全相信其中的任何一个！！！！！！！
如果我们具有两个事件，如果都发生的话，从直觉或者是理性思维上讲，是不是认定两个事件发生就找到了那个最理想的估计值？好了，抽象一下，得到：两个事件同时发生的可能性越大，我们越相信它！要想考察它们同时发生的可能性，就是将两个事件单独发生的概率相乘。

那么，下一步就是对两个云团进行重叠，找到重叠最亮的点（实际上我们能够把云团看做一帧图像，这帧图像上面的每一个像素具有一个灰度值，灰度值大小代表的是该事件发生在这个点的概率密度），最亮的点，从直觉上面讲，就是以上两种预测准确的最大化可能性。也就是得到了最终的结果。非常神奇的事情是，对两个高斯分布进行乘法运算，得到新的概率分布规律仍然符合高斯分布，然后就取下图当中蓝色曲线峰值对应的横坐标不就是结果了嘛。证明如下：
我们考察单随机变量的高斯分布，期望为\mu，方差为\sigma ^2，概率密度函数为：

\mathcal{N}(x,\mu,\sigma) =\frac{1}{\sigma \sqrt{2\pi}}e^{-\frac{(x-\mu)^2}{2\sigma^2}} （4）

如果存在两个这样的高斯分布，只不过期望和方差不同，当两个分布相乘，得到什么结果？

是不是下式成立

\begin{equation}<br> \mathcal{N}(x,\mu_0,\sigma_0)\cdot \mathcal{N}(x,\mu_1,\sigma_1)\overset{?}{=}\mathcal{N}(x,\mu^{'},\sigma^{'}) \end{equation} （5）

将（4）代入（5），对照（4）的形式，求得：

\mu^{'} =\mu_0+k(\mu_1- \mu_0)

{\sigma^{'}}^2=k{\sigma_1}^2={\sigma_0}^2(1-k)

其中，k=\frac{\sigma_0^2}{\sigma_0^2+\sigma_1^2}

以上是单变量概率密度函数的计算结果，如果是多变量的，那么，就变成了协方差矩阵的形式：

K=\Sigma_0(\Sigma_0+\Sigma_1)^{-1}(6)

\overrightarrow\mu^{'}=\overrightarrow\mu_0+K(\overrightarrow\mu_1-\overrightarrow\mu_0)(7)

\Sigma^{'}=K\Sigma_1=\Sigma_0(I-K)(8)

K称为卡尔曼增益，在下一步将会起到非常重要的作用。 好了，马上就要接近真相！

卡尔曼估计

下面就是对传感器的量测结果和根据k-1时刻预测得到的结果进行融合。（由于刚才的推导是两个单变量，并且处在同一个空间内，下面的推导为了方便起见，我们将先验状态估计对应的结果映射到观测向量空间进行统一的运算）
第一个要解决的问题是：（7）和（8）两个式子中那个平均值和方差都对应多少？

(\mu_0,\Sigma_0)=(H_k\hat x_k,H_kP_kH_k^T)（9）

(\mu_1,\Sigma_1)=(z_k,R_k)（10）

将（9）和（10）代入（6）、（7）、（8）三个式子，整理得到：

H_k\hat x_k^{'}=H_k\hat x_k+K(\overrightarrow{z_k}-H_k\hat x_k)

H_kP_k^{'}H_k^T=H_kP_kH_k^T-KH_kP_kH_k^T

其中卡尔曼增益为：

K=H_kP_kH_k^T(H_kP_kH_k^T+R_k)^{-1}

最后一步，更新结果：

\hat x_k^{'}=\hat x_k+K^{'}(\overrightarrow{z_k}-H_k\hat x_k)

P_k^{'}=P_k-K^{'}H_kP_k

其中，

K^{'}=P_kH_k^T(H_kP_kH_k^T+R_k)^{-1}

\hat x_k^{'}就是第k次卡尔曼预测结果。P_k^{'}是该结果的协方差矩阵。它们都作为下一次先验估计的初始值参与到新的预测当中。总体上来讲，卡尔曼滤波的步骤大致分为两步，第一步是时间更新，也叫作先验估计，第二步是量测更新，也叫作后验估计，而当前的卡尔曼滤波过程的后验估计结果不仅可以作为本次的最终结果，还能够作为下一次的先验估计的初始值。下图是卡尔曼滤波的工作流程：

对卡尔曼滤波原理的理解，我参考了这篇文章，图片取自该文章，该文的图片和公式颜色区分是一大亮点，在此表示对作者的感谢。

举个栗子

这部分重点讲解一下利用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)

1状态向量初始化x_k

我想对物体的位置信息和速度信息进行跟踪，由于是二维的，所以位置信息x、y方向两个变量，

速度信息x、y方向两个变量，从而

m_KF.statePre

和

m_KF.statePost

是一个四维列向量。state=\begin{bmatrix}x\\y\\v_x\\v_y\end{bmatrix}该向量在初始化时设置为零。

2状态转移矩阵初始化F_k

在计算机屏幕上面，我自定义了一个该物体的运动空间，具有横纵坐标，后面会看到这个空间。

由于相机的帧率是30fps，所以相邻帧时间间隔\delta t\approx30ms，被测物体的实际速度大约为10mm/s，

所以在如此短的时间内，该物体能够认为是做匀速直线运动，故得到状态转移方程

F_k=\begin{bmatrix}1&&0&&\delta t&&0\\0&&1&&0&&\delta t\\0&&0&&1&&0\\0&&0&&0&&1\end{bmatrix}

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过程噪声激励协方差矩阵Q_k

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

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

4观测矩阵H_k

setIdentity(m_KF.measurementMatrix);

初始化得到：

H_k=\begin{bmatrix}0&&0&&1&&0\\0&&0&&0&&1\end{bmatrix}

由于传感器只是检测到了两个方向的速度，对位移没有检测，所以要将矩阵前两列初始化为0。

5预测估计协方差矩阵P_k

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

初始化为单位阵。

6测量噪声协方差矩阵R_k

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自带例子
教程
卡尔曼滤波的直觉理解

2023/12/17======================

最近又学习了一下这部分的内容，可以通过贝叶斯滤波来理解：

layout: post title: 卡尔曼滤波器学习笔记（一）：概率论和贝叶斯滤波 date: 2023-12-01 categories: Filters author: Yang tags: [Kalman Filtering] comments: true toc: false mathjax: true

感谢

点一个大大的赞！ 经典教材的重新排版

文章中深蓝色字体表示摘录自该教材。

给老王点赞！ 老王

这个作者真心用心地交互式展示数学和工程实践。 Kalman-and-Bayesian-Filters-in-Python

这篇容易让我们建立直觉的理解

3blue1brown-bayes-theorem

背景知识

一本很有名的书，学习作者对内容的安排。

需要的预备知识：

1. 线性代数
   
2. 概率，概率密度函数
   
3. 离散变量贝叶斯滤波
   
4. 连续变量贝叶斯滤波推导
   

学习心得

首先是概念理解，需要把概念代表的物理意义搞清楚：事件≠随机变量，随机变量是对事件的数量化描述，事件是随机变量的取值，随机变量的取值可以是一个数，也可以是一个向量。概率密度函数是对随机变量的取值的概率分布的描述，概率密度函数的积分就是概率。概率密度函数的积分就是概率，概率密度函数的积分就是概率，概率密度函数的积分就是概率。重要的事情说三遍。

亲自手写公式推导一遍，在这个过程中会强烈加深对内部逻辑的理解，争取可以做到经过很长时间之后，你仍然可以随便拿一张纸开始从零推导

感觉基于贝叶斯的随机过程计算其实是在更新变量的概率分布，并不是直接计算最终你看到的数据，最后你得到的数据只是这个过程当中的附带产品。

上面这一条的感受里面说的计算过程，基础是全概率下的全部样本空间，像是在一个无限大的平面上（因为真实值的取值是无穷无尽的）时刻飘过一团云，这团云和云笼罩下的区域是本次计算的结果。

符号说明

一般性地，我们用$\boldsymbol X$表示随机变量，用$$\boldsymbol x$$表示随机变量的某一个具体的取值。用$$\boldsymbol{y}$$表示测量值。$$\boldsymbol{u}$$表示系统的控制量。$$\boldsymbol{w}$$表示系统的过程噪声，$\boldsymbol{v}$表示测量噪声。黑体代表这些变量是向量。

概率论回顾

随机变量$\boldsymbol X$(Random Variable)的物理意义是把随机事件数量化。随机变量分为离散随机变量和连续随机变量。离散随机变量的取值是有限的，连续随机变量的取值是无限的。随机变量的取值可以是一个数，也可以是一个向量。个人觉得随机变量只是对随机事件的定量描述，是更加具象化的表达方式。更加核心的还是随机事件，所以下面的描述都是基于事件来推导公式的，并没有带入随机变量的表达。

条件概率的定义： 某事件B发生的条件下，事件A发生的概率，记为$P(A|B)$。 工程上是把事件看作随机变量在某一时刻取某一个数值，所以，在实际计算时，计算的是条件概率密度函数，并不是单单一个条件概率。

乘法公式： $P(AB)=P(A)P(B|A)=P(B)P(A|B)$ 描述的是随着时间的推移，事件A和事件B同时发生的概率可以通过事件A发生的概率和基于A发生B发生的概率相乘得到。方便针对每一个概率进行单独的计算从而得到一个总的概率。

全概率公式： 全概率公式是另一个很重要的公式，提供了计算复杂事件概率的一条有效的途径，使得一个复杂事件的概率可以通过简单的计算得到。

全概率公式：设$B_1,B_2,\cdots,B_n$是样本空间$\Omega$的一个划分，且$\cup_{i=1}^n B_i=\Omega$, $P(B_i)>0(i=1,2,\cdots,n)$，则对于任一事件$A$，有 $P(A)=\Sigma P(B_i)P(A|B_i)$

贝叶斯公式： 这个公式得到的前提是乘法公式和全概率公式。 设$B_1,B_2,\cdots,B_n$是样本空间$\Omega$的一个划分，且$\cup_{i=1}^n B_i=\Omega$, $P(B_i)>0(i=1,2,\cdots,n)$，则对于任一事件$A$，有 $P(B_i|A)=\frac{P(B_i)P(A|B_i)}{\Sigma_{j=1}^n P(B_j)P(A|B_j)}$

体会：虽然我没有查找托马斯·贝叶斯发现这个定理的过程是不是因为实际的应用问题，因为在状态估计这个领域，具有非常强的适配性，我们可以把样本空间理解成状态变量的空间，$B_i$理解成离散的单个样本，$A$理解成对样本空间的某次测量，$P(B_i)$理解成状态变量的先验概率，$P(A|B_i)$理解成测量值的似然概率 -- 就是假设真实值为$B_i$的条件下测量值为$A$的概率，$P(B_i|A)$理解成状态变量的后验概率（经过测量更新），这样就可以把贝叶斯公式应用到状态估计当中了。

维基百科里面说这个方程的分母是$P(B)$

然后我看到了它说$P(B)$是边缘分布，然后我点进去看啥是边缘分布： 边缘分布

在这里隐约感觉到：这里的论述和线性代数里面的空间向量怎么很类似？跟信号的傅里叶分解也很类似：全部都是把一个复杂的东西分解成一些简单的东西，然后再把这些简单的东西组合起来得到复杂的东西。这块简单的东西就是全样本空间下每一种样本的概率，类似于空间中的基向量，或者傅里叶变换中的基函数，然后这些基函数前面的系数就是权重或者是事件A和$B_i$重叠了多少程度，然后求和就得到了边缘概率。

最后再次用更好记忆的方式写一遍贝叶斯公式：

后验条件概率=\frac{先验概率*似然概率}{边缘概率}

条件概率是概率论中一个既重要又实用的概念。 -- 1.4《概率论与数理统计》茆诗松

• pdf: probability density function, 概率密度函数
• cdf: cumulative distribution function, 累积分布函数
• 先验概率：就是人们根据自己系统的模型给出来的经验概率
• 后验概率：就是通过传感器本身的测量特性--传感器测某个状态得到的结果这个变量的概率分布和先验的概率分布，得到的对系统的状态变量条件概率$P(X=x|Y=y)$
• 似然概率：是一个条件概率，意思是当真实值取值为$x$的时候，测量值取值为$y$的概率，$P(Y=y|X=x)$ (这里默认测量值测量到的就是真实值)

我觉得还需要加一个边缘概率：边缘概率就是全概率公式对于某一个事件的应用，就是基于所有可能的状态量，测量得到$y$之后的条件概率和先验概率乘积再相加。展现形式是和全概率一样的，只不过这里具有了更多的实际意义。

这里有一个问题抛出来：假如先验概率变了，后验概率会变吗？似然概率会变吗？边缘概率会变吗？

我的回答是似然概率不变，边缘概率会变，因为从全概率公式就可以知道那些“基底”变了，前面的参数没变，整个求和就变了，所以后验概率也变了。

老王的视频必须看一看

Kalman-and-Bayesian-Filters-in-Python

桥梁

在纯讲概率的书籍当中，很少结合工程应用来说明概念和实际的对应关系，导致中间缺少一个可以让工程师充分理解的概念体系：

比如你作为某公司机器人系统研发工程师，在拿到如下的产品性能参数之后，需要开发一套可以稳定准确跟踪目标的机器人系统，这个系统可以输出空间内的特定目标的三维坐标和旋转信息，但是它的输出数据具有误差，那个RMS就代表了在不同的深度下（对于相机而言）你的系统使用到的数据的不确定性。例如在距离相机2.4m的标记点，测量数据和实际值之间最差会达到0.11mm，凭借直觉你肯定在想，假如真实值是$x=5$, 出现$y=5.01$的情况的概率是多大？出现$y=5.11$的情况的概率又是多大？但你的系统又想尽量按照真实值去跟踪目标，这时候你会想到设计一个滤波器去解决这个不确定性，如何设计能够把不确定性降到最低？你设计的这个方案肯定是需要把这个视觉系统的技术参数作为配置项应用进去的，怎么应用呢？想要解决这个工程问题，需要首先清楚如下几个概念：

最基本的我认为是状态量或者测量量是随着时间变化的，是一个变量，而这个变量自带一个概率的分布，就是随机变量的分布函数$F(X)$，它表示当随机变量$X=x_0$这件事情发生的概率有多大。但是对于连续随机变量来说，在每一个点上这个变量发生的概率是0，这个事情就让人比较难以理解和处理，所以就定义出来连续随机变量的概率密度函数，对概率密度函数求积分就得到了$F(X<x_0)$的概率分布函数。

我们对随机变量的估计是根据测量数据而得到的，怎样把测量值考虑进去？不能绝对地相信测量值，因为传感器有误差，不能绝对地相信先验分布，因为很多时候不可能掌握，比如自动驾驶汽车在碎石路上行驶的过程中地面的摩擦系数变化很大，车轮容易打滑，汽车的纵向速度随时都有可能发生变化，所以需要根据车轮上的编码器和车辆的运动模型折中地做出估计。

连续随机变量

需要解决的问题： 1. 贝叶斯公式推导的是单独事件的概率，实际应用过程中需要的是对连续随机变量的概率密度函数进行后验估计，怎么做？ 2. 前面的内容都是需要在已知全样本以及对应的概率情况下计算当前的后验概率分布，但是实际情况中，我们无法预先知道全样本的分布情况，也不可能重复多次做实验来得到全样本，因为首先一点是连续变量理论上没有办法做有限的实验来覆盖所有的情况。

上面的问题对应于贝叶斯统计的相关知识。

贝叶斯统计推断的基础是总体信息，样本信息和先验信息。三个概念是后面推导的定性指导，可以帮助我们对公式进行变形。

总体信息是总体的分布或者是总体所属分布族提供的信息。 我对总体信息的理解是包含了并不需要很细致的参数信息，例如只是知道了分布是正态分布，并不清楚期望和方差是多少，或者书中举的例子，我国确认国产轴承寿命分布是韦布尔分布花费了五年时间。

样本信息是抽取样本所得观测值提供的信息，属于本次实验得到的结果。它可以进一步让人了解总体分布的具体参数，例如总体均值，总体方差等等。有了样本，才有了统计学，才可以通过计算机具体地描述这个分布的特性，也就是参数化这个分布（或者穷举出来）

先验信息，是和前面的样本信息对应的，就是在做实验之前，需要首先了解这个问题在历史上或者经验情况，利用这种信息得到的分布叫做先验分布。 我的理解：例如历史经验，根据牛顿第二定律，力，速度，加速度，位移具有一定的约束关系，如果我们关心的随机变量是速度，但是我们知道加速度和受力的关系，那么，通过函数的映射，就可以得到速度的分布。-- 这里我的理解可能比较牵强，需要后面再次确认是否正确，因为这里可能是对随机变量的函数的分布的理解。

摘录书中的原话：

贝叶斯学派的基本观点是：任一未知量$\boldsymbol\theta$都可以看作随机变量，可以用一个概率分布去描述，这个分布是先验分布。在获得样本之后，总体分布、样本与先验分布通过贝叶斯公式结合起来得到未知量$\boldsymbol\theta$新的分布--后验分布。任何关于$\boldsymbol\theta$的统计推断都应该基于后验分布。

贝叶斯公式的概率密度函数形式： 原书中介绍得已经很详细了，这里不再重复，只是写出来我的理解。 1.

基本的概念清楚了之后，我们关心的是后验概率，因为贝叶斯的统计推断就是得到后验概率来更准确地描述随机变量的分布情况。

当测量值是$y$的条件下，不同的$x$取值对应的概率分布是什么？其实这么写等于没有写，因为在任何一个点上面，结果是0！出现这个情况的原因是连续随机变量的概率是概率密度函数在这个点上的积分永远为0。

所以我们先写成概率分布函数，然后计算完概率分布函数之后对变量求导数，就得到了这个变量的概率密度函数。准确地说是条件概率分布函数： P(X<x|Y=y)=\int_{-\infty}^xf(x|y)dx

然后根据贝叶斯后验概率计算公式，尝试写一下求和后面的部分：

P(X=u|Y=y)=\frac{P(Y=y|X=u)P(X=u)}{P(Y=y)}

但是上面的式子是不能这么写的，因为分母是0。

改写为极限的形式：

P(X=u|Y=y)=\lim _{\epsilon \to 0}\frac{P(y<Y<y+\epsilon|X=u)P(X=u)}{P(y<Y<y+\epsilon)}

所以需要寻找一种恰当的表达方式来说明问题，根据连续变量的条件分布-- 3.5.1 条件分布 二 连续变量的条件分布:如果随机变量的概率密度函数是连续可导的，那么，取一个极限表达。 一般地，描述连续随机变量的概率分布写成贝叶斯公式如下：

P(X<x|Y=y)=\lim _{\epsilon \to 0} \Sigma _{u=-\infty}^x\frac{P(y<Y<y+\epsilon|X=u)P(u<X<u+\epsilon)}{P(y<Y<y+\epsilon)}

看到局部区间的概率，可以使用概率密度函数进行替代：

\frac{P(y<Y<y+\epsilon|X=u)P(X=u)}{P(y<Y<y+\epsilon)}=\frac{\int_{v=y}^{v=y+\epsilon}f_{Y|X}(v|u)dv\int_{v=u}^{v=u+\epsilon}f_X(v)dv}{\int_{v=y}^{v=y+\epsilon}f_Y(v)}

求和和取极限之间没有关系，把取极限放入到求和公式内部并且利用积分中值定理得到,注意等式右边的积分范围是$(y, y+ \epsilon)$：

\lim _{\epsilon \to 0} \frac{P(y<Y<y+\epsilon|X=u)P(X=u)}{P(y<Y<y+\epsilon)}=\lim _{\epsilon \to 0}\frac{(\int f(v|u)dv) / \epsilon \times \int f_X(v)dv / \epsilon}{(\int f_Y(v)dv) / \epsilon} =\lim _{\epsilon \to 0} \frac{f(y|u)f_X(u)}{f_Y(y)}\epsilon

上面的式子代入到求和公式当中，得到：

P(X<x|Y=y)=\lim _{\epsilon \to 0} \Sigma _{u=-\infty}^x\frac{f(y|u)f_X(u)}{f_Y(y)} \epsilon

推导到这里，再利用积分的定义， 就可以得到连续随变量的后验概率分布公式：

P(X<x|Y=y)=\int_{-\infty}^{x} \frac{f(y|u)f_X(u)}{f_Y(y)}du

在书里面概率密度函数写为$p$,在这里我写为了$f$

总结

贝叶斯滤波认为对随机变量的估计需要有一个先验的分布，也需要有采样得到的样本，这样综合起来把对随机变量的分布重新按照后验的方式进行估计，得到的分布结果是更接近于真实情况的。

参考

The Hilbert Space of Random Variables Electrical Engineering 126 (UC Berkeley)

iLecture

卡尔曼滤波算法的几何解释. 作者，范洪达,李相民

An Elementary Introduction to Kalman Filtering

A New Approach to Linear Filtering and Prediction Problems

!!!GREAT series: Kalman-and-Bayesian-Filters-in-Python

[思考，快与慢](https://book.douban.com/subject/10785583/)