[Matlab]卡爾曼濾波器設計

1、卡爾曼濾波器

卡爾曼濾波（Kalman filtering）是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。由於觀測數據中包括系統中的噪聲和干擾的影響，所以最優估計也可看作是濾波過程。

卡爾曼在NASA埃姆斯研究中心訪問時，發現他的方法對於解決阿波羅計劃的軌道預測很有用，後來阿波羅飛船的導航電腦便使用了這種濾波器。關於這種濾波器的論文最早由Swerling(1958)發表了這種想法。

數據濾波是去除噪聲還原真實數據的一種數據處理技術，Kalman濾波在測量方差已知的情況下能夠從一系列存在測量噪聲的數據中，估計動態系統的狀態。由於它便於計算機編程實現，並能夠對現場採集的數據進行實時的更新和處理，Kalman濾波是目前應用最爲廣泛的濾波方法，在通信，導航，制導與控制等多領域得到了較好的應用。

2、卡爾曼濾波器性質

①卡爾曼濾波是一個算法，它適用於線性、離散和有限維繫統。每一個有外部變量的自迴歸移動平均系統(ARMAX)或可用有理傳遞函數表示的系統都可以轉換成用狀態空間表示的系統，從而能用卡爾曼濾波進行計算。

②任何一組觀測數據都無助於消除x(t)的確定性。增益K(t)也同樣地與觀測數據無關。

③當觀測數據和狀態聯合服從高斯分佈時用卡爾曼遞歸公式計算得到的是高斯隨機變量的條件均值和條件方差，從而卡爾曼濾波公式給出了計算狀態的條件概率密度的更新過程線性最小方差估計，也就是最小方差估計。

3、基本動態模型

前提條件：

假設1：k時刻的真實狀態是從 $k-1$ 時刻的真實狀態演化而來；
假設2：演化與測量的過程由線性算子來描述。

3.1兩個基本的方程

四個狀態值的定義：
$\widehat{x}_{k-1|k-1}$ :表示 $k-1$ 時刻的估計狀態
${x}_{k-1}$ : 表示 $k-1$ 時刻的真實狀態
$\widehat{x}_{k|k-1}$ :表示 $k-1$ 時刻的估計狀態預測k時刻估計的預測值
$\widehat{x}_{k|k}$ :表示 $k$ 時刻的估計狀態
${x}_{k}$ : 表示 $k$ 時刻的真實狀態
狀態轉移方程：
$x_k = F_kx_{k-1} + B_ku_k + w_k$

$F_k$ :作用在 $k-1$ 時刻狀態 $x_{k-1}$ 上的變換矩陣；
$B_k$ :作用在控制器向量 $u_k$ 上的控制矩陣；
$w_k$ :過程噪聲，並假定；均值爲零，協方差矩陣爲 $Q_k$ 的獨立多元正態分佈，記作： $w_k~N(0,Q_k).Q_k = cov(w_k)$

狀態測量方程：

$z_k = H_kx_k + v_k$ $(2)^*$

$H_k$ :觀測矩陣，作用是將隱含的真實狀態空間映射到觀測空間；
$v_k$ :觀測噪聲，並假定；均值爲零，協方差矩陣爲 $R_k$ 的獨立多元正態分佈，記作： $v_k~N(0,R_k).R_k = cov(v_k)$

基於 $k-1$ 時刻狀態對 $k$ 時刻狀態的估計值與真實值之間的差稱爲估計誤差，該估計誤差的協方差矩陣的定義爲後驗估計誤差協方差矩陣，用下式表示：

$P_{k|k-1} = cov(x_k- \widehat{x}_{k|k-1})$ $(3)^*$

其模型拓撲結構用隱馬爾科夫鏈可以表示成圖1（維基百科:卡爾曼濾波）這樣：

$[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-24SVj4iw-1575739458352)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\隱馬爾科夫鏈0.png)]$

圖1 卡爾曼濾波的隱馬爾可夫鏈式模型

3.2兩個基本的過程

(1)預測過程

第一步： $k-1$ 時刻的估計狀態預測k時刻估計的預測值

$\widehat{x}_{k|k-1} = F_k\widehat{x}_{k-1|k-1} + B_ku_k$ (1)

注意：(1)與 $(1)^*$ 的差別，預測是理想值，不能引入不可控的隨機噪音。

第二步：計算後驗估計誤差協方差矩陣用來度量預測精度

