卡尔曼滤波器原理-优快云博客

本文深入浅出地介绍了卡尔曼滤波器的基本概念、历史背景及其核心算法。通过具体的例子，逐步解析卡尔曼滤波器如何融合预测与测量信息，以实现最优估计。

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

卡尔曼全名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

简单来说，卡尔曼滤波器是一个“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）来描述：
X(k)=A X(k-1)+B U(k)+W(k)
再加上系统的测量值：
Z(k)=H X(k)+V(k)
上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出（类似上一节那个温度的例子）。

首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。

到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表示covariance：
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)：
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5 个基本公式。根据这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滤波程序：

clear
N=200;
w(1)=0;
w=randn(1,N)
x(1)=0;
a=1;
for k=2:N;
x(k)=a*x(k-1)+w(k-1);
end

V=randn(1,N);
q1=std(V);
Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2;
q3=std(w);
Rww=q3.^2;
c=0.2;
Y=c*x+V;

p(1)=0;
s(1)=0;
for t=2:N;
p1(t)=a.^2*p(t-1)+Rww;
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));
p(t)=p1(t)-c*b(t)*p1(t);
end

t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');

=========================================================================================================

Kalman滤波器的历史渊源

We are like dwarfs on the shoulders of giants, by whose grace we see farther than they. Our study of the works of the ancients enables us to give fresh life to their finer ideas, and rescue them from time’s oblivion and man’s neglect.

—— Peter of Blois, late twelfth century

太喜欢第一句话了，“我们是巨人肩膀上的矮人，巨人们的优雅让我么看得更比他们更远”，谁说不是呢？

说起Kalman滤波器的历史，最早要追溯到17世纪，Roger Cotes开始研究最小均方问题。但由于缺少实际案例的支撑（那个时候哪来那么多雷达啊啥的这些信号啊），Cotes的研究让人看着显得很模糊，因此在估计理论的发展中影响很小。17世纪中叶，最小均方估计（Least squares Estimation）理论逐步完善，Tobias Mayer在1750年将其用于月球运动的估计，Leonard Euler在1749年、Pierre Laplace在1787分别用于木星和土星的运动估计。Roger Boscovich在1755用最小均方估计地球的大小。1777年，77岁的Daniel Bernoulli（大名鼎鼎的伯努利）发明了最大似然估计算法。递归的最小均方估计理论是由Karl Gauss建立在1809年（好吧，他声称在1795年就完成了），当时还有Adrien Legendre在1805年完成了这项工作，Robert Adrain在1808年完成的，至于到底谁是Boss，矮子们就别管了吧！

在1880年，丹麦的天文学家Thorvald Nicolai Thiele在之前最小均方估计的基础上开发了一个递归算法，与Kalman滤波非常相似。在某些标量的情况下，Thiele的滤波器与Kalman滤波器时等价的，Thiele提出了估计过程噪声和测量噪声中方差的方法（过程噪声和测量噪声是Kalman滤波器中关键的概念）。

上面提到的这么多研究估计理论的先驱，大多是天文学家而非数学家。现在，大部分的理论贡献都源自于实际的工程。“There is nothing so practical as a good theory”，应该就是“实践是检验真理的唯一标准”之类吧。

现在，我们的控制论大Wiener终于出场了，还有那个叫Kolmogorov（柯尔莫戈洛夫）的神人。在19世纪40年代，Wiener设计了Wiener滤波器，然而，Wiener滤波器不是在状态空间进行的（这个学过Wiener滤波的就知道，它是直接从观测空间z(n)=s(n)+w(n)进行的滤波），Wiener是稳态过程，它假设测量是通过过去无限多个值估计得到的。Wiener滤波器比Kalman滤波器具有更高的自然统计特性。这些也限制其只是更接近理想的模型，要直接用于实际工程中需要足够的先验知识（要预知协方差矩阵），美国NASA曾花费多年的时间研究维纳理论，但依然没有在空间导航中看到维纳理论的实际应用。

在1950末期，大部分工作开始对维纳滤波器中协方差的先验知识通过状态空间模型进行描述。通过状态空间表述后的算法就和今天看到的Kalman滤波已经极其相似了。Johns Hopkins大学首先将这个算法用在了导弹跟踪中，那时在RAND公司工作的Peter Swerling将它用在了卫星轨道估计，Swerling实际上已经推导出了（1959年发表的）无噪声系统动力学的Kalman滤波器，在他的应用中，他还考虑了使用非线性系统动力学和和测量方程。可以这样说，Swerling和发明Kalman滤波器是失之交臂，一线之隔。在kalman滤波器闻名于世之后，他还写信到AIAA Journal声讨要获得Kalman滤波器发明的荣誉（然而这时已经给滤波器命名Kalman了）。总结其失之交臂的原因，主要是Swerling没有直接在论文中提出Kalman滤波器的理论，而只是在实践中应用。

Rudolph Kalman在1960年发现了离散时间系统的Kalman滤波器，这就是我们在今天各种教材上都能看到的，1961年Kalman和Bucy又推导了连续时间的Kalman滤波器。Ruslan Stratonovich也在1960年也从最大似然估计的角度推导出了Kalman滤波器方程。

目前，卡尔曼滤波已经有很多不同的实现。卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。

从牛顿到卡尔曼

从现在开始，就要进行Kalman滤波器探讨之旅了，我们先回到高一，从物理中小车的匀加速直线运动开始。

话说，有一辆质量为m的小车，受恒定的力F，沿着r方向做匀加速直线运动。已知小车在t-ΔT时刻的位移是s(t-1)，此时的速度为v(t-1)。求：t时刻的位移是s(t)，速度为v(t)？

由牛顿第二定律，求得加速度： $a=F/m$

那么就有下面的位移和速度关系：

$s(t)=s(t-1)+v(t-1)\Delta{T}+\frac{1}{2}\frac{F}{m}{(\Delta{T})}^2$

$v(t)=v(t-1)+\frac{F}{m}\Delta{T}$

