基於MATLAB的卡爾曼濾波算法實現

以下為純小白對卡爾曼濾波算法的MATLAB實現進行簡單總結，以便以后復習。

一、 背景介紹

假設一小車作勻加速運動，初速度為0，加速度為5米每二次方秒，小車上裝有速度傳感器，采樣頻率為10Hz，傳感器測量噪聲為高斯白噪聲，需要充分利用這些信息來估計車輛的速度狀態，並驗證卡爾曼濾波算法的實驗原理與過程。

二、卡爾曼濾波原理

早在近百年前，就有人開始采用狀態變量模型研究隨機過程，隨后為了解決對非平穩、多輸入輸出隨機序列的估計問題，卡爾曼提出了遞推最優估計理論。它采用狀態空間法描述系統，由狀態方程和測量方程組成，即知道前一個狀態的估計值和最近一個觀測數據，采用遞推算法估計當前的狀態值。其具有以下特點：
（1） 算法是一個遞推過程，且狀態空間法采用在時域內設計濾波器的方法，因而適用於多維隨機過程的估計；離散型卡爾曼濾波算法適用於計算機處理。
（2） 采用遞推算法計算，不用知道所有過去的值，用狀態方程描述狀態變量的動態變化規律，卡爾曼濾波同樣適用於非平穩過程。
如圖所示，為卡爾曼濾波算法的實現流程圖，其基本思路是若有一組強而合理的假設，給出系統的歷史測量值，可以建立最大化這些早前測量值的后驗概率的系統狀態模型。其基本假設是被建系統是線性的，影響測量的噪聲屬於符合高斯分布的白噪聲。

三、卡爾曼濾波的狀態方程和測量方程

整個實現過程包含預測更新和測量更新兩大部分。
（1） 預測更新：
預測狀態量：

預測誤差協方差矩陣：

（2） 測量更新：
最優估計狀態量：

計算誤差增益：

誤差協方差矩陣：

設k時刻小車速度為 ，則系統狀態方程為：

測量方程為：

結合卡爾曼濾波算法的預測和測量更新流程，可有：

四、結果分析

根據以上理論分析，完成相應matlab程序（代碼見后），並畫出小車速度的觀測值、真實值、濾波值的對比曲線圖如下：

五、代碼

(1) kalman.m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %日 期： 2020.3.10
 %程序功能：使用卡爾曼濾波器估計小車勻加速運動時的速度狀態
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
clc
clear all
A = [1];
B = [1];
U = 0.5;      %A,B,U初始值來自於狀態方程
H = [1];       %H來自於測量方程
step = 500;
v = normrnd(0,10,1,step); %均值為0，方差為10噪聲
X_0 = zeros(1,step);         %先驗估計初始值
X0 = zeros(1,step);           %后驗估計初始值
Z = zeros(1,step);             %觀測值初始值
true = 0.5*(1:step);           %小車真實速度
temp = true + v(1,:);
Z(1,:) = temp;    %加入噪聲的速度，模擬觀測值
Q = [10];           %狀態方程噪聲協方差矩陣
R = [500];         %測量方程協方差矩陣
P0 = [1];           %初始后驗估計協方差
P_0 = [1];
for i = 2:step
    [X_0(i),P_0] = kalmanPredictor(A,B,U,P0,Q,X0(i-1));
    [X0(i),P0] = kalmanUpdater(H,R,P_0,Z(i),X_0(i));
end
figure;
plot(Z,'g.');
hold on;
plot(true,'r.');
hold on;
plot(X0,'b');
grid on;
legend('觀測值','真實值','kalman濾波');

(2)kalmanPredictor.m

function [Xp,Pp] = kalmanPredictor(A,B,U,P0,Q,X0)
Xp = A*X0 + B*U;
Pp = A*P0*A' + Q;
end

(3)kalmanUpdater.m

function [Xup,Pup] = kalmanUpdater(H,R,Pp,Z,Xp)
I = eye(size(Xp,1));
K = (Pp*H')/(H*Pp*H' + R);
Xup = Xp + K*(Z-H*Xp);
Pup = (I-K*H)*Pp;
end