在我总结Kalman filtering之前请允许我发泄一下，网上的各版本的卡尔曼滤波方程的变量字母真是他妈多，而范例却全都是同一个测量气温的简单例子，单纯看书的话公式自己又推不出来，真是日了狗了。

好了，说到卡尔曼滤波，才华横溢卡尔曼老先生到去年终于是去世了（此处仅有敬意），而卡尔曼滤波的作用却日益彰显。大概我对卡尔曼滤波的初步理解就是（反正这句话也是抄的，看看就好了，我其实也不懂）：根据当前时刻的观测值、上一时刻的预测值及预测误差,计算得到当前的最优量去预测下一刻的量。至于对卡尔曼滤波意义理解的话可以在知乎上搜一下，有个测猪体重的例子感觉十分生动。公式推导的话智商过低的本人也是推不出来的，所以在此仅希望帮助大家学会运用，如果帮得上的话。

首先，我们引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：

X(k)=AX(k-1)+BU(k)+W(k)

Z(k)=HX(k)+V(k)

X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声，他们的协方差分别是Q，R。由于系统中一般不太有控制量，所以B这个参数一般为0，也就是没有U(K)。

以下是编程需要的五个卡尔曼滤波的迭代方程：

首先利用系统的过程模型来预测系统下一状态，设在k时刻的系统状态为x(k),则可以根据系统模型，由上一状态预测出现在状态：

X(k|k-1)=AX(k-1|k-1)+Bu(k).....................(1)

其中x(k|k-1)是上一时刻的状态对现在时刻状态的预测，x(k-1|k-1)是上一时刻状态的最优结果， u(k)为现在时刻状态的控制量。

（一开始看我也一脸蒙蔽，主要看一下X(k |k-1)这样的变量到底代表什么）

系统的状态已经更新，现在需要更新系统的误差估计协方差矩阵，用p(k|k-1)表示误差估计协方差矩阵：

P(k|k-1)=A*P(k-1|k-1)A'+Q.......................(2)

（这个协方差的由来是由(1)式的预测方程得到的，你看他乘的系数也是A就懂了）

其中p(k|k-1)是在k时刻由上一状态对此状态的预测， p(k-1|k-1)是x(k-1|k-1)对应的误差估计协方差矩阵，Q表示系统过程噪声的协方差。

现在我们得到了预测结果，然后我们根据得到的现在状态的测量值进行修正得到最优的估计量x(k|k):

X(k|k)=X(k|k-1)+Kg(k)*(Z(k)-Hx(k|k-1))....(3)

（常常在编程的时候会直接把（1）式直接代入这个式子，所以你们看不到（1）式 ）

(3)式中Kg(k)未知，则需要对其就行求解，就引出(4)式：

Kg(k)=P(k|k-1)*H'/(H*P(k|k-1)*H' + R)......(4)

(这个是卡尔曼增益，你只要知道这是用来修正预测值的一个参数就好了)

到现在，我们以及得出的k 时刻的系统状态的最优值x(k|k),为了让卡尔曼滤波器不断地进行下去，我们需要更新 x(k|k) 对应的p(k|k)

p(k|k)=(I-Kg(k)*H)*P(k|k-1).....................(5)

((3)(4)式计算出来的X(k|k)、P(k|k)又会迭代回(1)（2）式中，注意I是单位矩阵)

我们还是先来看看网上大肆流传的一个例子吧。白字是原文，红字是我的吐槽。

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的。（这里的假设相当于状态方程的系数A为1）假设你对你的经验不是100%的相信，可能会有上下偏差几度，我们把这些偏差看成是高斯白噪声（这里就是W(k)）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。（温度计的测量值就是Z(k),而由于温度测到的温度就是温度，不用再换算，所以系数H就是1，偏差就是V(k)）。好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值X(k|k-1)）和温度计的值（测量值Z(k)）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差（p(k-1|k-1)就是上一时刻的p(k|k)，反正你懂）是3，你对自己预测的不确定度是4度（这个就是Q了，返回去看看黄色标注的Q是什么呗），他们平方相加再开方，就是5（算出来的就是P(k|k-1)））。然后，你从温度计那里得到了k时刻的温度值（测量值Z(k)），假设是25度，同时该值的偏差是4度（这个就是R了）。由于我们用于估算k时刻的实际温度有两个温度值，分别是23 度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance（协方差）来判断。因为 Kg^2=5^2/(5^2+4^2)(该式的计算相当于上面的（4）式子)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度（该式对应（3）式，就是算出最优估算值）。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。（我一直搞不懂这个协方差为什么一定要求几何的误差也就是为什么要平方再开根号，如果有人明白麻烦在评论里告诉我一下）