如果将上面的表达式用矩阵写在一起，就变成下面这样：

$\begin{pmatrix} s(t) \\ v(t) \end{pmatrix}={\begin{pmatrix} 1 & \Delta{T} \\ 0 & 1 \end{pmatrix}} {\begin{pmatrix} s(t-1) \\ v(t-1) \end{pmatrix}} + {\begin{pmatrix} \frac{(\Delta{T})^2}{2} \\ {\Delta{T}} \end{pmatrix}} {\frac{F}{m}}................................(1)$

卡尔曼滤波器是建立在动态过程之上，由于物理量（位移，速度）的不可突变特性，这样就可以通过t-1时刻估计（预测）t时刻的状态，其状态空间模型为：

$\mathbf{x(t)=Ax(t-1)+Bu(t)+w(t)}................................(2)$

对比一下(1)(2)式，长得及其相似有木有：

$A={\begin{pmatrix} 1 & \Delta{T} \\ 0 & 1 \end{pmatrix}}, B={\begin{pmatrix} \frac{(\Delta{T})^2}{2} \\ {\Delta{T}} \end{pmatrix}}$

匀加速直线运动过程就是卡尔曼滤波中状态空间模型的一个典型应用。下面我们重点关注(2)式，鉴于研究的计算机信号都是离散的，将(2)是表示成离散形式为：

$\mathbf{x(n)=Ax(n-1)+Bu(n)+w(n)}................................(3)$

其中各个量之间的含义是：

x(n)是状态向量，包含了观测的目标（如：位移、速度）
u(n)是驱动输入向量，如上面的运动过程是通过受力驱动产生加速度，所以u(n)和受力有关
A是状态转移矩阵，其隐含指示了“n-1时刻的状态会影响到n时刻的状态（这似乎和马尔可夫过程有些类似）”
B是控制输入矩阵，其隐含指示了“n时刻给的驱动如何影响n时刻的状态”

从运动的角度，很容易理解：小车当前n时刻的位移和速度一部分来自于n-1时刻的惯性作用，这通过Ax(n)来度量，另一部分来自于现在n时刻小车新增加的外部受力，通过Bu(n)来度量。
w(n)是过程噪声，w(n)~N(0,Q)的高斯分布，过程噪声是使用卡尔曼滤波器时一个重要的量，后面会进行分析。

计算n时刻的位移，还有一种方法：拿一把长的卷尺（嗯，如果小车跑了很长时间，估计这把卷尺就难买到了），从起点一拉，直接就出来了，设测量值为z(n)。计算速度呢？速度传感器往那一用就出来了。

然而，初中物理就告诉我们，“尺子是量不准的，物体的物理真实值无法获得”，测量存在误差，我们暂且将这个误差记为v(n)。这种通过直接测量的方式获得所需物理量的值构成观测空间：

$\mathbf{z(n)=H(n)x(n)+v(n)} ................................(4)$

z(n)就是测量结果，H(n)是观测矢量，x(n)就是要求的物理量（位移、速度），v(n)~N(0,R)为测量噪声，同状态空间方程中的过程噪声一样，这也是一个后面要讨论的量。大部分情况下，如果物理量能直接通过传感器测量， $\mathbf{H(n)=I}$ 。

现在就有了两种方法（如上图）可以得到n时刻的位移和速度：一种就是通过(3)式的状态空间递推计算（Prediction），另一种就是通过(4)式直接拿尺子和传感器测量（Measurement）。致命的是没一个是精确无误的，就像上图看到的一样，分别都存在0均值高斯分布的误差w(n)和v(n)。

那么，我最终的结果是取尺子量出来的好呢，还是根据我们伟大的牛顿第二定律推导出来的好呢？抑或两者都不是！

一场递推的游戏

为充分利用测量值（Measurement）和预测值（Prediction），Kalman滤波并不是简单的取其中一个作为输出，也不是求平均。

设预测过程噪声w(n)~N(0,Q)，测量噪声v(n)~N(0,R)。Kalman计算输出分为预测过程和修正过程如下：

预测

预测值：

$\mathbf{\hat{x}(n|n-1)=A\hat{x}(n-1|n-1)+Bu(n)}................................(5)$

最小均方误差矩阵：

$\mathbf{P(n|n-1)=AP(n-1|n-1)A^T+Q}................................(6)$
修正

误差增益：

$\mathbf{K(n)=P(n|n-1)H^T(n)[R(n)+H(n)P(n|n-1)H^T(n)]^{-1}}.............(7)$

修正值：

$\mathbf{\hat{x}(n|n)=A\hat{x}(n|n-1)+K(n)[{z(n)-H(n)\hat{x}(n|n-1)}]}......................(8)$ 这里的A去掉，参见下面的图

最小均方误差矩阵：

$\mathbf{P(n|n)=[I-K(n)H(n)]P(n|n-1)}................................(9)$

从(5)~(9)中：

x(n)：Nx1的状态矢量
z(n)：Mx1的观测矢量，Kalman滤波器的输入
x(n|n-1)：用n时刻以前的数据进行对n时刻的估计结果
x(n|n)：用n时刻及n时刻以前的数据对n时刻的估计结果，这也是Kalman滤波器的输出
P(n|n-1)：NxN，最小预测均方误差矩阵，其定义式为

$\mathbf{P(n|n-1)=E\{[x(n)-\hat{x}(n|n-1)][x(n)-\hat{x}(n|n-1)]^T\}}$

通过计算最终得到(6)式。
P(n|n)：NxN，修正后最小均方误差矩阵。
K(n)：NxM，误差增益，从增益的表达式看，相当于“预测最小均方误差”除以“n时刻的测量误差+预测最小均方误差”，直观含义就是用n-1预测n时刻状态的预测最小均方误差在n时刻的总误差中的比重，比重越大，说明真值接近预测值的概率越小（接近测量值的概率越大），这也可以从(8)式中看到。

