轉(zhuǎn)自：https://blog.csdn.net/zhangquan2015/article/details/79264540

原

卡爾曼濾波應(yīng)用及其matlab實(shí)現(xiàn)

2018年02月11日 20:32:18 Joeyos 閱讀數(shù) 21935 版權(quán)聲明：本文為博主原創(chuàng)文章，遵循 CC 4.0 by-sa 版權(quán)協(xié)議，轉(zhuǎn)載請(qǐng)附上原文出處鏈接和本聲明。 本文鏈接：https://blog.csdn.net/zhangquan2015/article/details/79264540

Github個(gè)人博客：https://joeyos.github.io

線(xiàn)性卡爾曼濾波

卡爾曼濾波在溫度測(cè)量中的應(yīng)用

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

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

假定快時(shí)刻的溫度值、測(cè)量值為23.9攝氏度，房間真實(shí)溫度為24攝氏度，溫度計(jì)在該時(shí)刻測(cè)量值為24.5攝氏度，偏差為0.4攝氏度。利用k-1時(shí)刻溫度值測(cè)量第k時(shí)刻的溫度，其預(yù)計(jì)偏差為:
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時(shí)刻的偏差為P(k)=(1-K*H)P(k|k-1)=0.0186。
最后由X(k)和P(k)得出Z(k+1)。

matlab實(shí)現(xiàn)為：

% 程序說(shuō)明：Kalman濾波用于溫度測(cè)量的實(shí)例 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

卡爾曼濾波在自由落體運(yùn)動(dòng)目標(biāo)跟蹤中的應(yīng)用