$P_{k|k-1} = F_k{P}_{k-1|k-1} F_k^{T} +Q_k$ (2)

公式推導：

$P_{k|k-1} = cov(F_kx_{k-1} + B_ku_k + w_k - F_k\widehat{x}_{k-1|k-1} - B_ku_k )$

$=cov(F_k(x_{k-1}- \widehat{x}_{k-1|k-1} ) + w_k )$

$=F_kcov((x_{k-1}- \widehat{x}_{k-1|k-1} )F_k^{T} + COV(w_k) )$

$=F_k{P}_{k-1|k-1} F_k^{T} +Q_k$

(2) 更新過程

我們基於 $k-1$ 時刻對k時刻狀態的估計是否正確，需要用與實際測量值之間的誤差來衡量，並且考慮用這個誤差來補償。所以在更新之前，我們應該計算實際測量值與估計輸出值之間的差值及其協方差矩陣。

$\widetilde{y}_k = z_k + H_k\widehat{x}_{k|k-1}$ (3)

注意：此處代入方程與 $(2)^*$ 式的區別。

（3）式的協方差矩陣表示爲：

$S_k = H_k{P}_{k|k-1} H_k^{T} +R_k$ (4)

下面推導公式（4）：

$S_k = cov(\widetilde{y}_k) = cov(z_k + H_k\widehat{x}_{k|k-1})=cov(H_kx_k + v_k + H_k\widehat{x}_{k|k-1})$

$=cov(H_k(x_k-\widehat{x}_{k|k-1}) )+cov(v_k) =H_kcov((x_{k}- \widehat{x}_{k|k-1} )H_k^{T} + R_k )$

$= H_k{P}_{k|k-1} H_k^{T} +R_k$

然後我們再進行更新的步驟。更新是指：由基於k-1時刻對k時刻狀態的估計值應當如何得到k時刻的估計值。卡爾曼的思想就是：用基於k-1時刻對k時刻狀態的估計值與預測輸出值和實際輸出值之間的差進行線性組合得到k時刻的估計值，連接這兩者的就是卡爾曼增益。這裏體現的就是反饋的思想，更新過程的第一步用下式表示：

第一步： $k$ 時刻的估計狀態 $\widehat{x}_{k|k}$ 由 $\widehat{x}_{k|k-1}$ 和 $\widetilde{y}_k$ 線性組合而來：

$\widehat{x}_{k|k} =\widehat{x}_{k|k-1}+K_k \widetilde{y}_k$ (5)

其中： $K_k$ 爲卡爾曼增益。

那麼現在問題來了，如何求取這個卡爾曼增益呢？
這時候我們應該回到我們的出發點，我們希望的是濾除干擾真實狀態的噪聲，是濾波器的估計狀態與真實狀態最爲接近。最爲接近可以理解爲k時刻的真實狀態與k時刻的估計狀態之間的誤差二範平方和最小，也就等價於協方差矩陣的跡最小。可以表示爲：

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-z7rOsR1s-1575739458354)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\KALMAN.png)]

在這種情況下求取的卡爾曼增益稱爲最優卡爾曼增益。求取的過程就是直接上式對卡爾曼增益求一階導數。

第二步，計算卡爾曼增益：

$K_k = {P}_{k|k-1} H_k^{T} S_k^{-1}$ (6)

下面給出推導過程：

$[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-AiiXJ5WL-1575739458354)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\KKKKK3.png)]$

$[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-MrFKY8BJ-1575739458355)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\KKKKK4.png)]$

最優卡爾曼增益計算出來之後，我們發現在最優卡爾曼增益情況下可以對後驗誤差協方差矩陣進行簡化。第三步就是計算在最優卡爾曼增益下的後驗誤差協方差矩陣：

第三步:計算在最優卡爾曼增益 $K_k$ 下簡化後驗預測誤差協方差矩陣：

$P_{k|k} = (I-K_kH_k)P_{k|k-1}$ (7)

推導過程：

$P_{k|k} = P_{k|k-1} -K_kH_kP_{k|k-1}-P_{k|k-1}H_k^T K_k^T +K_k^T S_kK_k^T$

$= (I-K_kH_k)P_{k|k-1} -P_{k|k-1}H_k^T K_k^T +P_{k|k-1}H_k^T K_k^T$

$= (I-K_kH_k)P_{k|k-1}$

4、綜上所述:

通過上面的爾曼在濾波器的推導過程分析可得：

(1)預測過程