Kalman滤波算法的步骤是(5)(6)->(7)->(8)(9)。当然，建议找本教材复习下上面公式的推导过程，或参见Wiki上的介绍http://en.wikipedia.org/wiki/Kalman_filter。

公式就是那么的抽象，一旦认真研究懂了却也是茅塞顿开，受益也比只知皮毛的多。尽管如此，我还算更喜欢先感性后理性。仍以上面的运动的例子来直观分析：

Example:

还可以更简单一些：设小车做匀速（而非匀加速）直线运动，方便计算，假设速度绝对的恒定（不波动，所以相关的方差都为0），则u(t)==0恒成立。设预测（过程）位移噪声w(n)~N(0,2^2)，测量位移噪声v(n)~N(0,1^2)，n-1状态的位移 $\Small \hat{x}(n-1|n-1)=50m}$ ，速度为v=10m/s，n时刻与n-1时刻的物理时差为ΔT=1s。同时，也用尺子测了一下，结果位移为z(n)=62m。

则A = [1 ΔT; 0 1]=[1 1; 0 1]，根据(5)，预测值为

$\Small \hat{x}(n|n-1)=1*\hat{x}(n-1|n-1)+v*\Delta{T}=[60m; 10m/s]}$ 。

现在已经有了估计值和测量值，哪个更接近真值，这就通过最小均方误差矩阵来决定！

要求已知上次的修正后的最小均方误差P(n-1|n-1)=[1 0; 0 0]（匀速，所以P(2,2)=0，右斜对角线上为协方差，值为0，P(1,1)为n-1时刻位移量的均方误差，因为要计算P(1,1)还得先递推往前计算P(n-2|n-2)，所以这里暂时假设为1），则根据(6)式，最小预测预测均方误差为P(n|n-1)=[1 0; 0 0][1 1; 0 1][1 0; 0 0]=[1 0; 0 0]。

由物理量的关系知，H(n)=[1 1]，增益K(n)=[1;0]{1+[1 1][1 0; 0 0][1; 1]}^(-1)=[1/2;0]。

所以，最后的n时刻估计值既不是用n-1得到的估计值，也不是测量值，而是： $\Small \hat{x}(n|n)=[60;1]+[1/2;0]{62-[1 1][60;1]}=[60.5m; 1m/s]$ ，因此，最终的Kalman滤波器的输出位移是60.5m。

从上面的递推关系知道，要估计n时刻就必须知道n-1时刻，那么n=0时刻该如何估计，因此，卡尔曼滤波要初始化的估计值x(-1|-1)和误差矩阵P(-1|-1)，设x(-1,-1)~N(Us, Cs)，则初始化：

$\mathbf{x(-1|-1)=\mu_s}$

$\mathbf{P(-1|-1)=C_s}$

综上，借用一张图说明一下Kalman滤波算法的流程：

图中的符号和本文符号稍有差异，主要是P的表示上。从上图也可以看出，Kalman滤波就是给定-1时刻的初始值，然后在预测（状态空间）和修正（观测空间）之间不停的递推，求取n时刻的估计x和均方误差矩阵P。

均方误差中的门道

到这里，应该对Kalman滤波有个总体的概念了，有几个观点很重要，是建立Kalman滤波器的基础：

一个是n-1对n时刻估计值，一个是n时刻的测量值，估计值和测量值都存在误差，且误差都假设满足独立的高斯分布
Kalman滤波器就是充分结合了估计值和测量值得到n时刻更接近真值的估计结果
Kalman滤波器引入状态空间的目的是避免了“像Wiener滤波器一样需要对过去所有[0,n-1]时刻协方差先验知识都已知”，而直接可以通过上一时刻即n-1时刻的状态信息和均方误差信息就可递推得到n时刻的估计。尽管递推使得实际应用中方便了，但n-1对n时刻的估计实际上使用到了所有前[0,n-1]时刻的信息，只不过信息一直通过最小均方误差进行传递到n-1时刻。基于此，Kalman滤波也需要先验知识，即-1时刻的初始值。

在上小节中只看到Kalman的结论，那么Kalman滤波器是如何将估计值和测量值结合起来，如何将信息传递下去的呢？这其中，“独立高斯分布”的假设条件功劳不可谓不大！测量值z(n)~N(uz,σz^2)，估计值x(n)~N(ux,σx^2)。

Kalman滤波器巧妙的用“独立高斯分布的乘积”将这两个测量值和估计值进行融合！

如下图：估计量的高斯分布和测量量的高斯分布经过融合后为绿色的高斯分布曲线。

$y_f=\frac{1}{\sqrt{2\pi{\sigma^2_z}}}\exp^{-\frac{(r-uz)^2}{2\sigma^2_z}}*\frac{1}{\sqrt{2\pi{\sigma^2_x}}}\exp^{-\frac{(r-ux)^2}{2\sigma^2_x}}=\frac{1}{\sqrt{2\pi{\sigma^2}}}\exp^{-\frac{(r-\mu)^2}{2\sigma^2}}$

稍微计算一下，通过上式求出u和σ^2，

$\mu=\frac{u_x\sigma^2_z+u_z\sigma^2_x}{\sigma^2_z+\sigma^2_x}=u_x+\frac{\sigma^2_x}{\sigma^2_z+\sigma^2_x}(u_z-u_x)........(10)$ $\sigma^2=\frac{\sigma^2_x\sigma^2_z}{\sigma^2_x+\sigma^2_z}=\sigma^2_x(1-\frac{\sigma^2_x}{\sigma^2_x+\sigma^2_z})........(11)$

现在令

$K=\frac{\sigma^2_x}{\sigma^2_x+\sigma^2_z}........(12)$

则(10)(11)变成：

$\mu==u_x+K(u_z-u_x)........(13)$ $\sigma^2==\sigma^2_x(1-K)........(14)$