现在我们已经得到k时刻的最优温度值了，下一步就是要进入 k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56 度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35（该式对应（5）式）。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入 k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。就是这样，卡尔曼滤波器就不断的把 covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！（自学了好久菜看懂这个例子是什么意思）

我觉得应该我的标注应该能让大家看懂这个例子了解整个迭代计算的过程了，下面我们看看程序吧，这个是一个比较简单的波形跟随，希望能给大家一点启示。

后续可能还有关于不同信号类型的卡尔曼滤波分析

（见http://blog.csdn.net/sillykog/article/details/78535767）

对Kalman(卡尔曼)滤波器的理解

1．简介(Brief Introduction)

在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！

卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf

卡尔曼滤波器到底是干嘛的？我们来看下wiki上的解释：

卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用(如雷达、计算机视觉)中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。例如,对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。
斯坦利.施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。 关于这种滤波器的论文由Swerling (1958)、Kalman (1960)与 Kalman and Bucy (1961)发表。
目前,卡尔曼滤波已经有很多不同的实现.卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

2．卡尔曼滤波器的介绍（Introduction to the Kalman Filter）
为了可以更加容易的理解卡尔曼滤波器，首先应用形象的描述方法来讲解，然后我们结合其核心的5条公式进行进一步的说明和探索。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。
在介绍他的5条公式之前，先让我们来根据下面的例子做个直观的解释。
假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。
好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。
由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。
现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。
就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！
下面就要言归正传，讨论真正工程系统上的卡尔曼。

3． 卡尔曼滤波器算法（The Kalman Filter Algorithm）
在这一部分，我们就来描述源于Dr Kalman 的卡尔曼滤波器。下面的描述，会涉及一些基本的概念知识，包括概率（Probability），随即变量（Random Variable），高斯或正态分配（Gaussian Distribution）还有State-space Model等等。但对于卡尔曼滤波器的详细证明，这里不能一一描述。

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述，我们结合下面PPT截图进行说明：

上两式子中，x(k)是k时刻的系统状态，u(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。y(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。q(k)和r(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。先给出KF算法的流程和五个核心更新方程如下：

KF算法

五个更新方程为：

编写公式不方便，所以写成了PDF然后做了截图粘在了下面，下面就上面的例子和五个核心的公式对Kalman算法进行下说明：

就这样，算法就可以自回归的运算下去。

看到这聪明的同学可能已经看出来了，问道卡尔曼增益为什么会是第三步中那样求，现在只大致说一下原理，具体推到比较复杂，有兴趣的同学可以参考这文献去推一推。
还记得前面我们说的误差协方差矩阵$P_k$么，即求第k次最优温度的误差协方差矩阵，对应于上例中的3和2.35....这些值。看下面PPT，我们最小化P即可得到卡尔曼增益K，对应上例求解K只最小化最优温度值的偏差，即最小化P(K)：

我们由第四步可以看出，k时刻系统的最优温度值=k-1时刻状态估计值（由上一状态的最优温度值加上过程误差）+带卡尔曼增益权值项的偏差。如果观测误差远远大于估计误差，那么K就很小，k时刻的预测值约等于k时刻的状态估计值，如果对i时刻的状态估计值误差远远大于观测误差，此时相应的q较大，K较大，i时刻的状态估计值更倾向于观察的数据。

卡尔曼滤波器的原理基本描述就完成了，希望能帮助大家理解这这5个公式，其算法可以很容易的用计算机的程序实现。下面，我会用程序举一个实际运行的例子。

4． 简单例子（A Simple Example）
这里我们结合第二第三节，举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子，而且还会配以程序模拟结果。
根第二节的描述，把房间看成一个系统，然后对这个系统建模。当然，我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的，所以A=1。没有控制量，所以u(k)=0。因此得出：
x(k|k-1)=x(k-1|k-1) ……… (6)
式子（2）可以改成：
P(k|k-1)=P(k-1|k-1) +Q ……… (7)
因为测量的值是温度计的，跟温度直接对应，所以H=1。式子3，4，5可以改成以下：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=（1-Kg(k)）P(k|k-1) ……… (10)
现在我们模拟一组测量值作为输入。假设房间的真实温度为25度，我模拟了200个测量值，这些测量值的平均值为25度，但是加入了标准偏差为几度的高斯白噪声（在图中为蓝线）。
为了令卡尔曼滤波器开始工作，我们需要告诉卡尔曼两个零时刻的初始值，是X(0|0)和P(0|0)。他们的值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，X会逐渐的收敛。但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。我选了X(0|0)=1度，P(0|0)=10。

该系统的真实温度为25度，图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果（该结果在算法中设置了Q=1e-6，R=1e-1）。

附matlab下面的kalman滤波程序：

1. clear

2. N=200;

3. w(1)=0;                     %w为过程噪声

4. w=randn(1,N)

5. x(1)=25;

6. a=1;                        %a为方程中A(k)

7. for k=2:N;

8. x(k)=a*x(k-1)+w(k-1);

9. end

10. 
    

11. V=randn(1,N);               %V为观察噪声

12. q1=std(V);

13. Rvv=q1.^2;

14. q2=std(x);

15. Rxx=q2.^2;

16. q3=std(w);

17. Rww=q3.^2;

18. c=0.2;                      %c为方程中H(k)

19. Y=c*x+V;                    %Y为观察值

20. 
    

21. p(1)=0;

22. s(1)=0;

23. for t=2:N;

24. p1(t)=a.^2*p(t-1)+Rww;     %p1为方程中p'

25. b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);

26. s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));

