卡爾曼濾波詳解：一維卡爾曼濾波實例解析(五個公式以及各個參數的意義)

一、引言

本文以rssi(接收信號強度)濾波爲背景，結合卡爾曼的五個公式，設計 rssi 一維卡爾曼濾波器，用MATLAB語言實現一維卡爾曼濾波器，並附上代碼和濾波結果圖；

本文工分爲以下幾個部分：

1、引言

2、模型的系統方程和狀態方程

3、卡爾曼濾波過程及五個基本公式

4、公式中每個參數詳細註釋

5、結合rssi濾波實例設計濾波器

6、MATLAB實現濾波器

二、模型的系統方程和狀態方程

系統的狀態方程：

$\LARGE {\color{DarkBlue} }x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}$

狀態方程是根據上一時刻的狀態和控制變量來推測當前時刻的狀態， $\large w_{k-1}$ 是服從高斯分佈的噪聲，是預測過程的噪聲，它對應了 $\large x_k$ 中每個分量的噪聲，是期望爲 0，協方差爲 Q 的高斯白噪聲 $\large w_{k-1} \sim N_{(0, Q))}$ ,Q即下文的過程激勵噪聲Q.

系統的觀測方程：

$\LARGE z_k = Hx_k + v_k$

觀測方式是當前時刻的量測信息， $\large v_k$ 是觀測的噪聲，服從高斯分佈， $\large v_k \sim N_{(0,R)}$ ,R即下文的測量噪聲R。

卡爾曼濾波算法有兩個基本假設:

( 1) 信息過程的足夠精確的模型，是由白噪聲所激發的線性( 也可以是時變的) 動態系統;

( 2) 每次的測量信號都包含着附加的白噪聲分量。當滿足以上假設時，可以應用卡爾曼濾波算法。

三、卡爾曼濾波過程及五個基本公式

卡爾曼濾波時間更新（預測）
1. 向前推算狀態變量

$\LARGE \hat{x}_{k}^{-} = A\hat{x}_{k-1} + Bu_{k-1}$

2. 向前推算誤差協方差

$\LARGE P_{k}^{-} = AP_{k-1} A^{T} + Q$

卡爾曼濾波測量更新（校正）
3. 計算卡爾曼增益

$\LARGE K_k = \frac{P_{k}^{-}{H^T}}{HP_{k}^{-}H^T + R}$

4. 由觀測變量 $\large z_k$ 更新估計

$\LARGE \hat{x}_k = \hat{x}_{k}^{-} + K_k(z_k - H\hat{x}_{k}^{-})$

5. 更新測量誤差

$\LARGE P_k = (I - K_kH)P_{k}^{-}$

四、公式中每個參數詳細註釋

$\LARGE \hat{x}_{k-1}$ 和 $\LARGE \hat{x}_{k}$ ：分別表示 $\LARGE k-1$ 時刻和 $\LARGE k$ 時刻的後驗狀態估計值，是濾波的結果之一，即更新後的結果，也叫最優估計（估計的狀態，根據理論，我們不可能知道每時刻狀態的確切結果所以叫估計）。
$\LARGE \hat{x}_{k}^{-}$ ： $\LARGE k$ 時刻的先驗狀態估計值，是濾波的中間計算結果，即根據上一時刻（ $\LARGE k-1$ 時刻）的最優估計預測的 $\LARGE k$ 時刻的結果，是預測方程的結果。
$\LARGE P_{k-1}$ 和 $\LARGE P_k$ ：分別表示 k － 1 時刻和 k 時刻的後驗估計協方差（即 $\LARGE \hat{x}_{k-1}$ 和 $\LARGE \hat{x}_k$ 的協方差，表示狀態的不確定度），是濾波的結果之一。
$\LARGE P_{k}^{-}$ ： k 時刻的先驗估計協方差（ $\LARGE \hat{x}_{k}^{-}$ 的協方差），是濾波的中間計算結果。
$\LARGE H$ ：是狀態變量到測量（觀測）的轉換矩陣，表示將狀態和觀測連接起來的關係，卡爾曼濾波里爲線性關係，它負責將 m 維的測量值轉換到 n 維，使之符合狀態變量的數學形式，是濾波的前提條件之一。
$\LARGE z_k$ ：測量值（觀測值），是濾波的輸入。
$\LARGE K_k$ ：濾波增益矩陣，是濾波的中間計算結果，卡爾曼增益，或卡爾曼係數。
$\LARGE A$ ：狀態轉移矩陣，實際上是對目標狀態轉換的一種猜想模型。例如在機動目標跟蹤中，狀態轉移矩陣常常用來對目標的運動建模，其模型可能爲勻速直線運動或者勻加速運動。當狀態轉移矩陣不符合目標的狀態轉換模型時，濾波會很快發散。
$\LARGE Q$ ：過程激勵噪聲協方差（系統過程的協方差）。該參數被用來表示狀態轉換矩陣與實際過程之間的誤差。因爲我們無法直接觀測到過程信號，所以 Q 的取值是很難確定的。是卡爾曼濾波器用於估計離散時間過程的狀態變量，也叫預測模型本身帶來的噪聲。狀態轉移協方差矩陣。
$\LARGE R$ ：測量噪聲協方差。濾波器實際實現時，測量噪聲協方差 R一般可以觀測得到，是濾波器的已知條件。
$\LARGE B$ ：是將輸入轉換爲狀態的矩陣。
$\LARGE (z_k - H\hat{x}_{k}^{-})$ ：實際觀測和預測觀測的殘差，和卡爾曼增益一起修正先驗（預測），得到後驗。

