滤波

最新推荐文章于 2023-04-03 14:05:06 发布

jks456

最新推荐文章于 2023-04-03 14:05:06 发布

阅读量1k

点赞数

本文将不解释卡尔曼滤波具体的数学原理，不作为小白晋升高端玩家的台阶，只说卡尔曼滤波在简单场景下（处理温度、加速度计、RSSI等数据）怎样只用三行代码实现，及相关参数的设置。代码提供Matlab和Objective-C版本。

最近做的项目用到iPhone收集的蓝牙信号强度，即RSSI值。发现果然如文献中所说，即使手机保持静止，检测到的来自同一蓝牙信标的信号强度也会不停地上下波动，且数据（滤除粗大误差后）呈高斯分布。许多文献中也提到，对付这样的噪声可以用卡尔曼滤波器。同样地，做自平衡车、四旋翼等用到的陀螺仪、加速度数据也常常用到卡尔曼滤波。

图一：iPhone以10Hz的频率扫描蓝牙，记录下的RSSI值，大致呈高斯分布。

图二：蓝线为RSSI原始数据，大致围绕-67上下浮动；红线为卡尔曼滤波后平缓变化的数据。

百度百科对卡尔曼滤波的简介：

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

卡尔曼滤波的数学原理当然是它最美妙的部分，不过据说挺难看懂的，我也就知难而退了，毕竟在程序中实现好像不需要非常理解其数学原理。有两篇英文博客写得不错，比较生动形象地描述了这个滤波器的工作过程，建议阅读：

一、最后一点点数学

为了不让实现滤波器的代码显得突兀，这里还是留下几个最最不可或缺的方程。实在心急，可以直接跳过这一部分。
（要找OC代码的朋友请不要一并跳过第二部分，那里讲到了参数设置）

被观测的系统

首先我们需要用方程来描述被观测的系统，因为后面滤波器的方程要用到这里的一些参数。卡尔曼滤波器适用于线性系统，设该系统的状态方程和观测方程为：

x(k) = A · x(k-1) + B · u(k) + w(k)
z(k) = H · x(k) + y(k)

x(k) —— k时刻系统的状态
u(k) —— 控制量
w(k) —— 符合高斯分布的过程噪声，其协方差在下文中为Q
z(k) —— k时刻系统的观测值
y(k) —— 符合高斯分布的测量噪声，其协方差在下文中为R
A、B、H —— 系统参数，多输入多输出时为矩阵，单输入单输出时就是几个常数

注意在后面滤波器的方程中我们将不会再直接面对两个噪声w(k)和y(k)，而是用到他们的协方差Q和R。至此，A、B、H、Q、R这几个参数都由被观测的系统本身和测量过程中的噪声确定了。

滤波器

工程中用到的卡尔曼滤波是一个迭代的过程，每得到一个新的观测值迭代一次，来回来去地更新两个东西：“系统状态”(x)和“误差协方差”(P)。由于每次迭代只用到上一次迭代的结果和新的测量值，这样滤波对计算资源的占用是很小的。

每一次迭代，两个步骤：预测和修正。预测是根据前一时刻迭代的结果，即x(k-1|k-1)和P(k-1|k-1)，来预测这一时刻的系统状态和误差协方差，得到x(k|k-1)和P(k|k-1)：

x(k|k-1) = A · x(k-1|k-1) + B · u(k)
P(k|k-1) = A · P(k-1|k-1) · A^T + Q

(这里用到的A、B、H、Q、R就是从前面的状态/观测方程中拿来的)
然后计算卡尔曼增益K(k)，和这一次的实际测量结果z(k)一起，用于修正系统状态x(k|k-1)及误差协方差P(k|k-1)，得到最新的x(k|k)和P(k|k)：

K(k) = P(k|k-1) · H^T · (H · P(k|k-1) · H^T + R)^-1
x(k|k) = x(k|k-1) + K(k) · (z(k) - H · x(k|k-1))
P(k|k) = (I - K(k) · H) · P(k|k-1)

好了好了不会有新的公式了。至此这次迭代就算结束了，x(k|k)就是我们要的滤波后的值，它和P(k|k)将会作为x(k-1|k-1)和P(k-1|k-1)用在下一时刻的迭代里。别看现在公式还是一大堆，在我们所谓的“简单场景”下，系统参数一定下来，就能简化很多很多。

先看状态方程和观测方程。假设我们是在测温度、加速度或者记录蓝牙RSSI，此时控制量是没有的，即：

B · u(k) ≡ 0

另外，参数A和H也简单地取1。现在滤波器的预测方程简化为：

① x(k|k-1) = x(k-1|k-1)
② P(k|k-1) = P(k-1|k-1) + Q

同时修正方程变成：

③ K(k) = P(k|k-1) / (P(k|k-1) + R)
④ x(k|k) = x(k|k-1) + K(k) · (z(k) - x(k|k-1))
⑤ P(k|k) = (1 - K(k)) · P(k|k-1)