到这里，请将(13)-(14)与(8)-(9)式对比！标量的情况下，在小车的应用中有：A=1，H=1，正态分布的均值u就是我们要的输出结果，正态分布的方差σz^2就是最小均方误差。推广到矢量的情况，最小均方误差矩阵就是多维正态分布的协方差矩阵。

从(12)式也很容易看到卡尔曼增益K的含义：就是估计量的方差占总方差（包括估计方差和测量方差）的比重。

一切都变得晴朗起来了，然而这一切的一切，却都源自于“估计量和测量量的独立高斯分布”这条假设。进一步总结Kalman滤波器：

假设状态空间的n-1时刻估计值和观测空间的n时刻测量值都满足独立高斯分布，Kalman滤波器就是通过高斯分布的乘积运算将估计值和测量值结合，获得最接近真值的n时刻估计。

高斯分布乘积运算的结果仍为高斯分布，高斯分布的均值对应n时刻的估计值，高斯分布的方差对应n时刻的均方误差。

Matlab程序看过来

下面的一段Matlab代码是从网上找到的，程序简单直接，但作为学习分析用很棒，

% KALMANF - updates a system state vector estimate based upon an
%           observation, using a discrete Kalman filter.
%
% Version 1.0, June 30, 2004
%
% This tutorial function was written by Michael C. Kleder
%
% INTRODUCTION
%
% Many people have heard of Kalman filtering, but regard the topic
% as mysterious. While it's true that deriving the Kalman filter and
% proving mathematically that it is "optimal" under a variety of
% circumstances can be rather intense, applying the filter to
% a basic linear system is actually very easy. This Matlab file is
% intended to demonstrate that.
%
% An excellent paper on Kalman filtering at the introductory level,
% without detailing the mathematical underpinnings, is:
% "An Introduction to the Kalman Filter"
% Greg Welch and Gary Bishop, University of North Carolina
% http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
%
% PURPOSE:
%
% The purpose of each iteration of a Kalman filter is to update
% the estimate of the state vector of a system (and the covariance
% of that vector) based upon the information in a new observation.
% The version of the Kalman filter in this function assumes that
% observations occur at fixed discrete time intervals. Also, this
% function assumes a linear system, meaning that the time evolution
% of the state vector can be calculated by means of a state transition
% matrix.
%
% USAGE:
%
% s = kalmanf(s)
%
% "s" is a "system" struct containing various fields used as input
% and output. The state estimate "x" and its covariance "P" are
% updated by the function. The other fields describe the mechanics
% of the system and are left unchanged. A calling routine may change
% these other fields as needed if state dynamics are time-dependent;
% otherwise, they should be left alone after initial values are set.
% The exceptions are the observation vectro "z" and the input control
% (or forcing function) "u." If there is an input function, then
% "u" should be set to some nonzero value by the calling routine.
%
% SYSTEM DYNAMICS:
%
% The system evolves according to the following difference equations,
% where quantities are further defined below:
%
% x = Ax + Bu + w  meaning the state vector x evolves during one time
%                  step by premultiplying by the "state transition
%                  matrix" A. There is optionally (if nonzero) an input
%                  vector u which affects the state linearly, and this
%                  linear effect on the state is represented by
%                  premultiplying by the "input matrix" B. There is also
%                  gaussian process noise w.
% z = Hx + v       meaning the observation vector z is a linear function
%                  of the state vector, and this linear relationship is
%                  represented by premultiplication by "observation
%                  matrix" H. There is also gaussian measurement
%                  noise v.
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
%       v ~ N(0,R) meaning v is gaussian noise with covariance R
%
% VECTOR VARIABLES:
%
% s.x = state vector estimate. In the input struct, this is the
%       "a priori" state estimate (prior to the addition of the
%       information from the new observation). In the output struct,
%       this is the "a posteriori" state estimate (after the new
%       measurement information is included).
% s.z = observation vector
% s.u = input control vector, optional (defaults to zero).
%
% MATRIX VARIABLES:
%
% s.A = state transition matrix (defaults to identity).
% s.P = covariance of the state vector estimate. In the input struct,
%       this is "a priori," and in the output it is "a posteriori."
%       (required unless autoinitializing as described below).
% s.B = input matrix, optional (defaults to zero).
% s.Q = process noise covariance (defaults to zero).
% s.R = measurement noise covariance (required).
% s.H = observation matrix (defaults to identity).
%
% NORMAL OPERATION:
%
% (1) define all state definition fields: A,B,H,Q,R
% (2) define intial state estimate: x,P
% (3) obtain observation and control vectors: z,u
% (4) call the filter to obtain updated state estimate: x,P
% (5) return to step (3) and repeat
%
% INITIALIZATION:
%
% If an initial state estimate is unavailable, it can be obtained
% from the first observation as follows, provided that there are the
% same number of observable variables as state variables. This "auto-
% intitialization" is done automatically if s.x is absent or NaN.
%
% x = inv(H)*z
% P = inv(H)*R*inv(H')
%
% This is mathematically equivalent to setting the initial state estimate
% covariance to infinity.

function s = kalmanf(s)

% set defaults for absent fields:
if ~isfield(s,'x'); s.x=nan*z; end
if ~isfield(s,'P'); s.P=nan; end
if ~isfield(s,'z'); error('Observation vector missing'); end
if ~isfield(s,'u'); s.u=0; end
if ~isfield(s,'A'); s.A=eye(length(x)); end
if ~isfield(s,'B'); s.B=0; end
if ~isfield(s,'Q'); s.Q=zeros(length(x)); end
if ~isfield(s,'R'); error('Observation covariance missing'); end
if ~isfield(s,'H'); s.H=eye(length(x)); end

if isnan(s.x)
   % initialize state estimate from first observation
   if diff(size(s.H))
      error('Observation matrix must be square and invertible for state autointialization.');
   end
   s.x = inv(s.H)*s.z;
   s.P = inv(s.H)*s.R*inv(s.H'); 
else

   % This is the code which implements the discrete Kalman filter:

   % Prediction for state vector and covariance:
   s.x = s.A*s.x + s.B*s.u;
   s.P = s.A * s.P * s.A' + s.Q;

   % Compute Kalman gain factor:
   K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);

   % Correction based on observation:
   s.x = s.x + K*(s.z-s.H*s.x);
   s.P = s.P - K*s.H*s.P;

   % Note that the desired result, which is an improved estimate
   % of the sytem state vector x and its covariance P, was obtained
   % in only five lines of code, once the system was defined. (That's
   % how simple the discrete Kalman filter is to use.) Later,
   % we'll discuss how to deal with nonlinear systems.

end

return

该程序中使用的符号的含义与本文一致，函数前的注释再清晰不过了，就不多说。下面是一段测试代码：

% Define the system as a constant of 12 volts:
clear all
s.x = 12;
s.A = 1;
% Define a process noise (stdev) of 2 volts as the car operates:
s.Q = 2^2; % variance, hence stdev^2
% Define the voltimeter to measure the voltage itself:
s.H = 1;
% Define a measurement error (stdev) of 2 volts:
s.R = 2^2; % variance, hence stdev^2
% Do not define any system input (control) functions:
s.B = 0;
s.u = 0;
% Do not specify an initial state:
s.x = nan;
s.P = nan;
% Generate random voltages and watch the filter operate.
tru=[]; % truth voltage
for t=1:20
   tru(end+1) = randn*2+12;
   s(end).z = tru(end) + randn*2; % create a measurement
   s(end+1)=kalmanf(s(end)); % perform a Kalman filter iteration
end
figure
hold on
grid on
% plot measurement data:
hz=plot([s(1:end-1).z],'r.');
% plot a-posteriori state estimates:
hk=plot([s(2:end).x],'b-');
ht=plot(tru,'g-');
legend([hz hk ht],'observations','Kalman output','true voltage',0)
title('Automobile Voltimeter Example')
hold off

Kalman的参数中s.Q和s.R的设置非常重要，之前也提过，一般要通过实验统计得到，它们分布代表了状态空间估计的误差和测量的误差。

Kalman滤波器的效果是使输出变得更平滑，但没办法去除信号中原有的椒盐噪声，而且，Kalman滤波器也会跟踪这些椒盐噪声点，因此推荐在使用Kalman滤波器前先使用中值滤波去除椒盐噪声。

Kalman滤波C程序

我就在上面公式的基础上实现了基本的Kalman滤波器，包括1维和2维状态的情况。先在头文件中声明1维和2维Kalman滤波器结构：

/*
 * FileName : kalman_filter.h
 * Author   : xiahouzuoxin @163.com
 * Version  : v1.0
 * Date     : 2014/9/24 20:37:01
 * Brief    : 
 * 
 * Copyright (C) MICL,USTB
 */
#ifndef  _KALMAN_FILTER_H
#define  _KALMAN_FILTER_H

/* 
 * NOTES: n Dimension means the state is n dimension, 
 * measurement always 1 dimension 
 */

/* 1 Dimension */
typedef struct {
    float x;  /* state */
    float A;  /* x(n)=A*x(n-1)+u(n),u(n)~N(0,q) */
    float H;  /* z(n)=H*x(n)+w(n),w(n)~N(0,r)   */
    float q;  /* process(predict) noise convariance */
    float r;  /* measure noise convariance */
    float p;  /* estimated error convariance */
    float gain;
} kalman1_state;

/* 2 Dimension */
typedef struct {
    float x[2];     /* state: [0]-angle [1]-diffrence of angle, 2x1 */
    float A[2][2];  /* X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2 */
    float H[2];     /* Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2   */
    float q[2];     /* process(predict) noise convariance,2x1 [q0,0; 0,q1] */
    float r;        /* measure noise convariance */
    float p[2][2];  /* estimated error convariance,2x2 [p0 p1; p2 p3] */
    float gain[2];  /* 2x1 */
} kalman2_state;                   

extern void kalman1_init(kalman1_state *state, float init_x, float init_p);
extern float kalman1_filter(kalman1_state *state, float z_measure);
extern void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2]);
extern float kalman2_filter(kalman2_state *state, float z_measure);

