传感器滤波算法

1. 什么是滤波算法

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

最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Kолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。

现设线性时变系统的离散状态防城和观测方程为：

X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)

Y(k) = H(k)·X(k)+N(k)

其中

X(k)和Y(k)分别是k时刻的状态矢量和观测矢量

F(k,k-1)为状态转移矩阵

U(k)为k时刻动态噪声

T(k,k-1)为系统控制矩阵

H(k)为k时刻观测矩阵

N(k)为k时刻观测噪声

则卡尔曼滤波的算法流程为：

预估计X(k)^= F(k,k-1)·X(k-1)

计算预估计协方差矩阵
C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'
Q(k) = U(k)×U(k)'

计算卡尔曼增益矩阵
K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)
R(k) = N(k)×N(k)'

更新估计
X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^]

计算更新后估计协防差矩阵
C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)'

X(k+1) = X(k)~
C(k+1) = C(k)~

2. LMS自适应滤波算法中要求的期望输出和滤波器的输入之间有什么区别，采集的实际信号往往是无法知道期望输出

自适应滤波算法有几种应用类型，不同类型的目的、原理和手段不同，所以相对应的选取输入和期望信号也很不一样。
1.系统辨识：当我们想描述一个未知系统（如一组复杂的模拟电路），解析的算出系统的冲击响应或者系统函数是比较困难的。这时，我们就可以用未知系统的输入和输出训练自适应滤波器（未知系统的输入作为自适应滤波器的输入，未知系统的输出作为自适应滤波器的期望信号，当自适应滤波器收敛后，对应的滤波器就可以看做是未知系统的近似）。

3. 卡尔曼滤波理解与实现

本文为离散卡尔曼滤波算法的一 一个简明教程，从算法思想、实现过程、理论推导和程序实现四个方面阐述和分析了卡尔曼滤波算法。

XU Ruilin完成本教程主要部分的编写，WANG Xuejun完成第3节的编写，ZHU Ximin完成2.2节的编写，WEN Shuhan完成2.3节的编写，MAO Bo完成全文整理、修订和排版。

卡尔曼滤波（Kalman Filtering）及其一系列的优化和改进算法是目前在求解运动状态推算问题上最为普遍和高效的方法。 鲁道夫·卡尔曼 （Rudolf Emil Kalman） 在NASA埃姆斯研究中心访问时，发现他的方法适用于解决阿波罗计划的轨迹预测问题。阿波罗飞船的导航电脑就是使用这种滤波器进行轨迹预测。

卡尔曼滤波尤其适用于动态系统，这种方法对于内存要求极低而运算速度快，且能够保持较好的计算精度，这使得这种方法非常适合解决实时问题和应用于嵌入式系统，也就是说，卡尔曼滤波天然的适用于解决舰艇指控系统的航迹推算问题。在接下来的内容里，我们将逐步领会卡尔曼滤波的这些绝佳特点。

不过，现在我们先从复杂的舰艇航迹推算问题中解脱出来，从一个更加熟悉和简单的问题中来理解这个滤波算法的思想、过程和算法。

假设有一辆无人车WALL-E，需要导引它从A点到达B点，共有两种手段（ 图1 ）：

显然，两种方法都有一定的误差。如果单独采用某一种方法进行定位，WALL-E在误差的影响下将无法到达B点。因此，需要将两种方法结合起来，得到一个更加精确的结果，这就是卡尔曼滤波要解决的问题。

卡尔曼滤波方法如何看待我们的问题呢？在探究这个问题之前，我们先对问题进行抽象，并用数学语言来描述我们的问题。

我们用矢量 来描述WALL-E的运动状态，这个列矢量 包括位置矢量 和速度矢量 两个分量。在WALL-E的问题上，我们现在不知道位置 和速度 的准确值，但是知道WALL-E的运动模型满足 状态方程 ，定位的方法，也即观测WALL-E运动状态的方法满足 观测方程 . 当然，我们也知道，这两种方法都存在一定的误差 ，那么我们的问题就可以转化为一个优化问题——