%%%%%%%%%%%%% % 卡爾曼濾波用于自由落體運(yùn)動(dòng)目標(biāo)跟蹤問(wèn)題 %%%%%%%%%%%%%% function main N=1000; %仿真時(shí)間，時(shí)間序列總數(shù) %噪聲 Q=[0,0;0,0];%過(guò)程噪聲方差為0，即下落過(guò)程忽略空氣阻力 R=1; %觀(guān)測(cè)噪聲方差 W=sqrt(Q)*randn(2,N);%既然Q為0，即W=0 V=sqrt(R)*randn(1,N);%測(cè)量噪聲V(k) %系數(shù)矩陣 A=[1,1;0,1];%狀態(tài)轉(zhuǎn)移矩陣 B=[0.5;1];%控制量 U=-1; H=[1,0];%觀(guān)測(cè)矩陣 %初始化 X=zeros(2,N);%物體真實(shí)狀態(tài) X(:,1)=[95;1];%初始位移和速度 P0=[10,0;0,1];%初始誤差 Z=zeros(1,N); Z(1)=H*X(:,1);%初始觀(guān)測(cè)值 Xkf=zeros(2,N);%卡爾曼估計(jì)狀態(tài)初始化 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); %二維系統(tǒng) %%%%%%%%%%%%% for k=2:N%物體下落，受狀態(tài)方程的驅(qū)動(dòng)X(:,k)=A*X(:,k-1)+B*U+W(k);%位移傳感器對(duì)目標(biāo)進(jìn)行觀(guān)測(cè)Z(k)=H*X(:,k)+V(k);%卡爾曼濾波X_pre=A*Xkf(:,k-1)+B*U;%狀態(tài)預(yù)測(cè) P_pre=A*P0*A'+Q;%協(xié)方差預(yù)測(cè)Kg=P_pre*H'*inv(H*P_pre*H'+R);%計(jì)算卡爾曼增益Xkf(:,k)=X_pre+Kg*(Z(k)-H*X_pre);%狀態(tài)更新P0=(I-Kg*H)*P_pre;%方差更新%誤差均方值err_P(k,1)=P0(1,1);err_P(k,2)=P0(2,2); end %誤差計(jì)算 messure_err_x=zeros(1,N);%位移的測(cè)量誤差 kalman_err_x=zeros(1,N);%卡爾曼估計(jì)的位移與真實(shí)位移之間的偏差 kalman_err_v=zeros(1,N);%卡爾曼估計(jì)的速度與真實(shí)速度之間的偏差 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 %%%%%%%%%%%%%%% %畫(huà)圖輸出 %噪聲圖 figure plot(V); title('messure noise') %位置偏差 figure hold on,box on; plot(messure_err_x,'-r.');%測(cè)量的位移誤差 plot(kalman_err_x,'-g.');%卡爾曼估計(jì)位置誤差 legend('測(cè)量誤差','kalman估計(jì)誤差') 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導(dǎo)航定位

% Kalman濾波在船舶GPS導(dǎo)航定位系統(tǒng)中的應(yīng)用 function main clc;clear; T=1;%雷達(dá)掃描周期 N=80/T;%總的采樣次數(shù) X=zeros(4,N);%目標(biāo)真實(shí)位置、速度 X(:,1)=[-100,2,200,20];%目標(biāo)初始位置、速度 Z=zeros(2,N);%傳感器對(duì)位置的觀(guān)測(cè) Z(:,1)=[X(1,1),X(3,1)];%觀(guān)測(cè)初始化 delta_w=1e-2;%如果增大這個(gè)參數(shù)，目標(biāo)真實(shí)軌跡就是曲線(xiàn) Q=delta_w*diag([0.5,1,0.5,1]) ;%過(guò)程噪聲均值 R=100*eye(2);%觀(guān)測(cè)噪聲均值 F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%狀態(tài)轉(zhuǎn)移矩陣 H=[1,0,0,0;0,0,1,0];%觀(guān)測(cè)矩陣 %%%%%%%%%%%%%%%%%%%%%%%%%%%% for t=2:NX(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);%目標(biāo)真實(shí)軌跡Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1); %對(duì)目標(biāo)觀(guān)測(cè) end %卡爾曼濾波 Xkf=zeros(4,N); Xkf(:,1)=X(:,1);%卡爾曼濾波狀態(tài)初始化 P0=eye(4);%協(xié)方差陣初始化 for i=2:NXn=F*Xkf(:,i-1);%預(yù)測(cè)P1=F*P0*F'+Q;%預(yù)測(cè)誤差協(xié)方差K=P1*H'*inv(H*P1*H'+R);%增益Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%狀態(tài)更新P0=(eye(4)-K*H)*P1;%濾波誤差協(xié)方差更新 end %誤差更新 for i=1:NErr_Observation(i)=RMS(X(:,i),Z(:,i));%濾波前的誤差Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));%濾波后的誤差 end %畫(huà)圖 figure hold on;box on; plot(X(1,:),X(3,:),'-k');%真實(shí)軌跡 plot(Z(1,:),Z(2,:),'-b.');%觀(guān)測(cè)軌跡 plot(Xkf(1,:),Xkf(3,:),'-r+');%卡爾曼濾波軌跡 legend('真實(shí)軌跡','觀(guān)測(cè)軌跡','濾波軌跡') figure hold on; box on; plot(Err_Observation,'-ko','MarkerFace','g') plot(Err_KalmanFilter,'-ks','MarkerFace','r') legend('濾波前誤差','濾波后誤差') %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %計(jì)算歐式距離子函數(shù) 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

卡爾曼濾波在視頻目標(biāo)跟蹤中的應(yīng)用

幀間差法目標(biāo)檢測(cè)

% 目標(biāo)檢測(cè)函數(shù)，這個(gè)函數(shù)主要完成將目標(biāo)從背景中提取出來(lái) function detect clear,clc; %計(jì)算背景圖片數(shù)目 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,圖像對(duì)比度低Imwork = double(Im);%檢測(cè)目標(biāo)[cc(i),cr(i),radius,flag] = extractball(Imwork,Imback,i);if flag==0 %沒(méi)檢測(cè)到目標(biāo)，繼續(xù)下一幀圖像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 %目標(biāo)中心的位置，也就是目標(biāo)的x，y坐標(biāo) figure plot(cr,'-g*') hold on plot(cc,'-r*')

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。