本文將不解釋卡爾曼濾波具體的數(shù)學(xué)原理，不作為小白晉升高端玩家的臺(tái)階，只說(shuō)卡爾曼濾波在簡(jiǎn)單場(chǎng)景下（處理溫度、加速度計(jì)、RSSI等數(shù)據(jù)）怎樣只用三行代碼實(shí)現(xiàn)，及相關(guān)參數(shù)的設(shè)置。代碼提供Matlab和Objective-C版本。
最近做的項(xiàng)目用到iPhone收集的藍(lán)牙信號(hào)強(qiáng)度，即RSSI值。發(fā)現(xiàn)果然如文獻(xiàn)中所說(shuō)，即使手機(jī)保持靜止，檢測(cè)到的來(lái)自同一藍(lán)牙信標(biāo)的信號(hào)強(qiáng)度也會(huì)不停地上下波動(dòng)，且數(shù)據(jù)（濾除粗大誤差后）呈高斯分布。許多文獻(xiàn)中也提到，對(duì)付這樣的噪聲可以用卡爾曼濾波器。同樣地，做自平衡車、四旋翼等用到的陀螺儀、加速度數(shù)據(jù)也常常用到卡爾曼濾波。
百度百科對(duì)卡爾曼濾波的簡(jiǎn)介：
卡爾曼濾波（Kalman filtering）一種利用線性系統(tǒng)狀態(tài)方程，通過(guò)系統(tǒng)輸入輸出觀測(cè)數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法。由于觀測(cè)數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計(jì)也可看作是濾波過(guò)程。
卡爾曼濾波的數(shù)學(xué)原理當(dāng)然是它最美妙的部分，不過(guò)據(jù)說(shuō)挺難看懂的，我也就知難而退了，畢竟在程序中實(shí)現(xiàn)好像不需要非常理解其數(shù)學(xué)原理。有兩篇英文博客寫得不錯(cuò)，比較生動(dòng)形象地描述了這個(gè)濾波器的工作過(guò)程，建議閱讀：
一、最后一點(diǎn)點(diǎn)數(shù)學(xué)
為了不讓實(shí)現(xiàn)濾波器的代碼顯得突兀，這里還是留下幾個(gè)最最不可或缺的方程。實(shí)在心急，可以直接跳過(guò)這一部分。
（要找OC代碼的朋友請(qǐng)不要一并跳過(guò)第二部分，那里講到了參數(shù)設(shè)置）
- 被觀測(cè)的系統(tǒng)
首先我們需要用方程來(lái)描述被觀測(cè)的系統(tǒng)，因?yàn)楹竺鏋V波器的方程要用到這里的一些參數(shù)?？柭鼮V波器適用于線性系統(tǒng)，設(shè)該系統(tǒng)的狀態(tài)方程和觀測(cè)方程為：
x(k) = A · x(k-1) + B · u(k) + w(k)
z(k) = H · x(k) + y(k)
x(k) —— k時(shí)刻系統(tǒng)的狀態(tài)
u(k) —— 控制量
w(k) —— 符合高斯分布的過(guò)程噪聲，其協(xié)方差在下文中為Q
z(k) —— k時(shí)刻系統(tǒng)的觀測(cè)值
y(k) —— 符合高斯分布的測(cè)量噪聲，其協(xié)方差在下文中為R
A、B、H —— 系統(tǒng)參數(shù)，多輸入多輸出時(shí)為矩陣，單輸入單輸出時(shí)就是幾個(gè)常數(shù)
注意在后面濾波器的方程中我們將不會(huì)再直接面對(duì)兩個(gè)噪聲w(k)和y(k)，而是用到他們的協(xié)方差Q和R。至此，A、B、H、Q、R這幾個(gè)參數(shù)都由被觀測(cè)的系統(tǒng)本身和測(cè)量過(guò)程中的噪聲確定了。
- 濾波器
工程中用到的卡爾曼濾波是一個(gè)迭代的過(guò)程，每得到一個(gè)新的觀測(cè)值迭代一次，來(lái)回來(lái)去地更新兩個(gè)東西：“系統(tǒng)狀態(tài)”(x)和“誤差協(xié)方差”(P)。由于每次迭代只用到上一次迭代的結(jié)果和新的測(cè)量值，這樣濾波對(duì)計(jì)算資源的占用是很小的。
每一次迭代，兩個(gè)步驟：預(yù)測(cè)和修正。預(yù)測(cè)是根據(jù)前一時(shí)刻迭代的結(jié)果，即x(k-1|k-1)和P(k-1|k-1)，來(lái)預(yù)測(cè)這一時(shí)刻的系統(tǒng)狀態(tài)和誤差協(xié)方差，得到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就是從前面的狀態(tài)/觀測(cè)方程中拿來(lái)的)
然后計(jì)算卡爾曼增益K(k)，和這一次的實(shí)際測(cè)量結(jié)果z(k)一起，用于修正系統(tǒng)狀態(tài)x(k|k-1)及誤差協(xié)方差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)
好了好了不會(huì)有新的公式了。至此這次迭代就算結(jié)束了，x(k|k)就是我們要的濾波后的值，它和P(k|k)將會(huì)作為x(k-1|k-1)和P(k-1|k-1)用在下一時(shí)刻的迭代里。別看現(xiàn)在公式還是一大堆，在我們所謂的“簡(jiǎn)單場(chǎng)景”下，系統(tǒng)參數(shù)一定下來(lái)，就能簡(jiǎn)化很多很多。
先看狀態(tài)方程和觀測(cè)方程。假設(shè)我們是在測(cè)溫度、加速度或者記錄藍(lán)牙RSSI，此時(shí)控制量是沒(méi)有的，即：
B · u(k) ≡ 0
另外，參數(shù)A和H也簡(jiǎn)單地取1?，F(xiàn)在濾波器的預(yù)測(cè)方程簡(jiǎn)化為：
① x(k|k-1) = x(k-1|k-1)
② P(k|k-1) = P(k-1|k-1) + Q
同時(shí)修正方程變成：
③ 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)
①對(duì)②、③無(wú)影響，于是④可以寫成：
x(k|k) = x(k-1|k-1) + K(k) · (z(k) - x(k-1|k-1))
在程序中，該式寫起來(lái)更簡(jiǎn)單：
x = x + K * (新觀測(cè)值 - x);
觀察②，對(duì)這一時(shí)刻的預(yù)測(cè)值不就是上一時(shí)刻的修正值+Q嘛，不妨把它合并到上一次迭代中，即⑤改寫成：
P(k+1|k) = (1 - K(k)) · P(k|k-1) + Q
這一時(shí)刻的P(k+1|k)，會(huì)作為下一時(shí)刻的P(k|k-1)，剛好是③需要的。于是整個(gè)濾波過(guò)程只用這三個(gè)式子來(lái)迭代即可：
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實(shí)現(xiàn) & 參數(shù)設(shè)置
經(jīng)過(guò)前面的推導(dǎo)，我們發(fā)現(xiàn)在程序中只需要不斷重復(fù)這一小段即可實(shí)現(xiàn)卡爾曼濾波：
while(新觀測(cè)值)
{
K = P / (P + R);
x = x + K * (新觀測(cè)值 - x);
P = (1 - K) · P + Q;
}
沒(méi)看前面推導(dǎo)的朋友們別擔(dān)心?，F(xiàn)在我們只從程序的角度來(lái)看，不管物理意義。只要定好參數(shù)、賦好初值，它就能好好迭代下去。我們發(fā)現(xiàn)這一堆字母中，x和P只需要賦初值，每次迭代會(huì)產(chǎn)生新值；K用不著賦初值；Q和R賦值以后在之后的迭代中也可以改。
好消息是，x和P的初值是可以隨便設(shè)的，強(qiáng)大的卡爾曼濾波器馬上就能抹除不合理之處。但需注意，P的初值不能為0，否則濾波器會(huì)認(rèn)為已經(jīng)沒(méi)有誤差了，不玩兒了，看下圖——
剩下的也就是Q和R了。他倆的物理意義是噪聲的協(xié)方差，他們的值是需要我們?cè)嚦鰜?lái)的。舉個(gè)例子，我們?nèi)杂蒙蠄D的數(shù)據(jù)——
減小R值以后，濾波效果就沒(méi)有這么明顯了。但這并不意味著R值越大越好，因?yàn)槲覀兊臄?shù)據(jù)也可能是這樣：
右圖中增大了R值以后，雖然濾波結(jié)果更平滑，但濾波器會(huì)變得不敏感，會(huì)有滯后。因此應(yīng)該根據(jù)具體的使用場(chǎng)景收集到的數(shù)據(jù)來(lái)決定Q和R的取值。他倆的取值當(dāng)然也可以是時(shí)變的，可以識(shí)別跳變，可以自適應(yīng)……這些就是高端玩法了，這里不做討論。
至此，所有準(zhǔn)備工作就做完了。Matlab代碼也很簡(jiǎn)單：
% 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
調(diào)用及作圖：
% 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');
做平面定位的時(shí)候可能需要濾x軸和y軸的數(shù)據(jù)，加速度計(jì)還可能有z軸數(shù)據(jù)。Matlab里面可以很方便地把A、B、H、Q、R變成矩陣，一次性把幾個(gè)數(shù)據(jù)同時(shí)濾了；但實(shí)質(zhì)上當(dāng)這些數(shù)據(jù)互相之間沒(méi)什么影響時(shí)，同時(shí)濾和分開(kāi)濾是一毛一樣的，所以上面的代碼都可以不改，幾個(gè)軸的數(shù)據(jù)分別調(diào)用濾波函數(shù)就是了。
三、Objective-C實(shí)現(xiàn)
雖然代碼不多，但還是單開(kāi)一個(gè)類好了。代碼借鑒了MGKalman庫(kù)。
// 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
初始化及調(diào)用：
……
#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];
……
// 完成一次測(cè)量時(shí)調(diào)用
float newX = [_kalmanFilter filterWithObservation:(float)新觀測(cè)值];
……
這樣每一次得到的newX就是濾波后的數(shù)據(jù)。如果同時(shí)要濾幾個(gè)軸的數(shù)據(jù)，也可以放在矩陣?yán)镞\(yùn)算。但同樣地，如果幾個(gè)軸之間的數(shù)據(jù)互不影響的話，一起濾和分開(kāi)濾是一樣的。
前面提到的MGKalman是一個(gè)OC實(shí)現(xiàn)的矩陣形式的卡爾曼濾波器，其用法在GitHub上有說(shuō)明。本文的數(shù)學(xué)方程也是從那里摘抄以后修改而來(lái)的。
作者給的示例代碼中同時(shí)濾了兩個(gè)相互獨(dú)立的量；經(jīng)過(guò)我的測(cè)試，把他倆拆開(kāi)，不用矩陣，分別用兩個(gè)濾波器來(lái)處理，效果確實(shí)一毛一樣。這也是我為什么我在項(xiàng)目中沒(méi)有用他矩陣的那一套，而是看過(guò)一點(diǎn)原理以后只用那三行代碼的原因。畢竟使用場(chǎng)景簡(jiǎn)單嘛。
卡爾曼濾波(KF)的簡(jiǎn)單使用就介紹到這里了。接下來(lái)還可以去實(shí)時(shí)調(diào)整Q和R，可以加控制量u(k)，可以進(jìn)一步實(shí)現(xiàn)擴(kuò)展卡爾曼濾波(EKF)、無(wú)跡卡爾曼濾波(UKF)等各種算法來(lái)應(yīng)對(duì)非線性、非高斯等各種情況。只是對(duì)于我來(lái)說(shuō)，現(xiàn)在這樣就暫時(shí)夠用了。