27. p(t)=p1(t)-c*b(t)*p1(t);

28. end

29. 
    

30. t=1:N;

31. plot(t,s,'r',t,Y,'g',t,x,'b');

更为详细的过程可参考有关的资料。

文章参考了：

1 博文http://hi.baidu.com/irvkqscjezbrtwq/item/4ad3bb018b8c7e37a3332a07

2 自动化所董秋雷上课课件

3 《学习Opencv》 于仕琪 P384 kalman滤波器部分

4 如果做视频跟踪具体参数选择可参考《数字视频处理》黎洪松 P102-106

5 如果想探索其具体推导过程可参考《现代信号处理》 张贤达 P177-188

详解卡尔曼滤波原理

在网上看了不少与卡尔曼滤波相关的博客、论文，要么是只谈理论、缺乏感性，或者有感性认识，缺乏理论推导。能兼顾二者的少之又少，直到我看到了国外的一篇博文，真的惊艳到我了，不得不佩服作者这种细致入微的精神，翻译过来跟大家分享一下，原文链接：http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
我不得不说说卡尔曼滤波，因为它能做到的事情简直让人惊叹！意外的是很少有软件工程师和科学家对对它有所了解，这让我感到沮丧，因为卡尔曼滤波是一个如此强大的工具，能够在不确定性中融合信息，与此同时，它提取精确信息的能力看起来不可思议。

什么是卡尔曼滤波？

你可以在任何含有 不确定信息 的动态系统中使用卡尔曼滤波，对系统下一步的走向做出 有根据的预测 ，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。
在 连续变化的系统 中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。
在Google上找到的大多数关于实现卡尔曼滤波的数学公式看起来有点晦涩难懂，这个状况有点糟糕。实际上，如果以正确的方式看待它，卡尔曼滤波是非常简单和容易理解的，下面我将用漂亮的图片和色彩清晰的阐述它，你只需要懂一些基本的概率和矩阵的知识就可以了。

我们能用卡尔曼滤波做什么？

用玩具举例：你开发了一个可以在树林里到处跑的小机器人，这个机器人需要知道它所在的确切位置才能导航。

我们可以说机器人有一个状态

，表示位置和速度：

注意这个状态只是关于这个系统基本属性的一堆数字，它可以是任何其它的东西。在这个例子中是位置和速度，它也可以是一个容器中液体的总量，汽车发动机的温度，用户手指在触摸板上的位置坐标，或者任何你需要跟踪的信号。
这个机器人带有GPS，精度大约为10米，还算不错，但是，它需要将自己的位置精确到10米以内。树林里有很多沟壑和悬崖，如果机器人走错了一步，就有可能掉下悬崖，所以只有GPS是不够的。