五、結合rssi濾波實例設計濾波器

1、建立模型系統方程和量測方程

由於分析對象是無線信號的一維rssi狀態，所以具體空間過程不關心，只需要從發射端發射到接收端接收是沒有其他控制狀態的，但是在傳輸過程中是存在噪聲的，根據公式

$\LARGE {\color{DarkBlue} }x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}$

可得：A 爲[1]，B爲[0]， $\large w_{k-1}$ 爲高斯白噪聲可不關心

2、建立量測方程

由於接收設備可直接輸出rssi值，根據公式

$\LARGE z_k = Hx_k + v_k$

可得：H爲[1]， $\large v_k$ 爲量測噪聲可不關心

3、分析Q和R

假如 $\large w$ 已經分析出一系列的數據，則 $\large Q = cov(w)$ ;

假如 $\large v$ 已經分析出一些列的數據，則 $\large R = cov(v)$ ;

4、初始值確定

給濾波過程的初始狀態初始化。

5、最後一步，就是對照公式根據理解，套公式，寫程序。

六、MATLAB實現濾波器

1、濾波器設計

function z = kalmanFilter(x)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  function z = kalmanFilter(x)
%
%>
%> @brief 一維卡爾曼濾波
%>
%> @param[out]  z             濾波後的結果
%> @param[in]   x             需要濾波的數據
%>
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % 卡爾曼相關變量定義
    persistent xk xk_1;             % 狀態量
    persistent zk;                  % 觀測量
    persistent A;                   % 狀態轉移矩陣
%     persistent B;                   % 控制輸入模型
    persistent H;                   % 觀測矩陣
    persistent Pk Pk_1;             % 誤差協方差矩陣
    persistent Q;                   % 狀態噪聲協方差矩陣
    persistent R;                   % 觀測噪聲協方差矩陣
    
    % 卡爾曼相關參數初始化
    if isempty(xk)
        A = 1;
        H = 1;
        Pk = 1;
        Pk_1 = 1;                   % 初始誤差協方差爲1
        Q = 0.01;                   % 反應兩個時刻rssi方差
        R = 0.05;                   % 反應測量rssi的測量精度
        xk = 0;
        xk_1 = 0;
        zk = 0;
    end
    
    I = 1;
    if xk_1 == 0
        xk_1 = x;
        xk = x;
    else
        zk = H*x;                   % 觀測量方程
        % 預測
        X = A*xk_1;                 % 狀態預測
        P = A*Pk_1*A' + Q;          % 誤差協方差預測
        % 更新(校正)
        K = P*H'*inv(H*P*H'+R);     % 卡爾曼增益更新
        xk = X + K*(zk - H*X);      % 更新校正
        xk_1 = xk;                  % 保存校正後的值，下一次濾波使用
        Pk = (I - K*H)*P;           % 更新誤差協方差
        Pk_1 = Pk;                  % 保存校正後的誤差協方差，下一次濾波使用
    end
    
    % 濾波結果返回
    z = xk;
end

2、仿真運行

%% 1. 導入數據
[fname, pname] = uigetfile('*', 'Sample Dialog Box');
fileID = fopen(strcat(pname, fname));
data = cell2mat(textscan(fileID,'%f%f','delimiter', ',','headerlines',0));
rssi = data(:, 1);

%% 2. 卡爾曼濾波
rssi_opt = zeros(size(rssi,1), 1);
for k = 1:size(rssi,1)
    rssi_opt(k) = kalmanFilter(rssi(k));
end

%% 3. 濾波檢驗
figure(1);
plot(rssi, 'Color', 'r', 'Marker', 'o'); hold on;
plot(rssi_opt, 'Color', 'b', 'LineStyle', '-', 'Marker', '+'); hold off;
legend('rssi原始波形', 'rssi經過濾波後的波形');
title('rssi濾波對比');

3、運行結果

卡爾曼濾波詳解：一維卡爾曼濾波實例解析(五個公式以及各個參數的意義)