Kalman濾波簡介
Kalman濾波是一種線性濾波與預測方法，原文為：A New Approach to Linear Filtering and Prediction Problems。文章推導很復雜，看了一半就看不下去了，既然不能透徹理解其原理，但總可以通過實驗來理解其具體的使用方法。
Kalman濾波分為2個步驟，預測(predict)和校正(correct)。預測是基於上一時刻狀態估計當前時刻狀態，而校正則是綜合當前時刻的估計狀態與觀測狀態，估計出最優的狀態。預測與校正的過程如下：
預測：
校正：
公式1是狀態預測，公式2是誤差矩陣預測，公式3是kalman增益計算，公式4是狀態校正，其輸出即是最終的kalman濾波結果，公式5是誤差矩陣更新。各變量說明如下表：
| xk |
k時刻的狀態 |
| A |
狀態轉移矩陣，和具體的線性系統相關 |
| uk |
K時刻外界對系統的作用 |
| B |
輸入控制矩陣，外界的影響如何轉化為對狀態的影響 |
| P |
誤差矩陣 |
| Q |
預測噪聲協方差矩陣 |
| R |
測量噪聲協方差矩陣 |
| H |
觀測矩陣 |
| Kk |
K時刻的kalman增益 |
| zk |
K時刻的觀測值 |
算法實現與分析
Kalman濾波最復雜的計算應該就是公式3中的矩陣求逆，考慮到實現的方便性，采用matlab來簡單實現，本文主要是分析kalman濾波中各個變量的作用和對濾波結果的影響。具體代碼如下：
function filter = Kalman(filter) %predict predict_x = filter.A * filter.x + filter.B * filter.u; filter.P = filter.A * filter.P * filter.A' + filter.Q; %correct filter.K = filter.P * filter.H' / (filter.H * filter.P * filter.H' + filter.R); filter.x = predict_x + filter.K * (filter.z - filter.H * predict_x); filter.P = filter.P - filter.K * filter.H * filter.P; end
在matlab中，kalman濾波實際上就是上面那5個公式，而難點卻是在測試代碼中針對不同問題各個變量的初始化上，下面來逐個分析。
1.建立模型，明確觀測量，系統狀態以及其轉移方程(下面這段公式太多，通過word寫好后截圖)
2.初始化噪聲協方差矩陣
經過上面一步，只有PQRK四個矩陣還未確定了。顯然增益矩陣K是不需要初始化的，P是誤差矩陣，初始化可以是一個隨機的矩陣或者0，只要經過幾次的處理基本上就能調整到正常的水平，因此也就只會影響前面幾次的濾波結果。
Q和R分別是預測和觀測狀態協方差矩陣，一般可以簡單認為系統狀態各維之間(即上面的a和b)相互獨立，那么Q和R就可以設置為對角陣。而這兩個對角線元素的大小將直接影響着濾波結果，若Q的元素遠大於R的元素，則預測噪聲大，從而更相信觀測值，這樣可能使得kalman濾波結果與觀測值基本一致；反之，則更相信預測，kalman濾波結果會表現得比較規整和平滑；若二者接近，則濾波結果介於前面兩者之間，根據實驗效果看也缺乏實際使用價值。
以上幾個矩陣確定后，對於狀態x，由於0時刻我們沒有任何關於該系統的知識，可以使用0時刻的測量值z0來初始x0，預測從k=1開始；也可以初始化-1時刻的狀態，當然這個狀態實際是未知的，也就可隨機取。2種方式都可以，但使用0時刻測量值來初始化狀態，可以使得前面幾次預測更准確。
3.實驗分析
首先使用下面代碼生成一組數據存在z.mat中：
interval = pi/18; t = 1:interval:100*pi; len = size(t, 2); a = t + 4 * (rand(1,len)-0.5); b = t .* sin(t/10) + 10 * (rand(1,len)-0.5); z = [a; b]; save('z.mat','z'); plot(z(1,:),z(2,:),'o')
可以看出其近似為一條振幅不斷增大的正弦曲線疊加一個隨機噪聲。繪制出來如下：
如果使用上面推導的恆定狀態系統模型，代碼與實驗結果如下：
clear close all clc dim_observe = 2; %觀測值維數 n = dim_observe; %狀態維數，觀測狀態每個維度都有1個速度，故需乘2 filter.A = eye(n);%[1,0,1,0;0,1,0,1;0,0,1,0;0,0,0,1]; filter.B = 0; filter.u = 0; filter.P = eye(n); filter.K = zeros(n); filter.H = eye(n);%[1,0,0,0;0,1,0,0]; cQ = 1e-8; cR = 1e-2; filter.Q = eye(n) * cQ; %這里簡單設置Q和R對角線元素都相等，設為不等亦可 filter.R = eye(dim_observe) * cR; filter.x = zeros(n,1); %初始狀態x0 load('z.mat'); figure(1),subplot(2,2,1), t = 1; out = []; for i=1:size(z,2) filter.z = z(:,i); filter = Kalman(filter); plot(filter.x(1),filter.x(2), 'r*');hold on plot(filter.z(1),filter.z(2), 'bo'); hold on out=[out filter.x]; % pause(.5) end figure(1), str = sprintf('cQ = %e, cR = %e', cQ, cR); title(str) %畫局部放大 subplot(2,2,2), plot(out(1,:),out(2,:), 'r*');hold on plot(z(1,:),z(2,:), 'bo'); hold on axis([120 170 80 200])
可以看出濾波結果完全滯后於測量數據，其根本原因在於建立的模型存在問題。
如果采用上面推導的物體運動模型則只需要修改部分代碼，主要是矩陣A和H，以及其他矩陣對應的維數，具體如下：
dim_observe = 2; %觀測值維數 n = 2 * dim_observe; %狀態維數，觀測狀態每個維度都有1個速度，故需乘2 filter.A = [1,0,1,0;0,1,0,1;0,0,1,0;0,0,0,1]; filter.B = 0; filter.u = 0; filter.P = eye(n); filter.K = zeros(n); filter.H = [1,0,0,0;0,1,0,0];
運行結果如下圖，藍色為觀測數據，紅色為kalman濾波數據，右側為局部放大圖。可以看出經過濾波后的數據相當平滑，這里Q和R中元素的量級分別為cQ和cR，下圖結果可以看到cR比cQ多了6個數量級。
（1）
增加幾組結果用於對比分析，對於的cQ和cR見圖的標題。
（2）
（3）
（4）
（5）
（6）
首先看圖1和2，cR與cQ大小均相差了3個數量級，而二者的比值相同，則kalman濾波結果相同。
再看圖2~圖6，cR/cQ在不斷減小，kalman濾波結果的平滑性也在不斷降低，到圖5和6中，濾波結果完全和觀測值相同，說明此時kalman濾波已經完全相信觀測值了。原因在於cR/cQ過小，系統認為預測噪聲的方差很大，不值得信賴，而觀測值的噪聲方差小，可信度高。
總結
根據上面的實驗結果，可以看出Kalman濾波應用中的幾個問題：
1.模型建立的正確性從根本上決定了濾波效果的正確性。
上面使用物體靜止模型進行濾波，結果完全不對，而使用勻速運動模型則能達到較好的效果。從根本上講，上面的數據也不是勻速運動的，為何結果會基本正確？看看第一個使用靜止模型的濾波結果，雖然我們假定了物體是靜止的，但由於觀測數據的作用，kalman濾波結果也會有相應的運動而不是完全靜止，也就是說濾波器在不停地修正這個狀態，而在勻速運動模型中，物體的速度我們認為不變，但同樣地kalman濾波器也會不停地修正這個速度，濾波器中計算的速度實質的偏離了真實速度的，因此最終也會有相應的偏差，不過這個偏差在我們容許范圍內，也就可以大膽使用了。
如果能確定物體是勻變速直線運動，使用相應帶加速度的模型會得到更准確的效果。但是越嚴格的模型其適用范圍也相應越小。
2.影響濾波結果平滑性的因素是cR/cQ，這個值反映了我們對於預測和觀測值的信任程度；其值越大則越相信預測結果，濾波結果平滑性好；反之則越相信觀測結果，濾波結果越偏向於觀測值。一般我們使用kalman濾波器是為了能平滑數據的波動，因此應盡量保證cR/cQ稍大，上面的測試結果該值在1e4以上數據較為平滑。