①对②、③无影响，于是④可以写成：

x(k|k) = x(k-1|k-1) + K(k) · (z(k) - x(k-1|k-1))

在程序中，该式写起来更简单：

x = x + K * (新观测值 - x);

观察②，对这一时刻的预测值不就是上一时刻的修正值+Q嘛，不妨把它合并到上一次迭代中，即⑤改写成：

P(k+1|k) = (1 - K(k)) · P(k|k-1) + Q

这一时刻的P(k+1|k)，会作为下一时刻的P(k|k-1)，刚好是③需要的。于是整个滤波过程只用这三个式子来迭代即可：

K(k) = P(k|k-1) / (P(k|k-1) + R)
x(k|k) = x(k-1|k-1) + K(k) · (z(k) - x(k-1|k-1))
P(k+1|k) = (1 - K(k)) · P(k|k-1) + Q

二、Matlab实现 & 参数设置

经过前面的推导，我们发现在程序中只需要不断重复这一小段即可实现卡尔曼滤波：

while(新观测值)
{
    K = P / (P + R);
    x = x + K * (新观测值 - x);
    P = (1 - K) · P + Q;
}

没看前面推导的朋友们别担心。现在我们只从程序的角度来看，不管物理意义。只要定好参数、赋好初值，它就能好好迭代下去。我们发现这一堆字母中，x和P只需要赋初值；K用不着赋初值；Q和R需要赋值，在之后的迭代中也可以改。

好消息是，x和P的初值是可以随便设的，强大的卡尔曼滤波器马上就能抹除不合理之处。但需注意，P的初值不能为0，否则滤波器会认为已经没有误差了，不玩儿了，看下图——

只要P(0) ≠ 0，一切都好。

剩下的也就是Q和R了。他俩的物理意义是噪声的协方差，他们的值是需要我们试出来的。举个例子，我们仍用上图的数据——

减小R值以后，滤波效果就没有这么明显了。但这并不意味着R值越大越好，因为我们的数据也可能是这样：

右图中增大了R值以后，虽然滤波结果更平滑，但滤波器会变得不敏感，会有滞后。因此应该根据具体的使用场景收集到的数据来决定Q和R的取值。他俩的取值当然也可以是时变的，可以识别跳变，可以自适应……这些就是高端玩法了，这里不做讨论。

至此，所有准备工作就做完了。Matlab代码也很简单：

% kalman_filter.m

function X = kalman_filter(data,Q,R,x0,P0)
N = length(data);

K = zeros(N,1);
X = zeros(N,1);
P = zeros(N,1);

X(1) = x0;
P(1) = P0;

for i = 2:N
    K(i) = P(i-1) / (P(i-1) + R);
    X(i) = X(i-1) + K(i) * (data(i) - X(i-1));
    P(i) = P(i-1) - K(i) * P(i-1) + Q;
end

调用及作图：

% test.m

clear;

data = [-59,-60,-55,-56,-56,-56,-57,-57,-59,-61,-57,-57,-57,-57,-57,-58,-57,-61,-57,-61,-59,-61,-60,-60,-59,-57,-56,-58,-58,-57,-57,-57,-57,-57,-60,-58,-61,-60,-60,-60,-57,-57,-57,-57,-58,-57,-58,-57,-58,-57,-58,-61,-58,-61,-62,-61,-61,-57,-57,-57,-57,-58,-57,-58,-57,-58,-61,-61,-60,-60,-58,-57,-57,-57,-57,-58,-58,-58,-58,-60,-61,-60,-57,-57,-57,-57,-57,-58,-58,-57,-57,-57,-60,-57,-60,-60,-59,-60,-57,-56]';
result = kalman_filter(data,1e-6,4e-4,-60,1);
i = 1:length(data);
plot(i,result,'r',i,data,'b');

做平面定位的时候可能需要滤x轴和y轴的数据，加速度计还可能有z轴数据。Matlab里面可以很方便地把A、B、H、Q、R变成矩阵，一次性把几个数据同时滤了；但实质上当这些数据互相之间没什么影响时，同时滤和分开滤是一毛一样的，所以上面的代码都可以不改，几个轴的数据分别调用滤波函数就是了。

三、Objective-C实现

虽然代码不多，但还是单开一个类好了。代码借鉴了MGKalman库。

//  KalmanFilter.h

#import <Foundation/Foundation.h>

@interface KalmanFilter : NSObject

/*-----------------------------------------------*\
|  Kalman Filter equations                        |
|                                                 |
|  state equation                                 |
|  x(k) = A·x(k-1) + B·u(k) + w(k-1)              |
|                                                 |
|  observations equation                          |
|  z(k) = H·x(k) + y(k)                           |
|                                                 |
|  prediction equations                           |
|  x(k|k-1) = A·x(k-1|k-1) + B·u(k)               |
|  P(k|k-1) = A·P(k-1|k-1)·A^T + Q                |
|                                                 |
|  correction equations                           |
|  K(k) = P(k|k-1)·H^T·(H·P(k|k-1)·H^T + R)^(-1)  |
|  x(k|k) = x(k|k-1) + K(k)·(z(k) - H·x(k|k-1))   |
|  P(k|k) = (I - K(k)·H)·P(k|k-1)                 |
\*-----------------------------------------------*/

