進行目標跟蹤時，先驗知識告訴我們定位軌跡是平滑的，目標當前時刻的狀態與上一時刻的狀態有關，濾波方法可以將這些先驗知識考慮進來得到更準確的定位軌跡。本文簡單介紹卡爾曼濾波及其使用。

原理

這裡僅從目標定位跟蹤的角度做一個簡化版的介紹。

定位跟蹤時，可以通過某種定位技術（比如位置指紋法）得到一個位置估計（觀測位置），也可以根據我們的經驗（運動目標常常是勻速運動的）由上一時刻的位置和速度來預測出當前位置（預測位置）。把這個觀測結果和預測結果做一個加權平均作為定位結果，權值的大小取決於觀測位置和預測位置的不確定性程度，在數學上可以證明在預測過程和觀測過程都是線性高斯時，按照卡爾曼的方法做加權是最優的。

擴充套件：在有些應用中，如果不是線性高斯的情況該怎麼辦？可以採用EKF（擴充套件卡爾曼濾波），在工作點附近對系統進行線性化，即使不是高斯也近似成高斯去做。這樣做有點太粗糙了，於是又有了IEKF（迭代卡爾曼濾波，對工作點進行迭代優化），UKF或SPKF（無跡卡爾曼濾波，不做線性化，而是投影做出一個高斯分佈去近似）。或者拋棄各種假設，直接採用蒙特卡洛的方式，假設出很多的粒子去近似分佈，就是PF（粒子濾波）。

步驟

理解下面這個五個方程的含義就可以了：
\[\hat{x}_k^- = A\hat{x}_{k-1} + Bu_{k-1}\]\[P_k^- = A P_{k-1} A^T + Q\]

• 公式（步驟）1：由上一時刻的狀態預測當前狀態，加上外界的輸入。
• 公式（步驟）2：預測過程增加了新的不確定性\(Q\)，加上之前存在的不確定性。
• 公式（步驟）3：由預測結果的不確定性\(P_k^-\)和觀測結果的不確定性\(R\)計算卡爾曼增益（權重）。
• 公式（步驟）4：對預測結果和觀測結果做加權平均，得到當前時刻的狀態估計。
• 公式（步驟）5：更新\(P_k\)
  ，代表本次狀態估計的不確定性。

需要注意的是，在定位中狀態\(x_k\)是一個向量，除了座標外還可以包含速度，比如\(x_k = (座標x，座標y，速度x，速度y)\)，狀態是向量而不僅僅是一個標量，上面的幾個公式中的矩陣乘法實際上是同時對多個狀態進行計算，表示不確定性的方差也就成了協方差矩陣。

實踐

下面用matlab動手寫一個卡爾曼濾波：首次使用卡爾曼濾波時先呼叫函式kf_init()對初始化結構體kf_params的各項引數，之後每次濾波時，設定當前的觀測值，呼叫kf_update()進行更新，定位結果包含在返回的引數kf_params中。
Github地址
python版參考http://www.cnblogs.com/rubbninja/p/6256072.html

函式kf_update()

function kf_params = kf_update(kf_params)
    % 以下為卡爾曼濾波的五個方程（步驟）
    x_ = kf_params.A * kf_params.x + kf_params.B * kf_params.u;
    P_ = kf_params.A * kf_params.P * kf_params.A' + kf_params.Q;
    kf_params.K = P_ * kf_params.H' * (kf_params.H * P_ * kf_params.H' + kf_params.R)^-1;
    kf_params.x = x_ + kf_params.K * (kf_params.z - kf_params.H * x_);
    kf_params.P = P_ - kf_params.K * kf_params.H * P_;
end
 

函式kf_init()