或许我们知道一些机器人如何运动的信息：例如，机器人知道发送给电机的指令，知道自己是否在朝一个方向移动并且没有人干预，在下一个状态，机器人很可能朝着相同的方向移动。当然，机器人对自己的运动是一无所知的：它可能受到风吹的影响，轮子方向偏了一点，或者遇到不平的地面而翻倒。所以，轮子转过的长度并不能精确表示机器人实际行走的距离，预测也不是很完美。
GPS 传感器 告诉了我们一些状态信息，我们的 预测 告诉了我们机器人会怎样运动，但都只是间接的，并且伴随着一些不确定和不准确性。但是，如果使用所有对我们可用的信息，我们能得到一个比任何依据自身估计更好的结果吗？回答当然是YES，这就是卡尔曼滤波的用处。

卡尔曼滤波是如何看到你的问题的

下面我们继续以只有位置和速度这两个状态的简单例子做解释。

我们并不知道实际的位置和速度，它们之间有很多种可能正确的组合，但其中一些的可能性要大于其它部分：

卡尔曼滤波假设两个变量（位置和速度，在这个例子中）都是随机的，并且 服从高斯分布 。每个变量都有一个 均值 μ，表示随机分布的中心（最可能的状态），以及 方差

，表示不确定性。

在上图中，位置和速度是 不相关 的，这意味着由其中一个变量的状态无法推测出另一个变量可能的值。下面的例子更有趣：位置和速度是 相关 的，观测特定位置的可能性取决于当前的速度：

这种情况是有可能发生的，例如，我们基于旧的位置来估计新位置。如果速度过高，我们可能已经移动很远了。如果缓慢移动，则距离不会很远。跟踪这种关系是非常重要的，因为它带给我们 更多的信息 ：其中一个测量值告诉了我们其它变量可能的值，这就是卡尔曼滤波的目的，尽可能地在包含不确定性的测量数据中提取更多信息！
这种相关性用 协方差矩阵 来表示，简而言之，矩阵中的每个元素

表示第 i 个和第 j 个状态变量之间的相关度。（你可能已经猜到协方差矩阵是一个 对称矩阵 ，这意味着可以任意交换 i 和 j）。协方差矩阵通常用“

”来表示，其中的元素则表示为“

”。

使用矩阵来描述问题

我们基于高斯分布来建立状态变量，所以在时刻 k 需要两个信息：最佳估计

（即均值，其它地方常用 μ 表示），以及协方差矩阵

。

(1)

（当然，在这里我们只用到了位置和速度，实际上这个状态可以包含多个变量，代表任何你想表示的信息）。接下来，我们需要根据 当前状态 （ k-1 时刻）来 预测 下一状态 （ k 时刻）。记住，我们并不知道对下一状态的所有预测中哪个是“真实”的，但我们的预测函数并不在乎。它对所有的可能性进行预测，并给出新的高斯分布。

我们可以用矩阵

来表示这个预测过程：

它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢？下面用一个基本的运动学公式来表示：

现在，我们有了一个预测矩阵来表示下一时刻的状态，但是，我们仍然不知道怎么更新协方差矩阵。此时，我们需要引入另一个公式，如果我们将分布中的每个点都乘以矩阵 A ，那么它的协方差矩阵

会怎样变化呢？很简单，下面给出公式：

结合方程（4）和（3）得到：

外部控制量

我们并没有捕捉到一切信息，可能存在外部因素会对系统进行控制，带来一些与系统自身状态没有相关性的改变。
以火车的运动状态模型为例，火车司机可能会操纵油门，让火车加速。相同地，在我们机器人这个例子中，导航软件可能会发出一个指令让轮子转向或者停止。如果知道这些额外的信息，我们可以用一个向量

来表示，将它加到我们的预测方程中做修正。
假设由于油门的设置或控制命令，我们知道了期望的加速度

，根据基本的运动学方程可以得到：

以矩阵的形式表示就是：

称为控制矩阵，

称为控制向量（对于没有外部控制的简单系统来说，这部分可以忽略）。让我们再思考一下，如果我们的预测并不是100%准确的，该怎么办呢？

外部干扰

如果这些状态量是基于系统自身的属性或者已知的外部控制作用来变化的，则不会出现什么问题。
但是，如果存在未知的干扰呢？例如，假设我们跟踪一个四旋翼飞行器，它可能会受到风的干扰，如果我们跟踪一个轮式机器人，轮子可能会打滑，或者路面上的小坡会让它减速。这样的话我们就不能继续对这些状态进行跟踪，如果没有把这些外部干扰考虑在内，我们的预测就会出现偏差。
在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”（即我们没有跟踪的干扰）之间的不确定性模型：

原始估计中的每个状态变量更新到新的状态后，仍然服从高斯分布。我们可以说

的每个状态变量移动到了一个新的服从高斯分布的区域，协方差为