第一步： $\widehat{x}_{k|k-1} = F_k\widehat{x}_{k-1|k-1} + B_ku_k$ (1)

第二步： $P_{k|k-1} = F_k{P}_{k-1|k-1} F_k^{T} +Q_k$ (2)

$\widetilde{y}_k = z_k + H_k\widehat{x}_{k|k-1}$ (3)

$S_k = H_k{P}_{k|k-1} H_k^{T} +R_k$ (4)

(2) 更新過程

第一步： $\widehat{x}_{k|k} =\widehat{x}_{k|k-1}+K_k \widetilde{y}_k$ (5)

第二步： $K_k = {P}_{k|k-1} H_k^{T} S_k^{-1}$ (6)

第三步： $P_{k|k} = (I-K_kH_k)P_{k|k-1}$ (7)

將這些方程做成框圖：

$[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-pinmkUTJ-1575739458356)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\kalman444.png)]$

從中可以看到，只要對初始狀態進行設定，卡爾曼濾波器就可以完成迭代了。

5、卡爾曼濾波器程序設計：

(1).溫度測量

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
% 程序說明：Kalman濾波用於溫度測量的實例(一維）    
function main  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%初始化參數
N=120;  %採樣點個數
A=1;  
B=1;  
H=1; 
Q=0.01;  
R=0.25;  
W=sqrt(Q)*randn(1,N);  
V=sqrt(R)*randn(1,N);  
CON=25;  %溫度真實值
%分配空間
X=zeros(1,N);    
Z=zeros(1,N); 
Xkf=zeros(1,N);  
P=zeros(1,N);   
X(1)=25.1;  
P(1)=0.01;  
Z(1)=24.9;  
Xkf(1)=Z(1);  
I=eye(1);   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%kalman過程
for k=2:N  
    X(k)=A*X(k-1)+B*W(k);   
    Z(k)=H*X(k)+V(k);  
    X_pre(k)=A*Xkf(k-1);             
    P_pre(k)=A*P(k-1)*A'+Q;          
    Kg=P_pre(k)/(H*P_pre(k)*H'+R);             
    Xkf(k)=X_pre(k)+Kg*(Z(k)-H*X_pre(k));           
    P(k)=(I-Kg*H)*P_pre(k);  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%誤差過程
Err_Messure=zeros(1,N);  
Err_Kalman=zeros(1,N);  
for k=1:N  
    Err_Messure(k)=Z(k)-X(k);  
    Err_Kalman(k)=Xkf(k)-X(k);  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=1:N;  
figure;  
plot(t,CON*ones(1,N),'-b',t,X,'-r',t,Z,'g+',t,Xkf,'-k');  
legend('期望值','真實值','測量值','kalman估計值');           
xlabel('採樣時間');  
ylabel('溫度');  
title('Kalman Filter Simulation');  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;  
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k');  
legend('測量誤差','kalman誤差');           
xlabel('採樣時間');  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

$[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-74GmTYsy-1575739458357)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\kalmen_tem.bmp)]$

$[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-S7AEOW1s-1575739458357)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\kalmen_tem2.bmp)]$

(2).自由落體追蹤

%kalman濾波用於自由落體運動目標追蹤（二維，x,v)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%初始化參數
N=1000;
Q=[0,0;0,0];
R=0.5;
w=sqrt(Q)*randn(2,N);
v=sqrt(R)*randn(1,N);
A=[1,1;0,1];%狀態轉移矩陣
B=[0.5;1];%控制矩陣
U=-10;%控制變量
H=[1,0];%觀測矩陣
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%分配空間，x,p0,z,xkf
x=zeros(2,N);%物體真實狀態
x(:,1)=[95;1];%初始位移和速度
p0=[10,0;0,1];%初始誤差
z=zeros(1,N);%觀測值
z(1)=H*x(:,1);%觀測真實值,第一列的列向量，
xkf=zeros(2,N);%kalman估計狀態
xkf(:,1)=x(:,1);%kalman估計狀態初始化,第一列的列向量，
err_p=zeros(N,2);
err_p(1,1)=p0(1,1);
err_p(1,2)=p0(2,2);
I=eye(2);%2*2單位矩陣表明二維繫統
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%kalman迭代過程
for k=2:N;
    x(:,k)=A*x(:,k-1)+B*U+w(k);%模型
    z(k)=H*x(:,k)+v(k);%模型
    x_pre=A*xkf(:,k-1)+B*U;%①
    p_pre=A*p0*A'+Q;%②
    kg=p_pre*H'/(H*p_pre*H'+R);%③
    xkf(:,k)=x_pre+kg*(z(k)-H*x_pre);%④
    p0=(I-kg*H)*p_pre;%⑤
    err_p(k,1)=p0(1,1);%位移誤差均方值
    err_p(k,2)=p0(2,2);%速度誤差均方值
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%誤差計算
messure_err_x=zeros(1,N);
kalman_err_x=zeros(1,N);
kalman_err_v=zeros(1,N);
for k=1:N
    messure_err_x(k)=z(k)-x(1,k);%位移的測量誤差
    kalman_err_x(k)=xkf(1,k)-x(1,k);%kalman估計位移與真實位移之間偏差
    kalman_err_v(k)=xkf(2,k)-x(2,k);%kalman估計速度與真實速度之間偏差