在这一优化问题中，目标函数是要使预测（估计）误差最小，同时约束于估计方法 和 的条件下。在卡尔曼滤波中，我们的估计原则（也就是最小化估计误差的原则）是 最小方差无偏估计 [1] ，我们将通过后面的过程分析来说明这一点。

在我们正式开始引入公式分析卡尔曼滤波问题之前，我们还必须解决一个问题------把连续的线性系统离散化，也就是将连续时域问题转化为时间序列问题。当然，目前我们只讨论线性系统的情况，关于非线性系统问题，我们有扩展卡尔曼滤波（Extended Kalman Filtering, EKF）和无迹卡尔曼滤波（Unscented Kalman Filtering, UKF）两种方法来求解。

补充内容------连续线性时变系统的离散化
设连续线性时变系统的时域状态方程为

若采样周期为 ，则从时刻 到时刻 ，有

令 ， ，则离散化的状态方程为

通过对线性系统的离散化处理，我们现在可以考虑每一个时刻WALL-E的运动状态。接下来，我们将用 来表示在 时刻运动状态的最优估计值；用 表示用 时刻对 时刻的状态预测值；用 表示对 时刻综合预测和观测两种方法的最优估计值。

在估计WALL-E位置的问题上，假定我们已经知道它是匀速直线运动，WALL-E身上还携带有一个GPS传感器可以提供它的位置信息，WALL-E在前进过程中可能会遇到一些情况，比如停止前进或是受到风的影响。

加入我们已知的是WALL-E上一个时刻的最佳估计状态，即k-1时刻的位置和速度，要求的是下一时刻即k时刻的最佳估计状态，即k时刻的位置和速度，我们可以发现有两种方法可以得到它的k时刻的状态：

一种是通过WALL-E设定程序计算得到下一秒的状态，比如现在设定是匀速直线运动，那么下一秒的速度应该是恒定不变的，而位置则是在上一秒位置的基础上加上时间乘以速度即一秒内走过的路程，但是现实生活中并不是理想的，机器人会受到摩擦力、风力等的影响，当然也可能会有顽皮的小孩挡住他前进的道路，这些因素使得WALL-E在k时的真实状态与我们计算得到的数据有所不同。

另一种是通过WALL-E所携带的GPS来确定它的位置，因为GPS是测量出的就是WALL-E的实时状态，因此它比较准确。但是GPS测量k时刻的状态有两个问题，一是GPS只能测出WALL-E的位置，而测不出它的速度；二是GPS传感器测量的时候也会有仪器的误差，只能说它是比较准确的，比较接近真实值的。

那么接下来问题来了，我们如何得到k时刻WALL-E的真实状态呢？

我们将第一种方法得到的状态值称为预测值，第二种方法得到的状态值称为测量值，对汽车的最佳估计就是将这两部分信息结合起来，尽量的去逼近k时刻的真实值。

下面再深入一些思考，怎么将这两部分结合起来？

在初始时间k-1， 是WALL-E的最佳估计值，WALL-E其实可以是估计值附近的任何位置，并且这种不确定性由该概率密度函数描述。WALL-E最有可能在这个分布的平均值附近。在下一个时间，估计的不确定性增加，用一个更大的方差表示，这是因为在时间步骤k-1和k之间，WALL-E可能收到了风力的影响，或者脚可能向前滑了一点，因此，它可能已经行进了与模型预测的距离不同的距离。

WALL-E位置的另一个信息来源来自测量，方差表示误差测量的不确定性，真正的位置同样可以是平均值附近的任何位置。

预测值和测量值，对WALL-E的最佳估计是将这两部分信息结合起来，将两个概率函数相乘得到另一个高斯函数，该估计值的方差小于先前估计值，并且该概率密度函数的平均值为我们提供了WALL-E位置的最佳估计。

以下，我们将进行e的运算推导

设：

则有实际目标变量的表达式：

数学模型中目标变量的表达式：

实际模型中测量变量的表达式：

数学模型中测量变量的表达式：

将目标变量的实际值和估计值相减：