function kf_params = kf_init(Px, Py, Vx, Vy)
%% 本例中，狀態x為（座標x， 座標y， 速度x， 速度y），觀測值z為（座標x， 座標y）
 
    kf_params.B = 0; %外部輸入為0
    kf_params.u = 0; %外部輸入為0
    kf_params.K = NaN; %卡爾曼增益無需初始化
    kf_params.z = NaN; %這裡無需初始化，每次使用kf_update之前需要輸入觀察值z
    kf_params.P = zeros(4, 4); %初始P設為0
 
    %% 初始狀態：函式外部提供初始化的狀態，本例使用觀察值進行初始化，Vx，Vy初始為0
    kf_params.x = [Px; Py; Vx; Vy];
 
    %% 狀態轉移矩陣A
    kf_params.A = eye(4) + diag(ones(1, 2), 2); % 和線性系統的預測機制有關，這裡的線性系統是上一刻的位置加上速度等於當前時刻的位置，而速度本身保持不變
 
    %% 預測噪聲協方差矩陣Q：假設預測過程上疊加一個高斯噪聲，協方差矩陣為Q
    %大小取決於對預測過程的信任程度。比如，假設認為運動目標在y軸上的速度可能不勻速，那麼可以把這個對角矩陣的最後一個值調大。有時希望出來的軌跡更平滑，可以把這個調更小
    kf_params.Q = diag(ones(4, 1) * 0.001); 
 
    %% 觀測矩陣H：z = H * x
    kf_params.H = eye(2, 4); % 這裡的狀態是（座標x， 座標y， 速度x， 速度y），觀察值是（座標x， 座標y），所以H = eye(2, 4)
 
    %% 觀測噪聲協方差矩陣R：假設觀測過程上存在一個高斯噪聲，協方差矩陣為R
    kf_params.R = diag(ones(2, 1) * 2); %大小取決於對觀察過程的信任程度。比如，假設觀測結果中的座標x值常常很準確，那麼矩陣R的第一個值應該比較小
end

測試卡爾曼濾波的效果

模擬一條運動軌跡，然後加上高斯觀察噪聲，作為觀測位置軌跡。然後使用卡爾曼濾波得到濾波後的結果。可以分別計算出觀察位置軌跡的定位精度和濾波後軌跡的定位精度。

addpath('./filters');
addpath('./IP_raytracing');
%% 模擬一條運動軌跡，然後加上高斯觀察噪聲，作為觀測位置軌跡。然後使用卡爾曼濾波得到濾波後的結果。
% 速度為均值0.6m標準差0.05的高斯分佈
% 觀測噪聲標準差為2
 
%% 畫出實際的真實路徑
roomLength = 1000;
roomWidth = 1000;
t = 500;
trace_real = get_random_trace(roomLength, roomWidth, t);
figure; 
subplot(1, 3, 1); plot(trace_real(:, 1), trace_real(:, 2), '.');
title('實際的真實路徑');
 
%% 有觀測噪聲時的路徑
noise = 2; %2m的位置波動噪聲
trace = trace_real + normrnd(0, noise, size(trace_real));
subplot(1, 3, 2); plot(trace(:, 1), trace(:, 2), '.');
title('有噪聲時的路徑');
fprintf('卡爾曼濾波之前的定位精度： %f m\n', accuracy(trace, trace_real));

%% 對有噪聲的路徑進行卡爾曼濾波
kf_params_record = zeros(size(trace, 1), 4);
for i = 1 : t
    if i == 1
        kf_params = kf_init(trace(i, 1), trace(i, 2), 0, 0); % 初始化
    else
        kf_params.z = trace(i, 1:2)'; %設定當前時刻的觀測位置
        kf_params = kf_update(kf_params); % 卡爾曼濾波
    end
    kf_params_record(i, :) = kf_params.x';
end
kf_trace = kf_params_record(:, 1:2);
subplot(1, 3, 3); plot(kf_trace(:, 1), kf_trace(:, 2), '.');
title('卡爾曼濾波後的效果');
fprintf('卡爾曼濾波之後的定位精度： %f m\n', accuracy(kf_trace, trace_real));

典型的一組測試結果為：
卡爾曼濾波之前的定位精度： 2.424880 m
卡爾曼濾波之後的定位精度： 1.426890 m

在這組測試中，濾波後的軌跡更平滑，而且精度從2.4m提高到1.4m，之所以能到達這樣好的結果，是因為充分使用了先驗知識：目標的運動是連續且基本勻速的。

作者：rubbninja
出處：http://www.cnblogs.com/rubbninja/
關於作者：目前主要研究領域為機器學習與無線定位技術，歡迎討論與指正！
版權宣告：本文版權歸作者和部落格園共有，轉載請註明出處。