#endif  /*_KALMAN_FILTER_H*/

我都给了有详细的注释，kalman1_state是状态空间为1维/测量空间1维的Kalman滤波器，kalman2_state是状态空间为2维/测量空间1维的Kalman滤波器。两个结构体都需要通过初始化函数初始化相关参数、状态值和均方差值。

/*
 * FileName : kalman_filter.c
 * Author   : xiahouzuoxin @163.com
 * Version  : v1.0
 * Date     : 2014/9/24 20:36:51
 * Brief    : 
 * 
 * Copyright (C) MICL,USTB
 */

#include "kalman_filter.h"

/*
 * @brief   
 *   Init fields of structure @kalman1_state.
 *   I make some defaults in this init function:
 *     A = 1;
 *     H = 1; 
 *   and @q,@r are valued after prior tests.
 *
 *   NOTES: Please change A,H,q,r according to your application.
 *
 * @inputs  
 *   state - Klaman filter structure
 *   init_x - initial x state value   
 *   init_p - initial estimated error convariance
 * @outputs 
 * @retval  
 */
void kalman1_init(kalman1_state *state, float init_x, float init_p)
{
    state->x = init_x;
    state->p = init_p;
    state->A = 1;
    state->H = 1;
    state->q = 2e2;//10e-6;  /* predict noise convariance */
    state->r = 5e2;//10e-5;  /* measure error convariance */
}

/*
 * @brief   
 *   1 Dimension Kalman filter
 * @inputs  
 *   state - Klaman filter structure
 *   z_measure - Measure value
 * @outputs 
 * @retval  
 *   Estimated result
 */