将上述方程带入误差e的表达式，我们可得出误差e的解析解：

从推导结果中我们不难看出，估计值和实际值的误差随时间呈指数形式变化，当（F-KH）<1时，随着时间的推移，会无限趋近于零，也就是意味着估计值和实际值相吻合。这就是为什么卡尔曼滤波器可以完美预测出目标状态值的原理。

在估计WALL-E位置的问题上，我们不知道位置 和速度 的准确值，但是我们可以给出一个估计区间（ 图5.a ）。卡尔曼滤波假设所有的变量是随机的且符合高斯分布（正态分布）。每个变量有一个均值 和一个方差 （ 图5.b ）。而 图5.c 则表示速度和位置是相关的。

假如我们已知上一个状态的位置值，现在要预测下一个状态的位置值。如果我们的速度值很高，我们移动的距离会远一点。相反，如果速度慢，WALL-E不会走的很远。这种关系在跟踪系统状态时很重要，它给了我们更多的信息：一个观测值告诉我们另一个观测值可能是什么样子。这就是卡尔曼滤波的目的------从所有不确定信息中提取有价值的信息。

根据数理统计知识，我们知道这种两个观测值（随机变量）之间的关系可以通过一个协方差矩阵

描述（ 图6 ）。

我们假设系统状态的分布为 高斯分布（正态分布） ，所以在 时刻我们需要两个信息：最佳预估值 及其协方差矩阵 （如式(2)所示）。

下一步，我们需要通过 时刻的状态来预测 时刻的状态。请注意，我们不知道状态的准确值，但是我们的预测函数并不在乎，它仅仅是对 时刻所有可能值的范围进行预测转移，然后得出一个k时刻新值的范围。在这个过程中，位置 和速度 的变化为

我们可以通过一个状态转移矩阵 来描述这个转换关系

同理，我们更新协方差矩阵 为

到目前为止，我们考虑的都是匀速运动的情况，也就是系统没有对WALL-E的运动状态进行控制的情况。那么，如果系统对WALL-E进行控制，例如发出一些指令启动或者制动轮子，对这些额外的信息，我们可以通过一个向量 来描述这些信息，并将其添加到我们的预测方程里作为一个修正。假如我们通过发出的指令得到预期的加速度 ，运动状态方程就更新为

引入矩阵表示为

式中 称为控制矩阵， 称为控制向量（例如加速度 ）。当然，如果没有任何外界动力影响的系统，可以忽略这一部分。

我们增加另一个细节，假如我们的预测转换矩阵不是100%准确呢，会发生什么？如果状态只会根据系统自身特性演变，那样将不会有任何问题。如果所有外界作用力对系统的影响可以被计算得十分准确，那样也不会有任何问题。但是如果有些外力我们无法预测，例如我们在跟踪一个四轴飞行器，它会受到风力影响；或者在跟踪一个轮式机器人，轮子可能会打滑，地面上的突起会使它减速。我们无法跟踪这些因素，而这些不确定事件发生时，预测方程将会失灵。因此，我们将这些不确定性统一建模，在预测方程中增加一个不确定项。

通过这种方式，使得原始状态中的每一个点可以都会预测转换到一个范围，而不是某个确定的点（ 图7.a ）。 可以这样描述------ 中的每个点移动到一个符合方差 的高斯分布里（ 图7.b ）。换言之，我们把这些不确定因素描述为方差为 的高斯噪声，并用 表示。这样就会产生一个新的高斯分布，方差不同，但是均值相同（ 图7.c ）。

通过对 的叠加扩展，得到完整的预测转换方程为

新的预测转换方程只是引入了已知的系统控制因素。新的不确定性可以通过之前的不确定性计算得到。到这里，我们得到了一个模糊的估计范围------通过 和 描述的范围。

我们之前的工作仍然是在使用运动模型一种方法来估计系统的状态，现在，我们要把另一种方法，也就是观测（本问题中为GPS定位）考虑进来，以进一步修正对运动状态的估计（ 图8 ）。

