实用文档

低通滤波：又叫一阶惯性滤波，或一阶低通滤波。是使用软件编程实现普通
硬件RC低通滤波器的功能。
适用范围：单个信号，有高频干扰信号。
一阶低通滤波的算法公式为：
Y(n)X(n)(1)Y(n1)

式中：是滤波系数；X(n)是本次采样值；Y(n1)是上次滤波输出值；
Y(n)

是本次滤波输出值。
滤波效果1：

红色线是滤波前数据（matlab中生成的正弦波加高斯白噪声信号）
黄色线是滤波后结果。
滤波效果2：
实用文档

matlab中函数，相当于一阶滤波，蓝色是原始数据（GPS采集到的x（北）
方向数据，单位m），红色是滤波结果。

一阶滤波算法的不足：
一阶滤波无法完美地兼顾灵敏度和平稳度。有时，我们只能寻找一个平衡，
在可接受的灵敏度范围内取得尽可能好的平稳度。
实用文档

互补滤波：适用于两种传感器进行融合的场合。必须是一种传感器高频特性
好（动态响应好但有累积误差，比如陀螺仪。），另一传感器低频特性好（动态
响应差但是没有累积误差，比如加速度计）。他们在频域上互补，所以进行互补
滤波融合可以提高测量精度和系统动态性能。
应用：陀螺仪数据和加速度计数据的融合。
互补滤波的算法公式为：

1122
Y(n)X(n)(X(n)Y(n1))

式中：1和2是滤波系数；1X(n)和2X(n)是本次采样值；Y(n1)是上次滤
波输出值；Y(n)是本次滤波输出值。
滤波效果
（测试数据）：
蓝色是陀螺仪
信号，红色是加
速度计信号，黄
色是滤波后的
角度。
实用文档

互补滤波实际效果：
实用文档

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

(k)(k)(k)ZHXv
(k)X
是系统k时刻的状态，(k)U是系统k时刻的控制量。(k)Z是系统k时
刻的测量值。A和B为系统参数，(k)w和(k)v分别表示过程和测量的噪声，H是
测量系统参数。
在进行卡尔曼滤波时：
首先进行先验预测：
(k1|k)(k|k)(k)(k)XAXBUw

计算先验预测方差：
'
(k1|k)(k|k)(k)PAPAQ

计算增益矩阵：
(k1)(k1|k)'/((k1|k)'(k1))KgPHHPHR

后验估计值：
(k1|k1)(k1|k)(k1)(Z(k1)(k1|k))XXKgHX

后验预测方差：
(k1|k1)(1(k1))(k1|k)PKgHP

其中，(k)Q是系统过程激励噪声协方差，(k)R是测量噪声协方差。

举例说明：
（下文中加粗的是专有名词，需要理解）
预测小车的位置和速度的例子（博客+自己理解）:
实用文档

小车上面安装了GPS传感器，这个传感器的精度是10米。但是如果小车行
驶的荒野上面有河流和悬崖的话，10米的范围就太大，很容易掉进去进而无法
继续工作。所以，单纯靠GPS的定位是无法满足需求的。另外，如果有人说小车
本身接收操控着发送的运动指令，根据车轮所转动过的圈数时能够知道它走了多
远，但是方向未知，并且在路上小车打滑车轮空转的现象绝对是不可避免。所以，
GPS以及车轮上面电机的码盘等传感器是间接地为我们提供了小车的信息，这些
信息包含了很多的和不确定性。如果将所有这些信息综合起来，就能够通过计算
得到我们想要的准确信息
小车的状态向量可以写为：

{,}'kxpv
。协方差pppvvpvvP，一般初始化为1001

第一步：根据k-1时刻的状态来预测k时刻的状态，有
11kkkpptv

（上次位置+上次速度估计*t=当前位置先验估计）

1kkvv

（认为速度不变）

写成矩阵形式：

1101kktxx
（先验状态估计向量），令101tA




表示系统

固有特性参数（这个名词是我生造的，便于理解）矩阵。
T
kk
PAPA

(先验状态估计协方差矩阵)（有数学推导过程，是和概率论

和矩阵协方差相关的内容，还在研究中）
第二步：确定外部控制影响

比如在k-1时刻给小车发送了刹车指令，假设刹车加速度为a（0），
2
1112kkkpptvat


（第一步结果+刹车导致的位移）

1kkvvat

（上次速度+本次控制加速度*t=当前速度先验估计）

结合第一步，写成矩阵形式，
实用文档

212kktxAxat，令2
2
t

Bt





表示控制矩阵，令kua表

示控制向量。
第三步：确定外部不确定性影响。

假设不确定干扰项为kw，kw成高斯分布，协方差为kQ，称为过程激励
噪声协方差，那么结合前两步，得到

1kkkkxAxBuw

（先验状态估计向量）

T
kkk
PAPAQ

。(先验状态估计协方差矩阵)

前三步属于先验估计，进行一个简单总结：
从前三步看出：先验估计1kkkkxAxBuw，分为三个部分，一
是上一次的最优估计，二是这一次的控制量，三是这一次的不确定性影响，假定

不确定性影响成高斯分布。先验估计协方差矩阵1TkkkPAPAQ，根据
前一次的后验估计的协方差进行递推，再加上这次先验估计的不确定对系统造成
的影响。

第四步，确定测量系统参数H。
kk
zHx
。kz是只考虑测量单位换算后的测量结果。

传感器的输出值不一定与状态向量中的元素对等，需要进行换算。例如测量
到的气压换算成高度。但在本例中，传感器直接测量到了小车的位移和速度，所

以不需要进行换算，这时1001H，是单位矩阵。H的维数取决于状态
向量元素个数和观测量的个数，不一定是方阵。如果在本例中只测量了速度，那
么01H。同理，只测量位移，那么10H，这时，如果测量
的位移单位是mm，而状态向量中位移单位是m，那么0.0010H
第五步，确定测量系统噪声(k)v和其测量噪声协方差kR。
实用文档

传感器测量会有一定误差，假定误差量符合高斯分布。观测噪声向量用
(k)v
表示，其测量噪声协方差用kR表示。那么测量值kz可以表示为：

(k)kkzHxv
，kz是考虑测量单位和测量误差之后的测量结果。

（关于协方差的定义及数学意义，可以查阅
http://www.visiondummy.com/2014/04/geometric-interpretation-covarianc
e-matrix/）（结合概率论，用图形表示，是介绍比较好的一个版本）

第四到五步是传感器的测量结果，进行一个简单的总结
测量值(k)kkzHxv，分为两个部分，第一部分是考虑单位换算得
到的测量值kHx，第二部分是引入的测量误差(k)v，该测量误差(k)v成高
斯分布，其协方差是kR（测量噪声协方差）。

第六步：卡尔曼估计。
确定卡尔曼增益：K（描述“更相信由前一状态和控制量得到的估计还是
更相信当前测量值”的变量，是一个矩阵）
'/(')KPHHPHR
（有数学推导过程，和概率论相关的内容，，

还在研究中）。
后验估计（最终估计结果）：
'
()kkkkxxKzHx

（后验状态估计向量）

'
(1)kkPKHP

（后验状态估计协方差矩阵）。

至此，一个循环结束，下次的先验估计在当前后验估计的基础上继续进行。