float kalman1_filter(kalman1_state *state, float z_measure)
{
    /* Predict */
    state->x = state->A * state->x;
    state->p = state->A * state->A * state->p + state->q;  /* p(n|n-1)=A^2*p(n-1|n-1)+q */

    /* Measurement */
    state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);
    state->x = state->x + state->gain * (z_measure - state->H * state->x);
    state->p = (1 - state->gain * state->H) * state->p;

    return state->x;
}

/*
 * @brief   
 *   Init fields of structure @kalman1_state.
 *   I make some defaults in this init function:
 *     A = {{1, 0.1}, {0, 1}};
 *     H = {1,0}; 
 *   and @q,@r are valued after prior tests. 
 *
 *   NOTES: Please change A,H,q,r according to your application.
 *
 * @inputs  
 * @outputs 
 * @retval  
 */
void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2])
{
    state->x[0]    = init_x[0];
    state->x[1]    = init_x[1];
    state->p[0][0] = init_p[0][0];
    state->p[0][1] = init_p[0][1];
    state->p[1][0] = init_p[1][0];
    state->p[1][1] = init_p[1][1];
    //state->A       = {{1, 0.1}, {0, 1}};
    state->A[0][0] = 1;
    state->A[0][1] = 0.1;
    state->A[1][0] = 0;
    state->A[1][1] = 1;
    //state->H       = {1,0};
    state->H[0]    = 1;
    state->H[1]    = 0;
    //state->q       = {{10e-6,0}, {0,10e-6}};  /* measure noise convariance */
    state->q[0]    = 10e-7;
    state->q[1]    = 10e-7;
    state->r       = 10e-7;  /* estimated error convariance */
}

/*
 * @brief   
 *   2 Dimension kalman filter
 * @inputs  
 *   state - Klaman filter structure
 *   z_measure - Measure value
 * @outputs 
 *   state->x[0] - Updated state value, Such as angle,velocity
 *   state->x[1] - Updated state value, Such as diffrence angle, acceleration
 *   state->p    - Updated estimated error convatiance matrix
 * @retval  
 *   Return value is equals to state->x[0], so maybe angle or velocity.
 */
float kalman2_filter(kalman2_state *state, float z_measure)
{
    float temp0 = 0.0f;
    float temp1 = 0.0f;
    float temp = 0.0f;

    /* Step1: Predict */
    state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1];
    state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];
    /* p(n|n-1)=A^2*p(n-1|n-1)+q */
    state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0];
    state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1];
    state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0];
    state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1];

    /* Step2: Measurement */
    /* gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. */
    temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1];
    temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1];
    temp  = state->r + state->H[0] * temp0 + state->H[1] * temp1;
    state->gain[0] = temp0 / temp;
    state->gain[1] = temp1 / temp;
    /* x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/
    temp = state->H[0] * state->x[0] + state->H[1] * state->x[1];
    state->x[0] = state->x[0] + state->gain[0] * (z_measure - temp); 
    state->x[1] = state->x[1] + state->gain[1] * (z_measure - temp);

    /* Update @p: p(n|n) = [I - gain * H] * p(n|n-1) */
    state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0];
    state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];
    state->p[1][0] = (1 - state->gain[1] * state->H[0]) * state->p[1][0];
    state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];

    return state->x[0];
}

其实，Kalman滤波器由于其递推特性，实现起来很简单。但调参有很多可研究的地方，主要需要设定的参数如下：

init_x：待测量的初始值，如有中值一般设成中值（如陀螺仪）
init_p：后验状态估计值误差的方差的初始值
q：预测（过程）噪声方差
r：测量（观测）噪声方差。以陀螺仪为例，测试方法是：保持陀螺仪不动，统计一段时间内的陀螺仪输出数据。数据会近似正态分布，按3σ原则，取正态分布的(3σ)^2作为r的初始化值。

其中q和r参数尤为重要，一般得通过实验测试得到。

找两组声阵列测向的角度数据，对上面的C程序进行测试。一维Kalman（一维也是标量的情况，就我所知，现在网上看到的代码大都是使用标量的情况）和二维Kalman（一个状态是角度值，另一个状态是向量角度差，也就是角速度）的结果都在图中显示。这里再稍微提醒一下：状态量不要取那些能突变的量，如加速度，这点在文章“从牛顿到卡尔曼”一小节就提到过。

Matlab绘出的跟踪结果显示：

Kalman滤波结果比原信号更平滑。但是有椒盐突变噪声的地方，Kalman滤波器并不能滤除椒盐噪声的影响，也会跟踪椒盐噪声点。因此，推荐在Kalman滤波器之前先使用中值滤波算法去除椒盐突变点的影响。

上面所有C程序的源代码及测试程序都公布在我的Github上，希望大家批评指正其中可能存在的错误。

参考资料

Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation. Ramsey Faragher, Lecture Notes.
An Introduction to the Kalman Filter. Greg Welch and Gary Bishop.
http://robotsforroboticists.com/kalman-filtering/公式彩色着色，含pyhton源码
http://alumni.media.mit.edu/~wad/mas864/psrc/kalman.c.txt包含Kalman滤波的C代码
http://www.cs.unc.edu/~welch/kalman/比较全的Kalman链接

===================================================================================================================

过程方程：

X(k+1) = A X(k) + B U(k) + W(k) >>>>式1

量测方程：

Z(k+1) = H X（k+1）+ V(k+1) >>>>式2

A和B是系统参数，对于多模型系统，他们为矩阵；H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声，他们的协方差分别是Q，R。为了不失一般性，下面的讨论中将X,Z都视为矩阵，其中X是m行的单列矩阵，Z是n行的单列矩阵。

说明：下面的表达式中，不带前缀的量都代表实际量，其小括号里面的“k”或“k+1”代表该量是第k或第k+1时刻的实际量，如“Z(k+1)”就代表第k+1时刻的实际测量值；

带前缀“^”的量都代表预测量，如果小括号里面是“k+1|k”，就代表k+1时刻的先验预测值，如果小括号里面是“k+1|k+1”，就代表k+1时刻的后验预测值；（测量值可以通过测量得到，所以只有先验预测，没有后验预测。而实际状态值无法得知，既有先验预测，又有后验预测）

