轉自：https://blog.csdn.net/zhangquan2015/article/details/79264540

原

卡爾曼濾波應用及其matlab實現

2018年02月11日 20:32:18 Joeyos 閱讀數 21935 版權聲明：本文為博主原創文章，遵循 CC 4.0 by-sa 版權協議，轉載請附上原文出處鏈接和本聲明。 本文鏈接：https://blog.csdn.net/zhangquan2015/article/details/79264540

Github個人博客：https://joeyos.github.io

線性卡爾曼濾波

卡爾曼濾波在溫度測量中的應用

X(k)=AX(k-1)+TW(k-1)
Z(k)=H*X(k)+V(k)

房間溫度在25攝氏度左右，測量誤差為正負0.5攝氏度，方差0.25，R=0.25。Q=0.01,A=1,T=1,H=1。

假定快時刻的溫度值、測量值為23.9攝氏度，房間真實溫度為24攝氏度，溫度計在該時刻測量值為24.5攝氏度，偏差為0.4攝氏度。利用k-1時刻溫度值測量第k時刻的溫度，其預計偏差為:
P(k|k-1)=P(k-1)+Q=0.02
卡爾曼增益k=P(k|k-1) / (P(k|k-1) + R)=0.0741

X(k)=23.9+0.0741*(24.1-23.9)=23.915攝氏度。

k時刻的偏差為P(k)=(1-K*H)P(k|k-1)=0.0186。
最后由X(k)和P(k)得出Z(k+1)。

matlab實現為：

% 程序說明：Kalman濾波用于溫度測量的實例 function main N=120; CON=25;%房間溫度在25攝氏度左右 Xexpect=CON*ones(1,N); X=zeros(1,N); Xkf=zeros(1,N); Z=zeros(1,N); P=zeros(1,N); X(1)=25.1; P(1)=0.01; Z(1)=24.9; Xkf(1)=Z(1); Q=0.01;%W(k)的方差 R=0.25;%V(k)的方差 W=sqrt(Q)*randn(1,N); V=sqrt(R)*randn(1,N); F=1; G=1; H=1; I=eye(1); %%%%%%%%%%%%%%%%%%%%%%% for k=2:NX(k)=F*X(k-1)+G*W(k-1); Z(k)=H*X(k)+V(k);X_pre=F*Xkf(k-1); P_pre=F*P(k-1)*F'+Q; Kg=P_pre*inv(H*P_pre*H'+R); e=Z(k)-H*X_pre; Xkf(k)=X_pre+Kg*e; P(k)=(I-Kg*H)*P_pre; end %%%%%%%%%%%%%%%% Err_Messure=zeros(1,N); Err_Kalman=zeros(1,N); for k=1:NErr_Messure(k)=abs(Z(k)-X(k));Err_Kalman(k)=abs(Xkf(k)-X(k)); end t=1:N; figure('Name','Kalman Filter Simulation','NumberTitle','off'); plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-k',t,Xkf,'-g'); legend('expected','real','measure','kalman extimate'); xlabel('sample time'); ylabel('temperature'); title('Kalman Filter Simulation'); figure('Name','Error Analysis','NumberTitle','off'); plot(t,Err_Messure,'-b',t,Err_Kalman,'-k'); legend('messure error','kalman error'); xlabel('sample time'); %%%%%%%%%%%%%%%%%%%%%%%%%

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51

卡爾曼濾波在自由落體運動目標跟蹤中的應用