我们用矩阵 来描述观测方法的作用，于是有

再加入观测噪声 ，观测方程为

从控制论的角度出发，我们定义新息（也即观测值与预测值的误差）为

当然我们也知道，观测本身也会存在误差，比如本问题中的GPS定位精度仅有10m. 因此，我们用矩阵 来描述这种不确定性（ 图10 及 图11.a ）。

这时，我们新息的协方差为

现在我们需要把两种方法得到的可能性融合起来（ 图11.b ）。对于任何状态，有两个可能性：1. 传感器的观测值更接近系统真实状态；2. 模型推算的估计值更接近系统真实状态。如果有两个相互独立的获取系统状态的方式，并且我们想知道两者都准确的概率值，于是我们可以通过加权来解决更相信谁的问题（ 图11.c ）。

我们现在知道，系统模型的状态预测 与对系统的状态观测 服从高斯分布，把这个问题抽象一下就是——

根据我们的一个估计准则------ 最小方差估计 ，那么这个问题可以转化为优化问题求解

求导数（差分）得

则 ，从而

当维度高于一维时，我们用矩阵来描述，有

这里的 称为 卡尔曼增益 （Kalman Gain），也就是我们在解决更信任哪种方法时的偏向程度。

如果我们从两个独立的维度估计系统状态，那么根据系统模型的预测为

通过传感器的观测为

我们结合着两种方法得到

由 可知，卡尔曼增益为

将 约去（ 中也含有 项），得

此时的卡尔曼增益实际为

我们最后再来验证一下 估计的无偏性 ——

这里我们设 时刻的真值为 ，由于

由于 （ 从初值而来的无偏传递性 ）可知 ，即卡尔曼滤波满足无偏估计准则。显然，其中要求系统噪声和观测噪声是不相关、零期望的白噪声，且是线性系统，初始时刻的状态估计是无偏的。当这些条件不能满足时，卡尔曼滤波的估计结果是有偏的。

到这里，我们已经获得了卡尔曼滤波的全部要素。我们可以把整个过程总结为3个基本假设

假设一 和 都是零均值高斯白噪声，也即 ，

假设二 与 无关，也即

假设三 系统初值 的均值和方差已知，且 与 均不相关。

以及5个基本方程 方程一 状态预测

方程二 协方差预测

方程三 卡尔曼增益

4. 卡尔曼滤波

将预测值和测量值进行结合，对系统状态进行最优估计的算法。

在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。

根据k-1时刻的系统状态预测k时刻系统状态。

考虑外部因素控制的影响
外部因素会对系统进行控制，从而带来一些与系统自身状态没有相关性的改变。其中 成为控制矩阵， 称为控制向量，如果没有外部控制，这部分可以忽略。

外部噪声因素
在每次预测之后，我们可以添加一些新的不确定性来建立这种与“外界”（即我们没有跟踪的干扰）之间的不确定性模型

小结：
由上两式可知，新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正。 而新的不确定性由上一不确定性预测得到，并加上外部环境的干扰。

加入传感器观测数据
卡尔曼滤波的一大优点就是能处理传感器噪声，我们的传感器或多或少都有点不可靠，并且原始估计中的每个状态可以和一定范围内的传感器读数对应起来。 从测量到的传感器数据中，我们大致能猜到系统当前处于什么状态。但是由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。
传感器早上用协方差 表示，该分布的均值 是我们读取到的传感器数据。
于是我们得到两个高斯分布，一个是预测值附近，一个是传感器读数附近。把两个具有不同均值和方差的高斯分布相乘，得到一个新的具有独立均值和方差的高斯分布。
结果如下，其中，K为卡尔曼增益。

总结：
我们可以用这些公式对任何线性系统建立精确的模型，对于非线性系统来说，我们使用扩展卡尔曼滤波，区别在于EKF多了一个把预测和测量部分进行线性化的过程。

参考文章：
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
https://blog.csdn.net/u010720661/article/details/63253509

5. 如何通俗并尽可能详细解释卡尔曼滤波

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。 关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。