带前缀“~”的量都代表与预测值对应的偏差值。

实际状态值与先验预测状态值的偏差 = 实际状态值 – 先验预测状态值

~X(k+1|k) = X(k+1) - ^X(k+1|k) >>>> 式3

实际测量值与先验预测测量值的偏差 = 当前测量值 - 先验预测测量值

~Z(k+1|k) = Z(k+1) - ^Z(k+1|k) >>>>式4

并且

先验预测测量值 = 转换矩阵H * 先验预测状态值

^Z(k+1|k) = H ^X(k+1|k) >>>> 式5

得到测量值后，再对当前状态值X(k+1) 进行后验预测（设后验预测值为 ^Z(k+1|k+1) ），则后验预测值（同时也是最终预测值）的偏差为

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1) >>>>式6

为了得到当前状态值X(k+1)，根据式3，需要：

X(k+1) = ^X(k+1|k) + ~X(k+1|k) >>>> 式7

上式中，我们可以通过卡尔曼公式1（见附注2）计算出^X(k+1|k)，但我们无法得知实际状态值X(k+1)，因而~X(k+1|k) 也无法得知。我们最终的目的是得出一个比较接近实际状态值X(k+1)的滤波值^X(k+1|k+1)，根据式7，只要能准确的估计出~X(k+1|k)即可。

~X(k+1|k)本身虽无法得知，但~Z(k+1|k) 却可以通过测量得到，而且它们二者存在一定的相关性。不妨再设存在一个矩阵K（m行n列矩阵），能使得

~X(k+1|k) = K * ~Z(k+1|k) >>>>式8

那么最终的预测任务其实就是找到K。由于~X(k+1|k)和~Z(k+1|k)都是单列矩阵，因此不难看出，满足式8的矩阵K应有无穷多个。矩阵K中第i行第j列反映了量测变量偏差矩阵~Z(k+1|k)的第j个元素对状态变量偏差矩阵~X(k+1|k)的第i个元素的贡献。因此矩阵K的物理意义很明显，K的第i行第j列的元素表示：对于第i个待测的状态量来说，第j个测量仪器测到的偏差的可信度。某个测量值对应的可信度越高，滤波器越“相信”该测量值。

既然满足条件的K有无穷多个，那应该使用哪个K呢？实际上，我们并不知道~X(k+1|k)的值，所以也就无法直接计算出K，而只能通过某种方法找到一个Kg，使得将Kg带入式8后，等号两边的差（的平方）的期望尽可能小。

我们最终的预测值或滤波值是后验预测值^X(k+1|k+1)，因此最后的预测也应使 ~X(k+1|k+1) 的期望为0且方差最小（这与让8式两端的差最小是一致的，下面的式9体现了这一点），这样预测值才最可靠。下面详细说明。

^X(k+1|k+1) = ^X(k+1|k) + Kg * ~Z(k+1|k) （后验预测的状态值）

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1) （后验预测的偏差）

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1)

= ( ^X(k+1|k) + ~X(k+1|k) ) - ( ^X(k+1|k) + Kg * ~Z(k+1|k) )

= ~X(k+1|k) - Kg * ~Z(k+1|k)

>>>>式9

~Z(k+1|k) = Z(k+1) - ^Z(k+1|k)

= ( H X（k+1）+ V(k+1) ) - ( H ^X(k+1|k) )

= H ( X（k+1）-^X(k+1|k) ) + V(k+1)

= H ~ X(k+1|k) + V(k+1) >>>>式10

接下来的分析中，为了更直观的说明卡尔曼滤波的原理，我们用几何方法来解释。这时，~X和~Z矩阵中的每个元素应看做向量空间中的一个向量而不再是一个单纯的数。这个向量空间（统计测试空间）可以看成无穷多维的，每一个维对应一个可能的状态。~X和~Z矩阵中的每个元素向量都是由所有可能的状态按照各自出现的概率组合而成（在测量之前，~X和~Z 的实际值都是不可知的）。~X和~Z中的每个元素向量都应是0均值的，他们与自己的内积就是他们的协方差矩阵。我们无法给出~X和~Z中每个元素向量的具体表达，但我们通过协方差矩阵就可以知道所有元素向量的模长，以及相互之间的夹角（从内积计算）。

为了方便用几何方法解释，我们假设状态变量X是一个1行1列的矩阵（即只有一个待测状态量），而量测变量Z是一个2行1列的矩阵（即有两个测量仪器，共同测量同一个状态量X），也就是说，m=1，n=2。矩阵X中只有X[1]一项，矩阵Z中有Z[1]和Z[2]两项。Kg此时应是一个1行2列的矩阵，两个元素分别记作Kg1 和 Kg2 。H和V此时应是一个2行1列的矩阵。

将矩阵表达式9和10按元素展开：

~X(k+1|k+1)[1] = ~X(k+1|k)[1] - (Kg1 * ~Z(k+1|k)[1] + Kg2 * ~Z(k+1|k)[2] ) >>>> 式9i

~Z(k+1|k)[i] = H[i] ~ X(k+1|k) + V(k+1)[i] >>>>式10i

~X(k+1|k) 中各个元素（向量）的线性组合可以产生一个m维或更低维的向量子空间Vx，这里，按照我们的假设，m=1，所以Vx应是一维的；同时V(k+1)中的各个元素（向量）的线性组合也可以产生一个n维或更低维的向量子空间Vv，这里，按照我们的假设，n=2，所以Vv应是二维的。由于V(k+1)中的每一项与~X(k+1|k)中的每一项都不相关（见附注1），故这两个子空间相互垂直。如下图所示。式10i所体现的~Z(k+1|k)[i]、H[i]~ X(k+1|k)、V(k+1)[i] 三者之间的几何关系，也在下图中描绘了出来。

