本文将不解释卡尔曼滤波具体的数学原理，不作为小白晋升高端玩家的台阶，只说卡尔曼滤波在简单场景下（处理温度、加速度计、RSSI等数据）怎样只用三行代码实现，及相关参数的设置。代码提供Matlab和Objective-C版本。
最近做的项目用到iPhone收集的蓝牙信号强度，即RSSI值。发现果然如文献中所说，即使手机保持静止，检测到的来自同一蓝牙信标的信号强度也会不停地上下波动，且数据（滤除粗大误差后）呈高斯分布。许多文献中也提到，对付这样的噪声可以用卡尔曼滤波器。同样地，做自平衡车、四旋翼等用到的陀螺仪、加速度数据也常常用到卡尔曼滤波。
百度百科对卡尔曼滤波的简介：
卡尔曼滤波（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) · AT + 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) · HT · (H · P(k|k-1) · HT + 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，否则滤波器会认为已经没有误差了，不玩儿了，看下图——
剩下的也就是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)等各种算法来应对非线性、非高斯等各种情况。只是对于我来说，现在这样就暂时够用了。