@property (assign, nonatomic) UInt64 timestamp;
@property (assign, nonatomic) float Q;              // Process noise covariance
@property (assign, nonatomic) float R;              // Observation noise covariance
@property (assign, nonatomic) float K;              // Kalman gain
@property (assign, nonatomic) float X;              // State
@property (assign, nonatomic) float P;              // Covariance

- (KalmanFilter *)initWithQ:(float)q R:(float)r X0:(float)x0 P0:(float)p0;
- (float)filterWithObservation:(float)observation;

@end

//  KalmanFilter.m

#import "KalmanFilter.h"

@implementation KalmanFilter

- (KalmanFilter *)initWithQ:(float)q R:(float)r X0:(float)x0 P0:(float)p0 {
    if (self = [super init]) {
        _timestamp = [[NSDate date] timeIntervalSince1970] * 1000;
        _Q = q;
        _R = r;
        _K = 0;
        _X = x0;
        _P = p0;
    }

    return self;
}

- (float)filterWithObservation:(float)observation {
    /*-----------------------------------------------*\
    |  Simplified Kalman Filter equations             |
    |                                                 |
    |  state equation                                 |
    |  x(k) = x(k-1) + w(k)                           |
    |                                                 |
    |  observations equation                          |
    |  z(k) = x(k) + y(k)                             |
    |                                                 |
    |  prediction equations                           |
    |  x(k|k-1) = x(k-1|k-1)                          |
    |  P(k|k-1) = P(k-1|k-1) + Q                      |
    |                                                 |
    |  correction equations                           |
    |  K(k) = P(k|k-1)·(P(k|k-1) + R)^(-1)            |
    |  x(k|k) = x(k|k-1) + K(k)·(z(k) - x(k|k-1))     |
    |  P(k|k) = (I - K(k))·P(k|k-1)                   |
    |                                                 |
    |  Iteration equations                            |
    |  K(k) = P(k|k-1)/(P(k|k-1) + R)                 |
    |  x(k|k) = x(k-1|k-1) + K(k)·(z(k) - x(k-1|k-1)) |
    |  P(k+1|k) = (1 - K(k))·P(k|k-1) + Q             |
    \*-----------------------------------------------*/

    _timestamp = [[NSDate date] timeIntervalSince1970] * 1000;
    _K = _P / (_P + _R);
    _X = _X + _K * (observation - _X);
    _P = (1 - _K) * _P + _Q;

    return _X;
}

@end

初始化及调用：

……
#define val_Q       0.00002
#define val_R        0.0004
#define val_X0            0
#define val_P0            1
……
@property (strong, nonatomic) KalmanFilter *kalmanFilter;
……
// 初始化
_kalmanFilter = [[KalmanFilter alloc] initWithQ:val_Q
                                              R:val_R
                                             X0:val_X0
                                             P0:val_P0];
……
// 完成一次测量时调用
float newX = [_kalmanFilter filterWithObservation:(float)新观测值];
……

这样每一次得到的newX就是滤波后的数据。如果同时要滤几个轴的数据，也可以放在矩阵里运算。但同样地，如果几个轴之间的数据互不影响的话，一起滤和分开滤是一样的。

前面提到的MGKalman是一个OC实现的矩阵形式的卡尔曼滤波器，其用法在GitHub上有说明。本文的数学方程也是从那里摘抄以后修改而来的。

作者给的示例代码中同时滤了两个相互独立的量；经过我的测试，把他俩拆开，不用矩阵，分别用两个滤波器来处理，效果确实一毛一样。这也是我为什么我在项目中没有用他矩阵的那一套，而是看过一点原理以后只用那三行代码的原因。毕竟使用场景简单嘛。

卡尔曼滤波(KF)的简单使用就介绍到这里了。接下来还可以去实时调整Q和R，可以加控制量u(k)，可以进一步实现扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)等各种算法来应对非线性、非高斯等各种情况。只是对于我来说，现在这样就暂时够用了。

作者：伦啊伦
链接：http://www.jianshu.com/p/c512a2b82907
來源：简书
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

jks456

关注

0
点赞
踩
6

收藏

觉得还不错? 一键收藏
0
评论
滤波

本文将不解释卡尔曼滤波具体的数学原理，不作为小白晋升高端玩家的台阶，只说卡尔曼滤波在简单场景下（处理温度、加速度计、RSSI等数据）怎样只用三行代码实现，及相关参数的设置。代码提供Matlab和Objective-C版本。最近做的项目用到iPhone收集的蓝牙信号强度，即RSSI值。发现果然如文献中所说，即使手机保持静止，检测到的来自同一蓝牙信标的信号强度也会不停地上下波动，且数据（滤除粗
复制链接

扫一扫