从上图中可以看出，~Z(k+1|k)中各个元素（向量）的线性组合也可以产生一个n维或更低维的向量子空间Vz，这里已假设n=2，所以Vz是一个二维的平面，就是上图中两条红色的线所构成的平面。

通过先验预测的协方差矩阵（见卡尔曼公式2），可以得到~X(k+1|k)中各个元素的模长以及彼此间的夹角。这是因为协方差矩阵中的第i行第j列其实就代表了~X(k+1|k)中第i个元素向量与第j个元素向量的内积。

通过测量可以得到新息协方差（见卡尔曼公式3的分母部分），进而可以知道~Z(k+1|k)中各个元素的模长以及彼此间的夹角。

通过已知的量测噪声协方差矩阵R，可以得出V(k+1) 中各个元素的模长以及彼此间的夹角。

最后根据~X(k+1|k+1)[1]与Y垂直以及图1中所示的几何关系，用高中学的立体几何和向量知识就可以求得两个Kg的值了。如果将向量的内积都用协方差矩阵表示，就会发现，我们最后求得的Kg，其实就是卡尔曼公式3。

（上面讨论的是较低次的卡尔曼滤波，只有一个待测量，两个测量仪器。这种情况还是比较常见的，比如倾角测量系统中，我们用加速度计和陀螺仪共同测量倾角。对于更高次的卡尔曼滤波，X和Z都是多行矩阵时，用几何方法已经无法直观解释，只能用矩阵分析的方法证明。求解Kg的详细过程参考《卡尔曼滤波器及其应用基础》国防工业出版社敬喜编）

卡尔曼滤波的核心过程，就是求解能使得

E { ~X(k+1|k+1) ’ * ~X(k+1|k+1) }

取最小值的Kg增益矩阵的过程，~X(k+1|k+1)’代表的是~X(k+1|k+1)的转置（这里~X(k+1|k+1)中的元素代表数值，不是向量）。前面已经提到过，卡尔曼增益矩阵Kg中的元素，都代表测量仪器测到的偏差的可信度，或者叫估计权重。

附注1：

(a). v(k+1)中的每一项与~X(k+1|k)中的每一项都不相关

~X(k+1|k) = X(k+1) - ^X(k+1|k)

= X(k+1) - ( A ^X(k|k) + B U(k) )

= ( A X(k) + B U(k) + W(k) ) - ( A X(k) + B U(k) - A ~X(k|k) )

= W(k) + A ~X(k|k)

= W(k) + A ( ~X(k|k-1) - Kg(k)* ~ Z(k|k-1) ) -----这一步利用了式9

= W(k) + A ( ~X(k|k-1) - Kg(k)* ( H ~X(k|k-1) + v(k) ) ) ------这一步利用了式10

= W(k) - A Kg(k) *v(k) + A ( I - Kg(k) * H ) ~X(k|k-1)

上式最后一行出现了~X(k|k-1)，可见~X(k+1|k)可以递归表示。而且递归式中的过程噪声W(k)与v(k+1)不相关，同时由于 v本身是白噪声，所以 v(k+1)与v(k)亦不相关（白噪声的自相关是δ函数），因此通过递推式可以判断v(k+1)与~X(k+1|k)不相关。

(b). w(k+1)中的每一项与~X(k+1|k+1)中的每一项都不相关，w(k+1)中的每一项与~X(k+1|k)中的每一项都不相关。

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1)

= ( ^X(k+1|k) + ~X(k+1|k) ) - ( ^X(k+1|k) + Kg(k+1)*~Z(k+1|k) )

= ~X(k+1|k) - Kg(k+1)*~Z(k+1|k)

= ~X(k+1|k) - Kg(k+1)* ( H ~X(k+1|k) + v(k+1) )

= - Kg(k+1)* v(k+1) + ( I - Kg(k+1) * H ) ~X(k+1|k)

我们已经知道w(k+1)与v(k+1)不相关，因此只要~X(k+1|k+1)与上式的第二项也不相关，就说明结论(b)成立。根据(a)中的结论，~X(k+1|k)的递归展开式中出现的 v(k) ，w(k) ， v(k-1)，w(k-1)等等，显然 w(k+1)与 v (m=k，k-1……) 都不相关，另外，由于w(k+1)的自相关为δ函数，因此w(k+1)与 w(m=k，k-1……) 也不相关，也就得出w(k+1)与~X(k+1|k) 不相关。

进而可知，w(k+1)与~X(k+1|k+1)不相关。

正是因为 (a) (b)中的两个不相关，卡尔曼公式中的预测协方差矩（卡尔曼公式(2)）和新息协方差矩阵（卡尔曼公式(3)中的“分母”部分）才可以是简单的加式。

附注2：卡尔曼滤波的五个公式

先验预测值与先验预测协方差矩阵的计算。求解协方差时，都认为预测值的期望是实际值。因此，^X(k+1|k)的协方差矩阵同样也是 ~X(k+1|k) 的协方差矩阵，又因为偏差~X(k+1|k)的期望是0，因此协方差矩阵反映了~X(k+1|k)在向量空间中的模长。注意，协方差矩阵都是对称矩阵。

X(k+1|k)=A X(k|k)+B U(k) (1)

P(k+1|k)=A P(k|k) A’+Q(k) (2)

卡尔曼增益矩阵的计算。量测预测值为

Z(k+1|k) = H X(k+1|k)

新息协方差见公式（3）中的“分母”部分。量测预测值的期望是实际量测值。因此，^Z(k+1|k)的协方差矩阵同样也是 ~Z(k+1|k) 的协方差矩阵，又因为偏差~Z(k+1|k)的期望是0，因此协方差矩阵反映了~Z(k+1|k)在向量空间中的模长。

Kg(k+1)= P(k+1|k) H’/ (H P(k+1|k) H’ + R(k+1)) (3)

后验预测值与后验预测协方差矩阵的计算

X(k+1|k+1)= X(k+1|k)+Kg(k+1) (Z(k+1)-H X(k+1|k)) (4)