。换句话说就是，我们将这些没有被跟踪的干扰当作协方差为

的 噪声 来处理。

这产生了具有不同协方差（但是具有相同的均值）的新的高斯分布。

我们通过简单地添加

得到扩展的协方差，下面给出预测步骤的完整表达式：

由上式可知， 新的最优估计 是根据 上一最优估计 预测 得到的，并加上 已知外部控制量 的 修正 。
而 新的不确定性 由 上一不确定 性 预测 得到，并加上 外部环境的干扰 。
好了，我们对系统可能的动向有了一个模糊的估计，用

和

来表示。如果再结合传感器的数据会怎样呢？

用测量值来修正估计值

我们可能会有多个传感器来测量系统当前的状态，哪个传感器具体测量的是哪个状态变量并不重要，也许一个是测量位置，一个是测量速度，每个传感器间接地告诉了我们一些状态信息。

注意，传感器读取的数据的单位和尺度有可能与我们要跟踪的状态的单位和尺度不一样，我们用矩阵

来表示传感器的数据。

我们可以计算出传感器读数的分布，用之前的表示方法如下式所示：

卡尔曼滤波的一大优点就是能处理传感器噪声，换句话说，我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。

从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。

我们将这种不确定性（例如：传感器噪声）用 协方差

表示，该分布的 均值 就是我们读取到的传感器数据，称之为

。
现在我们有了两个高斯分布，一个是在预测值附近，一个是在传感器读数附近。

我们必须在 预测值 （ 粉红色 ）和 传感器测量值 （ 绿色 ）之间找到最优解。
那么，我们最有可能的状态是什么呢？对于任何可能的读数

，有两种情况：（1）传感器的测量值；（2）由前一状态得到的预测值。如果我们想知道这两种情况都可能发生的概率，将这两个高斯分布相乘就可以了。

剩下的就是重叠部分了，这个重叠部分的均值就是两个估计最可能的值，也就是给定的所有信息中的 最优估计 。
瞧！这个重叠的区域看起来像另一个高斯分布。

如你所见，把两个具有不同均值和方差的高斯分布相乘，你会得到一个新的具有独立均值和方差的高斯分布！下面用公式讲解。

融合高斯分布

先以一维高斯分布来分析比较简单点，具有方差

和 μ 的高斯曲线可以用下式表示：

如果把两个服从高斯分布的函数相乘会得到什么呢？

将式（9）代入到式（10）中（注意重新归一化，使总概率为1）可以得到：

将式（11）中的两个式子相同的部分用 k 表示：

下面进一步将式（12）和（13）写成矩阵的形式，如果 Σ 表示高斯分布的协方差，

表示每个维度的均值，则：

矩阵

称为卡尔曼增益，下面将会用到。放松！我们快要完成了！

将所有公式整合起来

我们有两个高斯分布，预测部分

，和测量部分

，将它们放到式（15）中算出它们之间的重叠部分：

由式（14）可得卡尔曼增益为：

将式（16）和式（17）的两边同时左乘矩阵的逆（注意

里面包含了

）将其约掉，再将式（16）的第二个等式两边同时右乘矩阵

的逆得到以下等式：

上式给出了完整的 更新步骤 方程。

就是新的最优估计，我们可以将它和

放到下一个 预测 和 更新 方程中不断迭代。

总结

以上所有公式中，你只需要用到式（7）、（18）、（19）。（如果忘了的话，你可以根据式（4）和（15）重新推导一下）
我们可以用这些公式对任何线性系统建立精确的模型，对于非线性系统来说，我们使用 扩展卡尔曼滤波 ，区别在于EKF多了一个把预测和测量部分进行线性化的过程。

（ps: 第一次用Markdown，添加图片和公式心累啊，什么时候能直接拖拽就好了~~）

附Markdown使用技巧：
1. 改变文本字体、字号与颜色。参考链接：(http://blog.csdn.net/testcs_dn/article/details/45719357/)
2. 在线公式编辑器，编辑好了右键“复制图片地址”，当作图片来添加。
链接：(http://www.codecogs.com/latex/eqneditor.php)
3. 段落首行缩进，按Shift+Space将输入法切换到全角状态，然后敲空格即可，一个空格代表一个汉字的间隔。
4. 设置图片大小及居中显示。参考链接：(http://blog.csdn.net/soindy/article/details/50427079)
5. 不懂百度。

https://blog.csdn.net/u010720661/article/details/63253509