【Matlab】卡爾曼濾波器的程序實現（kalman、Matlab）

Matlab——卡爾曼濾波器（matlab實現）

---

一、原理：看了網上很多資料，這兩個講的比較好。

1.高中生能看懂的詳細通俗講解卡爾曼濾波Kalman Filter原理及Python實現教程 https://blog.csdn.net/varyshare/article/details/95065650
2. 圖像處理之目標跟蹤(一)之卡爾曼kalman濾波跟蹤(主要爲知識梳理)(轉載)https://blog.csdn.net/coming_is_winter/article/details/79048700

---

二、方程概述：

1、狀態預測方程與不確定性的傳遞

1、卡爾曼濾波方程組中的第一條公式，狀態預測公式，而F就是狀態轉移矩陣，它表示我們如何從上一狀態來推測當前狀態。而B則是控制矩陣，它表示控制量u如何作用於當前狀態。
2、卡爾曼濾波方程組中的第二條公式，它表示不確定性在各個時刻間的傳遞關係。

3、輸出變量都是輸入變量的線性組合，這也就是稱卡爾曼濾波器爲線性濾波器的原因所在。
4、卡爾曼濾波算法是一種基於最小均方誤差的最優線性遞歸濾波方法，以狀態方程和觀測方程爲基礎，運用遞歸方法來預測線性系統變化。

• F：狀態轉移矩陣，它表示我們如何從上一狀態來推測當前狀態。
• B：控制矩陣，它表示控制量u如何作用於當前狀態。

---

2、協方差矩陣 Σ -

• : 系統中每一個時刻的不確定性都是通過協方差矩陣 Σ 來給出的。

  • 這種不確定性在每個時刻間還會進行傳遞。
  • 不僅當前物體的狀態（例如位置或者速度）是會（在每個時刻間）進行傳遞的，而且物體狀態的不確定性也是會（在每個時刻間）進行傳遞的。
  • 這種不確定性的傳遞就可以用狀態轉移矩陣來表示

• Q： 協方差矩陣 Q 來表示預測模型本身的噪聲（也即是噪聲在傳遞過程中的不確定性）

---

3、卡爾曼係數K(t)

• K：稱爲卡爾曼係數，它也是一個矩陣，它是對殘差的加權矩陣，有的資料上稱其爲濾波增益陣。

• K首先權衡預測狀態協方差矩陣Σ和觀測值矩陣R的大小，並以此來覺得我們是更傾向於相信預測模型還是詳細觀測模型。

  • 如果相信預測模型多一點，那麼這個殘差的權重就會小一點。
  • 如果相信觀察模型多一點，這個殘差的權重就會大一點。

• K（濾波增益陣）還負責把殘差的表現形式從觀測域轉換到了狀態域。

  • 觀測值與狀態值所採用的描述特徵或者單位都有可能不同，顯然直接用觀測值的殘差去更新狀態值是不合理的。

• R（協方差矩陣）來表示觀測中的不確定性。

• H:真實狀態到其觀測狀態的一個變換關係，這個變換關係我們記爲h(•)，而且這個h(•)還是一個線性函數，可記爲矩陣變換H（）。

---

4、對估計值進行修正

• 是根據上一狀態推測而來的，那麼它與“最優”估計值之間的差距現在就是等式右端加號右側的部分。
• 表示實際觀察值與預估的觀測值之間的殘差。這個殘差再乘以一個係數K就可以用來對估計值進行修正。K稱爲卡爾曼係數。

---

5、噪聲分佈 Σ 進行更新。

• 對最優估計值的噪聲分佈進行更新。
• 我們將這五個公式分成預測組和更新組。預測組總是根據前一個狀態來估計當前狀態。更新組則根據觀測信息來對預測信息進行修正，以期達到最優估計之目的。
  -----
• 我們將這五個公式分成預測組和更新組。預測組總是根據前一個狀態來估計當前狀態。更新組則根據觀測信息來對預測信息進行修正，以期達到最優估計之目的。

---

三、Matlab程序

%kalman濾波器
%阿汪先生的博客.ws
clc          %清除命令行
clear        %清空變量
N=200;    	 %數據個數N
w(1)=0;      %w爲預測模型的過程噪聲  
w=randn(1,N) %randn()創建隨機矩陣，服從正態分佈
x(1)=0;    	 %初始值
f=1;         %a爲方程1中的狀態轉移矩陣，這裏爲一維變量

%x的預測值
for k=2:N;    	
x(k)=f*x(k-1)+w(k-1);     %方程1，爲x的推測值
end   

%實際觀察值Y
H=0.4;          %H爲方程中H(k)  
y=H*x+V;        %Y爲實際觀察值  

%預測不確定性
q2=std(w);      %返回一個包含與每列對應的標準差的行向量，預測模型
Q=q2.^2;    	% Q 預測模型本身的噪聲（協方差矩陣）

%觀察不確定性
V=randn(1,N);    %V爲觀察噪聲，randn()創建隨機矩陣，服從正態分佈
q1=std(V);    	 %返回一個包含與每列對應的標準差的行向量
R=q1.^2;    	% R 來表示觀測中的不確定性（協方差矩陣）

%x的修正值（經卡爾曼濾波的值）
p(1)=0;      %協方差矩陣 Σ初值，代表一個時刻的不確定性
s(1)=0;        %修正值初值
for t=2:N;    
p1(t)=f.^2*p(t-1)+Q;		 %方程2，p1(t)協方差矩陣 Σ-，由上一狀態推測而來
k(t)=H*p1(t)/(H.^2*p1(t)+R);    	%方程3 ，k(t)爲卡爾曼係數
s(t)=f*s(t-1)+k(t)*(y(t)-f*H*s(t-1)); %方程4，根據估計值進行修正
p(t)=p1(t)-H*k(t)*p1(t);    		%方程5，p(t)爲協方差矩陣 Σ，對最優值的噪聲分佈進行更新
end

%畫曲線圖
t=1:N;    
plot(t,s,'r',t,y,'g',t,x,'b');
%紅色曲線：x的修正值
%綠色曲線，x的實際觀察值
%藍色曲線，x的預測值

程序效果