end
%噪聲圖
figure;
plot(w);xlabel('採樣時間');ylabel('過程噪聲');
figure;
plot(v);xlabel('採樣時間');ylabel('測量噪聲');
%位置誤差
figure;
hold on,box on;
plot(messure_err_x,'-r.');
plot(kalman_err_x,'-g.');
legend('測量位置','kalman估計位置');
xlabel('採樣時間');ylabel('位置偏差');
%kalman速度偏差
figure;
plot(kalman_err_v);
xlabel('採樣時間');ylabel('速度偏差');
%均方值
figure;
plot(err_p(:,1));
xlabel('採樣時間');ylabel('位移誤差均方值');
figure;
plot(err_p(:,1));
xlabel('採樣時間');ylabel('速度誤差均方值');

$[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-O2MgvHAS-1575739458359)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\KALMAN_KK.bmp)]$

$[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-mb2uJlrA-1575739458360)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\KALMAN_QQQ.bmp)]$

(3).GPS導航定位

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
%  Kalman濾波在船舶GPS導航定位系統中的應用(四維，x方向x,v   ,y方向x,v; ） 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
function main  
T=1;  %採樣週期
N=80/T;  %總的採樣次數
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];  
H=[1,0,0,0;0,0,1,0];  
Q=4e-4  ;
R=100*eye(2);
X=zeros(4,N);  
X(:,1)=[100,2,200,20]; %x四維，初始位置（-100,200）水平速度2，垂直速度20 
Z=zeros(2,N);  
Z(:,1)=[X(1,1),X(3,1)];  %z只觀測到位置，沒有速度
Xkf=zeros(4,N);  
Xkf(:,1)=X(:,1);  
P0=eye(4);  %協方差矩陣初始化，4*4單位矩陣
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
for t=2:N  %模型
    X(:,t)=F*X(:,t-1)+sqrt(Q)*randn(4,1);%目標真實軌跡  randn(4,1)4*1隨機矩陣
    Z(:,t)=H*X(:,t)+sqrt(R)*randn(2,1); %觀測軌跡randn(2,1)2*1隨機矩陣
end  
for i=2:N  
    X_pre=F*Xkf(:,i-1);  
    P_pre=F*P0*F'+Q;  
    K=P_pre*H'/(H*P_pre*H'+R);  
    Xkf(:,i)=X_pre+K*(Z(:,i)-H*X_pre);  
    P0=(eye(4)-K*H)*P_pre;  
end  
for i=1:N  
    Err_Observation(i)=RMS(X(:,i),Z(:,i));  
    Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;  
hold on;box on;  
plot(X(1,:),X(3,:),'-k');  %位置軌跡
plot(Z(1,:),Z(2,:),'-b.');  %觀測軌跡
plot(Xkf(1,:),Xkf(3,:),'-r+');  %Kalman估計軌跡
legend('真實軌跡','觀測軌跡','濾波軌跡')  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;  
hold on; box on;  
plot(Err_Observation,'-ko','MarkerFace','g')  
plot(Err_KalmanFilter,'-ks','MarkerFace','r')  
legend('濾波前誤差','濾波後誤差')  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
function dist=RMS(X1,X2);  
if length(X2)<=2  
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );  
else  
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

$[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-9nbcgeev-1575739458360)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\KALMAN_GPS.bmp)]$

$[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-eUOQCbaw-1575739458362)(G:\研究生\項目小組任務\筆記\第四周和第五週筆記\KALMAN_GPS_EN.bmp)]$

[Matlab]卡爾曼濾波器設計