%%%%%%%%%%%%% % 卡爾曼濾波用于自由落體運動目標跟蹤問題 %%%%%%%%%%%%%% function main N=1000; %仿真時間，時間序列總數 %噪聲 Q=[0,0;0,0];%過程噪聲方差為0，即下落過程忽略空氣阻力 R=1; %觀測噪聲方差 W=sqrt(Q)*randn(2,N);%既然Q為0，即W=0 V=sqrt(R)*randn(1,N);%測量噪聲V(k) %系數矩陣 A=[1,1;0,1];%狀態轉移矩陣 B=[0.5;1];%控制量 U=-1; H=[1,0];%觀測矩陣 %初始化 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);%卡爾曼估計狀態初始化 Xkf(:,1)=X(:,1); err_P=zeros(N,2); err_P(1,1)=P0(1,1); err_P(1,2)=P0(2,2); I=eye(2); %二維系統 %%%%%%%%%%%%% 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'*inv(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:Nmessure_err_x(k)=Z(k)-X(1,k);kalman_err_x(k)=Xkf(1,k)-X(1,k);kalman_err_v(k)=Xkf(2,k)-X(2,k); end %%%%%%%%%%%%%%% %畫圖輸出 %噪聲圖 figure plot(V); title('messure noise') %位置偏差 figure hold on,box on; plot(messure_err_x,'-r.');%測量的位移誤差 plot(kalman_err_x,'-g.');%卡爾曼估計位置誤差 legend('測量誤差','kalman估計誤差') figureplot(kalman_err_v); title('速度誤差') figure plot(err_P(:,1)); title('位移誤差均方值') figure plot(err_P(:,1)); title('速度誤差均方值') %%%%%%%%%%%%%%%%%%%%%%%

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56
• 57
• 58
• 59
• 60
• 61
• 62
• 63
• 64
• 65
• 66
• 67
• 68
• 69
• 70
• 71
• 72
• 73

卡爾曼濾波在船舶GPS導航定位

% Kalman濾波在船舶GPS導航定位系統中的應用 function main clc;clear; T=1;%雷達掃描周期 N=80/T;%總的采樣次數 X=zeros(4,N);%目標真實位置、速度 X(:,1)=[-100,2,200,20];%目標初始位置、速度 Z=zeros(2,N);%傳感器對位置的觀測 Z(:,1)=[X(1,1),X(3,1)];%觀測初始化 delta_w=1e-2;%如果增大這個參數，目標真實軌跡就是曲線 Q=delta_w*diag([0.5,1,0.5,1]) ;%過程噪聲均值 R=100*eye(2);%觀測噪聲均值 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];%觀測矩陣 %%%%%%%%%%%%%%%%%%%%%%%%%%%% for t=2:NX(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);%目標真實軌跡Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1); %對目標觀測 end %卡爾曼濾波 Xkf=zeros(4,N); Xkf(:,1)=X(:,1);%卡爾曼濾波狀態初始化 P0=eye(4);%協方差陣初始化 for i=2:NXn=F*Xkf(:,i-1);%預測P1=F*P0*F'+Q;%預測誤差協方差K=P1*H'*inv(H*P1*H'+R);%增益Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%狀態更新P0=(eye(4)-K*H)*P1;%濾波誤差協方差更新 end %誤差更新 for i=1:NErr_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+');%卡爾曼濾波軌跡 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)<=2dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 ); elsedist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 ); end %%%%%%%%%%%%%%%%%%%%%%%%

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53
• 54
• 55
• 56

卡爾曼濾波在視頻目標跟蹤中的應用

幀間差法目標檢測

% 目標檢測函數，這個函數主要完成將目標從背景中提取出來 function detect clear,clc; %計算背景圖片數目 Imzero = zeros(240,320,3); for i = 1:5%將圖像文件讀入矩陣ImIm{i} = double(imread(['DATA/',int2str(i),'.jpg']));Imzero = Im{i}+Imzero; end Imback = Imzero/5; [MR,MC,Dim] = size(Imback); %遍歷所有圖片 for i = 1 : 60%讀取所有幀Im = (imread(['DATA/',int2str(i), '.jpg']));imshow(Im); %顯示圖像Im,圖像對比度低Imwork = double(Im);%檢測目標[cc(i),cr(i),radius,flag] = extractball(Imwork,Imback,i);if flag==0 %沒檢測到目標，繼續下一幀圖像continueendhold onfor c = -0.9*radius: radius/20 : 0.9*radiusr = sqrt(radius^2-c^2);plot(cc(i)+c,cr(i)+r,'g.')plot(cc(i)+c,cr(i)-r,'g.')endpause(0.02) end %目標中心的位置，也就是目標的x，y坐標 figure plot(cr,'-g*') hold on plot(cc,'-r*')

總結

以上是生活随笔為你收集整理的卡尔曼滤波matlab程序实现的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。