数据滤波是去除噪声还原真实数据的一种数据处理技术, Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处理, Kalman滤波是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用。

6. Apollo卡尔曼滤波与EKF

传感器存在误差——需要跟踪——卡尔曼滤波

障碍物跟踪、车道线跟踪、障碍物预测以及定位等领域；

线性系统，且参数是符合高斯分布 ，即完全可以被均值和协方差参数化:X∼N(μ, σ²)。

预测状态（prediction）与观测状态(measurement)分别加权求和作为新时刻的状态向量。

prediction ：

(状态协方差矩阵，表示系统不确定度)

measurement update :

（实际观测到的测量值 z 与预测值 x' 之间差值 y）

（当前状态向量 x 的更新）

（根据卡尔曼增益，更新了系统的不确定度 P）

其中，
F （state transition matrix）：状态转移矩阵(运动学推导)；

B ：控制输入矩阵；

u ：引起运动的输入向量（如油门、刹车、转角）；

w ：过程向量中的噪声因素，符合高斯分布，w∼N(0,Q)；

P （state covariance matrix）：在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器中，不确定程度会变小；

Q （process covariance matrix）：过程噪声，即无法用 x'=Fx+u 表示的噪声，比如车辆运动时突然到了上坡，这个影响是无法用之前的状态转移估计的；（由于 Q 对整个系统存在影响，但又不能太确定对系统的影响有多大。工 程上，我们一般将 Q 设置为单位矩阵参与运算 ）

H （Measurement Matrix）：测量矩阵， 由于传感器的测量值不一定就是直接的状态向量 ，如毫米波雷达是角度和位置信息，这里H就是转换矩阵，将预测向量转为测量数据；

R （measurement covariance matrix）：测量噪声矩阵，表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供该值；

S 只是为了简化公式，写的一个临时变量；

K （Kalman Gain）：卡尔曼增益，也即y的权重。

问题 ：针对线性系统，且符合高斯分布。

当状态向量与测量量之间映射关系是 非线性 时，这将使得卡尔曼滤波的过程和测量符合高斯分布的假设无效。

如毫米波雷达，返回数据是基于极坐标系，包含：

此时，H转移矩阵（状态向量向测量数据转换），也即笛卡尔坐标向极坐标映射：

这个映射就是非线性函数。

EKF 使用 局部线性逼近非线性模型 ，通过计算当前状态估计的 一阶泰勒展开 得出。一阶的逼近也叫 雅克比矩阵 。

如对上 泰勒展开（在均值u处，本例为0），
求出雅克比矩阵如下：

对比公式

2. 自动驾驶中的传感器融合算法：第一部分 - 卡尔曼滤波器和扩展卡尔曼滤波器

5. 详解卡尔曼滤波原理

7. 为什么要进行遥控和传感器的数据滤波算法处理

基本条件：具备数据采集、FFT、低通、高通、带通和带阻数字滤波器等；
若作低通滤波 输入原始数据、作FFT、选低通滤波、设置截止频率、输出结果；
其它类似。如果有数据处理，这类工作是很简单的。

8. 对压力传感器得到的信号应进行怎样的滤波，高通还是低通

这个应该根据你的压力传感器和你的应用场合来选择如何去除噪声，可能单单的低通无法满足，比如如果你用的是manometer，可能需要较复杂的算法得到比较稳定的压力输出信号

9. AD数字信号（称重传感器）的防抖动滤波用哪种算法稳定

抖动一般是在0两侧分布的，所以几次采样求和取平均应该就可以了，而且还快，本身这种就是稳健估计，不易发散的

10. 传感器的信号抖动怎么去除

请问这是需要加一个低通滤波吗？我试了用RC滤波器可是看不出效果。是不是我的R和C的参数选择不合适？是否有其他更简便的方法呢？只要能把那个抖动去掉就可以。

另外，这种随机变化的传感器信号，它的频率怎么确定啊？传感器的输出大概40毫秒变化一次，也就是说可以认为是每一秒采样25次，是不是就是25Hz的信号呢？