加权求和公式_从加权求和的角度来看kalman滤波

一直想把自己工作学习中的感悟写出来与大家分享，但是每写到细节处，完美主义的思维模式时常让我止步不前。某一天，我突然想，还是先让自己放松一些，重点是分享出来。这之后，我终于可以写下去了。表述中有不严谨甚至不正确的地方，愿大家不吝赐教，给予指正。我总觉得，这个世界上最强大的网络，应该是人脑之间的联网，思想之间的共享。希望与大家在思维碰撞中，共同成长！

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－

网上已经有很多关于卡尔曼滤波的很好的讲解。在这里与大家分享一下我自己对卡尔曼滤波的学习及使用体会。

这里我们从最佳加权求和的角度来理解一下卡尔曼滤波。

首先介绍一下“逆方差加权”(https://baike.lichenjy.com/nifangchajq/3385.shtml)：

设X1与X2是对信号X的两次独立测量，假设两次测量均服从正态分布，测量方差分别为

和

，那么，对X的最佳估计为：

(1.1)

这里

与

为加权系数，计算如下:

由权重的计算，我们可以看出，每次测量的权重与它的测量方差成反比，也就是测量的越准，那么它的权重越大。同时，所有权重的和要为1，所以对每个权系数要进行归一化，也就是除以

。

下面我们来看一下卡尔曼滤波。在使用卡尔曼滤波之前，首先要建立两个方程：

(1.2)

(1.3)

公式(1.2)描述的是信号自身的运动规律（状态转移方程），该方程表明了信号的可预测性,

可以理解为预测噪声，该值越大，表明预测越不准确，反之，该值越小，则表明预测越可靠。推至极端，如果该值为0，则表明这是个确定性信号，完全可以准确预测，此时，也就不再需要卡尔曼滤波了，因为我们已经可以得到信号的准确值。

公式(1.3)描述了对信号的量测方法，也就是量测模型（量测方程），

是量测噪声，该值越大，则表明量测越不准确。反之，该值越小，则表明量测越可靠。同理，推至极端，如果该值为0，则证明可以准确的测得当前信号，那也就不需要进行滤波估计了，因为我们已经可以测得准确的信号值。

如上所述，正因为该信号可以以一定精度被预测，同时，也可以以一定精度被量测，而预测与量测都不完全准确，所以，我们可以将两者进行加权融合，以得到对信号的更高精度的估计。

那么，如何对两者进行加权融合呢？

我们可以结合前述的逆方差加权原理，对预测信号和量测信号按照逆方差加权进行加权求和。

要进行加权求和，首先要知道每一种测量得到的期望和方差。我们可以将预测过程，也看做是一种量测，只不过是根据上一次的测量，以一种预测的方式，来得到对当前信号的测量。测量的期望值与方差可以由状态转移方程来得到。

假设我们已经得到了k-1时刻对信号的最佳估计

，及其估计的协方差矩阵

。现在我们要由

去预测

，根据系统的状态转移方程（其实也就是预测方程），我们可以得到对k时刻信号的预测期望值：

(1.4)

而预测的噪声包含两部分，一部分来自于

的噪声，另一部分来自于k-1到k时刻新产生的过程噪声

。

由

的噪声导致的对k时刻X预测值的噪声的协方差矩阵为：

(1.5)

在一维的情况下，我们可以很容易推得(1.4)和(1.5)两个结果，对于高维情况，公式包含了矩阵运算的特点。

现在，我们再把k-1到k时刻新产生的过程噪声

也考虑进来，由其导致的对k时刻的X值

的预测噪声的协方差矩阵为：

(1.6)

这里

为噪声

的协方差矩阵，G为噪声驱动矩阵。

总的预测噪声

为这两部分的和：

(1.7)

至此，我们得到了对信号预测的期望

以及预测噪声的协方差矩阵

（在一维的情况下，协方差矩阵就是方差）。对于量测，我们可以直接使用本次测量到的值作为量测期望

，而量测的噪声一般都可以从测量设备本身的说明手册中查得，也可以自己通过对传感器进行测试获得。 也就是说，量测方程(1.3)中的

的统计特性是可以事先获得的，进而也就可以得到量测的协方差矩阵

。

现在我们有了对同一个信息的预测值、预测方差以及量测值、量测方差，而且这两种噪声又都是不相关的，那么我们可以利用这两路信息通过加权求和得到对该信息的最优估计。（不相关意味着，两路信息是独立的，无法通过A测量的信息获取B测量信息。试想，如果两路信息完全相关，则意味着，知道了A的测量值，也就等效于知道了B的测量值，那么此时，A与B是等价的，我们并没有获得额外的信息，那就谈不上融合了）。

接下来，我们要计算预测值与量测值在融合中的权重。如果是一维的情况，那很简单，我们直接套用文章开头介绍的逆方差加权公式就可以了。可是，在实践中，基本上都是多维的情况，加权系数可以通过协方差矩阵计算出来。计算权重的思路大致是：假设对预测值的加权系数为K1，那么对量测的加权系数就是(E-K1)，E为单位矩阵，根据预测及量测的协方差矩阵，我们可以计算出加权求和后的结果的协方差矩阵P(K1)，P(K1)是一个以K1为自变量的函数，求取P(K1)取最小值时的K1的值，就是我们要计算的权重了，而求极值，无非就是固定的套路，对自变量求导，然后让导数等于0，进而得到函数取得极值时自变量的值。严谨而全面的推导过程大家可以参考《卡尔曼滤波与组合导航原理》或《STATE ESTIMATION FOR ROBOTICS》的推导过程。

现在，我们重新梳理一下kalman滤波一次完整的运算过程，然后我们从结果上来分析一下卡尔曼滤波运行的内在逻辑。

(step 1)状态预测：

(1.8)

(step 2)计算预测的协方差矩阵:(描述预测的不准确程度)：

(1.9)

这里

是上一次滤波的结果的协方差矩阵，

是本次滤波周期内新产生的噪声，一般是输入值，可从传感器手册查得。以imu这种传感器为例，

对应的就是陀螺和加速度计的噪声所对应的协方差矩阵。

(step 3)由预测方差及量测方差计算滤波增益：

(1.10)

这里

为量测方程(1.3)中的量测噪声

的协方差矩阵，描述了量测的不准确程度。以GPS这种测量设备为例，

就是由GPS给出的位置、速度的测量方差构成的矩阵，这些参数一般可以在GPS手册上查得。

(step 4)计算状态估计：

(1.11)

(step 5)计算本次估计的协方差矩阵：(描述本次估计的质量)

(1.12)

前两步我们在之前介绍了，下面我们主要分析一下后三步。从公式(1.11)看，最终的状态估计值是由

与

两部分组成，前者是由预测方程得到的对当前状态的预测值，后者我们可以看成是由量测值计算出的对预测值的修正。 后者括号中的

为预测的量测值，而

为量测到的量测值，这两者相减即得到了对Z的量测与预测的差别，乘上增益矩阵

，即从量测与预测的差别中提取出对预测的校正量。不过这里我们也许会疑问：

得出的差值是与量测量Z向对应的，怎么乘以

后就变成对状态量

的修正量了呢？为解决这个疑问，我们还有必要再分析一下

。为了阅读方便，我们在此重写公式（1.10）

为预测值

的协方差矩阵，按照协方差矩阵定义

(1.13)

这里

为列向量。

(1.14)

由上式来看，

在乘上

后，变成了

与

的协方差矩阵。我们以GPS/IMU组合导航应用为例子来分析（分析kalman滤波，我们就以此应用为背景吧）。在组合导航应用中，常采用误差作为待估计的状态变量。这时，协方差矩阵的元素其实就是其所对应的状态变量之间的互相关（关于协方差，互相关，如果不是很熟悉的同学，建议在网上查一下，这些概念对理解统计滤波，很重要），“互相关”，顾名思义，它定量的描述了两个变量之间的变化关系。所以，

描述了X与Z之间的变化关系，乘上它，可以将对Z的修正量变成了对X的修正量。同时，

也描述了预测的不确定程度，

越大，证明预测越不准确，由（1.10）此时

会变大，表明量测的修正作用变大，也就是更相信量测。然后我们再来看（1.10）中的

部分，其中

为量测的协方差矩阵，描述了量测的不确定性程度，

越大，则

越小，也就是越不相信量测。

如果我们也把逆方差求和公式写成

对

的修正形式，再对比kalman滤波的公式，也许能帮助我们更容易的理解逆方差加权求和在kalman滤波过程中的体现。

我们将逆方差公式也写成

对

校正的形式如下：

(1.15)

同时，我们也将kalman滤波公式中的（1.10）代入（1.11），得到：

(1.16)

对比（1.15）与（1.16），

就相当于

，

就相当于

，只不过

与

的互相关为1，或者说是单位矩阵。

对于kalman滤波，我觉得还有一点值得我们讨论一下，就是kalman滤波中的协方差矩阵P。在我看来，kalman滤波中最重要的就是这个协方差矩阵了。整个滤波过程主要就是不断更新这个协方差矩阵。作为协方差矩阵，它不但描述了它所对应的状态变量的精度，更重要的，它还描述了状态变量之间的关系，也就是给出了各个状态变量之间的互相关。我们还以组合导航为例来了解一下协方差矩阵的作用。在典型的GPS/INS组合导航中，我们的观测量一般是GPS给出的位置、速度。可想而知，这时候，INS（惯性导航系统）的位置、速度误差能够得到直接的观测，进而保证了INS的位置、速度不会随时间而发散。不过，除此之外，我们会发现，俯仰角(pitch)和横滚角(roll)误差也都能得到很好的校正（这里假设我们的状态变量只有位置误差、速度误差、姿态误差，暂不考虑传感器零偏）。我们的直接观测量是位置误差和速度误差，那么俯仰角和横滚角的误差是怎么估计出来的呢？这主要就是协方差矩阵的作用了，因为它描述了各个状态变量之间的关系，而俯仰角和横滚角的误差是和位置、速度误差相关的，所以通过协方差矩阵我们可以由速度、位置误差得到俯仰角、横滚角的误差。我们也可以定性的来分析一下，假设俯仰和横滚出现了误差，那么必然导致算法计算的重力加速度在水平面上的投影出现误差，进而导致对载体加速度的计算误差，通过积分，导致速度误差，进一步积分，导致位置误差。所以，可见，俯仰角和横滚角的误差最终会反映到速度、位置误差中。反过来，通过对位置、速度误差的直接观测，也可以推知俯仰角和横滚角的误差。这里我们没有说到偏航角(yaw)的误差，因为对偏航角误差的观测还需要另外的条件，那就是需要载体在水平面内做有加速度的运动。如果载体在水平面内是静止或者匀速直线运动，那么加速度计在水平面内则没有测量输出，那么偏航角误差也就不会通过加速度投影反应到位置、速度误差当中，也就是说，此时的偏航角误差与位置、速度误差并不相关，所以，此时无法通过位置、速度误差推知偏航角的误差。

我们也从公式中来看一下协方差矩阵的作用：结合公式(1.16)和(1.14)来看，(1.16)中第二项当中的

，由(1.14)的推导我们可知

就是量测量与状态量之间的互相关，如果其中某项的互相关系数为0，那么由(1.16)看，也就不会差生修正量叠加到

上面。

另外，值得一提的是，协方差矩阵的逆又叫信息矩阵。如果在滤波过程中，我们一直维护的不是协方差矩阵，而是它的逆：信息矩阵，那么这个过程又叫做信息滤波。信息矩阵也有它的物理意义，在slam常用的图论方法当中，它描述了图中各个定点之间连接关系，扮演了邻接矩阵的角色。也就是说，如果其中的某一项元素不为0，那么表明该元素所对应的两个状态变量之间存在约束，也就是在图中，这两个顶点之间存在一条边。同时，信息矩阵也是非线性优化当中的Hessian矩阵，在非线性优化中扮演着重要角色。