转载：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-第一时刻温度值测量第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=CONones(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?randn(1,N); F=1; G=1; H=1; I=eye(1); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for k=2:N X(k)=FX(k-1) GW(k-1); Z(k)=HX(k) V(k); X_pre=FXkf(k-1); P_pre=FP(k-1)F’ Q; Kg=P_preinv(HP_preH’ R); e=Z(k)-HX_pre; Xkf(k)=X_pre Kge; P(k)=(I-Kg*H)*P_pre; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Err_Messure=zeros(1,N); Err_Kalman=zeros(1,N); for k=1:N Err_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?randn(1,N);%测量噪声V(k) %系数矩阵 A=[1，1；0，1] B=[0.五、一、%控制量 U=-1; H=[1，0]；%观察矩阵 %初始化 X=zeros(2,N);%物体的真实状态 X(:,1)=[95；1] P0=[10，0；0，1] Z=zeros(1,N); Z(1)=HX(，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)=AX(:,k-1) BU W(k); %位移传感器观察目标 Z(k)=HX(:,k) V(k); %卡尔曼滤波 X_pre=AXkf(:,k-1) BU;%状态预测 P_pre=AP0A’ Q;%协方差预测 Kg=P_preH’inv(HP_preH’ R);计算卡尔曼增益% Xkf(:,k)=X_pre Kg*(Z(k)-HX_pre);%状态更新 P0=(I-KgH)*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_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=zero(2,N);%传感器对位置的观测 Z(:,1)=[X(1,1),X(3,1)];%观测初始化 delta_w=1e-2;%如果增大这个参数，目标真实轨迹就是曲线 Q=delta_wdiag([0.5,1,0.5,1]) ;%过程噪声均值 R=100eye(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:N X(:,t)=FX(:,t-1)+sqrtm(Q)randn(4,1);%目标真实轨迹 Z(:,t)=HX(:,t)+sqrtm®randn(2,1); %对目标观测 end %卡尔曼滤波 Xkf=zeros(4,N); Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化 P0=eye(4);%协方差阵初始化 for i=2:N Xn=FXkf(:,i-1);%预测 P1=FP0F’+Q;%预测误差协方差 K=P1H’inv(HP1H’+R);%增益 Xkf(:,i)=Xn+K(Z(:,i)-HXn);%状态更新 P0=(eye(4)-KH)P1;%滤波误差协方差更新 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+’);%卡尔曼滤波轨迹 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 %%%%%%%%%%%%%%%%%%%%%%%% 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 %将图像文件读入矩阵Im Im{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 %没检测到目标，继续下一帧图像 continue end hold on for c = -0.9radius: radius/20 : 0.9radius r = sqrt(radius2-c2); plot(cc(i)+c,cr(i)+r,‘g.’) plot(cc(i)+c,cr(i)-r,‘g.’) end pause(0.02) end %目标中心的位置，也就是目标的x，y坐标 figure plot(cr,’-g’) hold on plot(cc,’-r*’)
% 提取目标区域的中心和能包含目标的最大半径 function [cc,cr,radius,flag]=extractball(Imwork,Imback,index) %初始化目标区域中心的坐标，半径 cc = 0; cr = 0; radius = 0; flag = 0; [MR,MC,Dim] = size(Imback); %除去背景，找到最大的不同区域，即目标区域 fore = zeros(MR,MC); %背景相减，得到目标 fore = (abs(Imwork(:,:,1)-Imback(:,:,1)) > 10) … | (abs(Imwork(:,:,2) - Imback(:,:,2)) > 10) … | (abs(Imwork(:,:,3) - Imback(:,:,3)) > 10); %图像腐蚀，除去微小的白噪声点 %bwmorph该函数的功能是：提取二进制图像的轮廓 foremm = bwmorph(fore,‘erode’,2);%“2”为次数 %选取最大的目标 labeled = bwlabel(foremm,4);%黑背景中甄别有多少白块块，4-连通 %labeled是标记矩阵，图像分割后对不同的区域进行不同的标记 stats = regionprops(labeled,[‘basic’]); [N,W] = size(stats); if N < 1 return%一个目标区域也没检测到就返回 end %在N个区域中，冒泡算法排序 id = zeros(N); for i = 1 : N id(i) = i; end for i = 1 : N-1 for j = i+1 : N if stats(i).Area < stats(j).Area tmp = stats(i); stats(i) = stats(j); stats(j) = tmp; tmp = id(i); id(i) = id(j); id(j) = tmp; end end end %确保至少有一个较大的区域（最大区域面积要大于100） if stats(1).Area < 100 return end selected = (labeled==id(1)); %计算最大区域的中心和半径 centroid = stats(1).Centroid; radius = sqrt(stats(1).Area/pi); cc = centroid(1); cr = centroid(2); flag = 1; return 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 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 视频目标跟踪 % kalman滤波用户检测视频中的目标，并对目标跟踪 function kalman_for_vedio_tracking clear,clc % 计算图像背景 Imzero = zeros(240,320,3); for i = 1:5 Im{i} = double(imread([‘DATA/’,int2str(i),’.jpg’])); Imzero = Im{i}+Imzero; end Imback = Imzero/5; [MR,MC,Dim] = size(Imback);
% Kalman 滤波器初始化 R=[[0.2845,0.0045]’,[0.0045,0.0455]’]; H=[[1,0]’,[0,1]’,[0,0]’,[0,0]’]; Q=0.01eye(4); P = 100eye(4); dt=1; % 采样时间，也就是图像帧时间 A=[[1,0,0,0]’,[0,1,0,0]’,[dt,0,1,0]’,[0,dt,0,1]’]; g = 6; % pixels^2/time step Bu = [0,0,0,g]’; kfinit=0; % kalman增益初始化 x=zeros(100,4); % 目标状态初始化
% 检测视频中每一帧图像 for i = 1 : 60 % 读取图像 Im = (imread([‘DATA/’,int2str(i), ‘.jpg’])); imshow(Im) imshow(Im) Imwork = double(Im);
% 检测目标（目标是一个球） [cc(i),cr(i),radius,flag] = extractball(Imwork,Imback,i); if flag==0 continue end
hold on for c = -1radius: radius/20 : 1radius r = sqrt(radius2-c2); plot(cc(i)+c,cr(i)+r,‘g.’) plot(cc(i)+c,cr(i)-r,‘g.’) end % Kalman update i if kfinit==0 xp = [MC/2,MR/2,0,0]’ else xp=Ax(i-1,:)’ + Bu end kfinit=1; PP = APA’ + Q K = PPH’inv(HPPH’+R) x(i,:) = (xp + K([cc(i),cr(i)]’ - Hxp))’; x(i,:) [cc(i),cr(i)] P = (eye(4)-KH)*PP
hold on for c = -1radius: radius/20 : 1radius r = sqrt(radius2-c2); plot(x(i,1)+c,x(i,2)+r,‘r.’) plot(x(i,1)+c,x(i,2)-r,‘r.’) end pause(0.3) end
% show positions figure plot(cc,‘r*’) hold on plot(cr,‘g*’) %end
%estimate image noise ® from stationary ball posn = [cc(55:60)’,cr(55:60)’]; mp = mean(posn); diffp = posn - ones(6,1)*mp; Rnew = (diffp’*diffp)/5; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 提取目标区域的中心和能包含目标的最大半径 function [cc,cr,radius,flag]=extractball(Imwork,Imback,index) cc = 0; cr = 0; radius = 0; flag = 0; [MR,MC,Dim] = size(Imback); fore = zeros(MR,MC); fore = (abs(Imwork(:,:,1)-Imback(:,:,1)) > 10) … | (abs(Imwork(:,:,2) - Imback(:,:,2)) > 10) … | (abs(Imwork(:,:,3) - Imback(:,:,3)) > 10); foremm = bwmorph(fore,‘erode’,2); labeled = bwlabel(foremm,4); stats = regionprops(labeled,[‘basic’]); [N,W] = size(stats); if N < 1 return end id = zeros(N); for i = 1 : N id(i) = i; end for i = 1 : N-1 for j = i+1 : N if stats(i).Area < stats(j).Area tmp = stats(i); stats(i) = stats(j); stats(j) = tmp; tmp = id(i); id(i) = id(j); id(j) = tmp; end end end if stats(1).Area < 100 return end selected = (labeled==id(1)); centroid = stats(1).Centroid; radius = sqrt(stats(1).Area/pi); cc = centroid(1); cr = centroid(2); flag = 1; return 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 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 扩展卡尔曼滤波 对于非线性系统滤波问题，常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题，其中应用最广泛的方法是扩展卡尔曼滤波（EKF）。扩展卡尔曼滤波建立在线性卡尔曼滤波的基础之上。其核心思想是，对一般的非线性系统，首先围绕滤波值Xk将非线性函数f和h展开成泰勒级数并略去二阶及以上项，得到一个近似的线性化模型，然后应用卡尔曼滤波完成对目标的滤波估计等处理。
EKF的优点是不必预先计算标称轨迹，过程噪声W(k)和观测噪声V(k)均为0,时非线性方程的解，但它只能在滤波误差及一步预测误差较小时才能使用。
简单非线性系统的扩展卡尔曼滤波器
% 函数功能：标量非线性系统扩展Kalman滤波问题 % 状态函数：X(k+1)=0.5X(k)+2.5X(k)/(1+X(k)^2)+8cos(1.2k) +w(k) % 观测方程：Z（k）=X(k)^2/20 +v(k) function EKF_for_One_Div_UnLine_System T=50;%总时间 Q=10;%Q的值改变，观察不同Q值时滤波结果 R=1;%测量噪声 %产生过程噪声 w=sqrt(Q)randn(1,T); %产生观测噪声 v=sqrt®randn(1,T); %状态方程 x=zeros(1,T); x(1)=0.1; y=zeros(1,T); y(1)=x(1)^2/20+v(1); for k=2:T x(k)=0.5x(k-1)+2.5x(k-1)/(1+x(k-1)^2)+8cos(1.2k)+w(k-1); y(k)=x(k)^2/20+v(k); end %EKF滤波算法 Xekf=zeros(1,T); Xekf(1)=x(1); Yekf=zeros(1,T); Yekf(1)=y(1); P0=eye(1); for k=2:T %状态预测 Xn=0.5Xekf(k-1)+2.5Xekf(k-1)/(1+Xekf(k-1)^2)+8cos(1.2k); %观测预测 Zn=Xn^2/20; %求状态矩阵F F=0.5+2.5 (1-Xn2)/(1+Xn2)^2; %求观测矩阵 H=Xn/10; %协方差预测 P=FP0F’+Q; %求卡尔曼增益 K=PH’inv(HPH’+R); %状态更新 Xekf(k)=Xn+K(y(k)-Zn); %协方差阵更新 P0=(eye(1)-K*H)*P; end %误差分析 Xstd=zeros(1,T); for k=1:T Xstd(k)=abs( Xekf(k)-x(k) ); end %画图 figure hold on;box on; plot(x,’-ko’,‘MarkerFace’,‘g’); plot(Xekf,’-ks’,‘MarkerFace’,‘b’); %误差分析 figure hold on;box on; plot(Xstd,’-ko’,‘MarkerFace’,‘g’); 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 真实值与估计值对比：
滤波误差：
EKF在目标跟踪中的应用 基于距离的目标跟踪算法Matlab程序 % 扩展Kalman滤波在目标跟踪中的应用 function EKF_For_TargetTracking clc;clear; T=1;%雷达扫描周期 N=60/T;%总的采样次数 X=zeros(4,N);%目标初始位置、速度 X(:,1)=[-100,2,200,20];%目标初始位置、速度 Z=zeros(1,N);%传感器对位置的观测 delta_w=1e-3;%如果增大这个参数，目标真实轨迹就是曲线 Q=delta_wdiag([0.5,1]);%过程噪声方差 G=[T2/2,0;T,0;0,T2/2;0,T];%过程噪声驱动矩阵 R=5;%观测噪声方差 F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵 x0=200;%观测站的位置，可以设为其他值 y0=300; Xstation=[x0,y0]; for t=2:N X(:,t)=FX(:,t-1)+Gsqrtm(Q)randn(2,1);%目标真实轨迹 end for t=1:N Z(t)=Dist(X(:,t),Xstation)+sqrtm®randn;%对目标观测 end %EKF滤波 Xekf=zeros(4,N); Xekf(:,1)=X(:,1);%卡尔曼滤波状态初始化 P0=eye(4);%协方差阵初始化 for i=2:N Xn=FXekf(:,i-1);%预测 P1=FP0F’+GQG’;%预测误差协方差 dd=Dist(Xn,Xstation);%观测预测 %求雅克比矩阵H H=[(Xn(1,1)-x0)/dd,0,(Xn(3,1)-y0)/dd,0];%即为所求一阶近似 K=P1H’inv(HP1H’+R);%增益 Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新 P0=(eye(4)-K*H)*P1;%滤波误差协方差更新 end %误差分析 for i=1:N Err_KalmanFilter(i)=Dist(X(:,i),Xekf(:,i));%滤波后的误差 end %画图 figure hold on;box on; plot(X(1,:),X(3,:),’-k.’);%真实轨迹 plot(Xekf(1,:),Xekf(3,:),’-r+’);%扩展卡尔曼滤波轨迹 legend(‘真实轨迹’,‘EKF轨迹’) figure hold on; box on; plot(Err_KalmanFilter,’-ks’,‘MarkerFace’,‘r’)
%子函数，求两点间的距离 function d=Dist(X1,X2); if length(X2)<=2 d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 ); else d=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 57
基于EKF的纯方位目标跟踪算法 % 扩展Kalman滤波在纯方位目标跟踪中的应用实例 function EKF_angle clc;clear; T=1;%雷达扫描周期 N=40/T;%总的采样次数 X=zeros(4,N);%目标真实位置、速度 X(:,1)=[0,2,1400,-10];%目标初始位置、速度 Z=zeros(1,N); %传感器对位置的观测 delta_w=1e-4;%如果增大这个参数，目标真实轨迹就是曲线了 Q=delta_wdiag([1,1]) ;%过程噪声均值 G=[T2/2,0;T,0;0,T2/2;0,T];%过程噪声驱动矩阵 R=0.1pi/180;%观测噪声方差 F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵 x0=0;%观测站的位置，可以设为其他值 y0=1000; Xstation=[x0;y0];
w=sqrtm®randn(1,N);%均值为0，方差为1的高斯噪声 for t=2:N X(:,t)=FX(:,t-1)+Gsqrtm(Q)randn(2,1);%目标真实轨迹 end for t=1:N Z(t)=hfun(X(:,t),Xstation)+w(t);%目标观测 %对sqrtm®w(t)转化为角度sqrtm®w(t)/pi180可以看出噪声的大小 end %EKF滤波 Xekf=zeros(4,N); Xekf(:,1)=X(:,1);%卡尔曼滤波状态初始化 P0=eye(4);%协方差阵初始化 for i=2:N Xn=FXekf(:,i-1);%预测 P1=FP0F’+GQG’;%预测误差协方差 dd=hfun(Xn,Xstation);%观测预测 %求雅克比矩阵H D=Dist(Xn,Xstation); H=[-(Xn(3,1)-y0)/D,0,(Xn(1,1)-x0)/D,0];%即为所求一阶近似 K=P1H’inv(HP1H’+R);%增益 Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新 P0=(eye(4)-KH)P1;%滤波误差协方差更新 end %误差分析 for i=1:N Err_KalmanFilter(i)=sqrt(Dist(X(:,i),Xekf(:,i))); end %画图 figure hold on;box on; plot(X(1,:),X(3,:),’-k.’);%真实轨迹 plot(Xekf(1,:),Xekf(3,:),’-r+’);%扩展卡尔曼滤波轨迹 legend(‘真实轨迹’,‘EKF轨迹’) figure hold on; box on; plot(Err_KalmanFilter,’-ks’,‘MarkerFace’,‘r’) figure hold on;box on; plot(Z/pi180,’-r.’,‘MarkerFace’,‘r’);%真实角度值 plot(Z/pi180+w/pi180,’-ko’,‘MarkerFace’,‘g’);%受噪声污染的观测值 legend(‘真实角度’,‘观测角度’); function cita=hfun(X1,X0) %需要注意各个象限角度的变化 if X1(3,1)-X0(2,1)>=0 if X1(1,1)-X0(1,1)>0 cita=atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) )); elseif X1(1,1)-X0(1,1)==0 cita=pi/2; else cita=pi/2+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) )); end else if X1(1,1)-X0(1,1)>0 cita=3pi/2+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) )); elseif X1(1,1)-X0(1,1)==0 cita=3*pi/2; else cita=pi+atan(abs( (X1(3,1)-X0(2,1))/(X1(1,1)-X0(1,1)) )); end end function d=Dist(X1,X2); if length(X2)<=2 d=( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 ); else d=( (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 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 跟踪轨迹
跟踪误差RMS
观测值和真实值的对比
EKF在三维纯方位寻的导弹制导中的应用 % 程序说明：目标跟踪程序，实现运动弹头对运动物体的三维跟踪，主函数 % 状态方程： x(t)=Ax(t-1)+Bu(t-1)+w(t） function main delta_t=0.01;%测量周期，采样周期 longa=10000;%机动时间常数的倒数，即机动频率 tf=3.7; T=tf/delta_t;%时间长度3.7s，一共采样T=370次 %状态转移矩阵F F=[eye(3),delta_teye(3),(exp(-1longadelta_t)+… longadelta_t-1)/longa^2eye(3); zeros(3),eye(3),(1-exp(-1longadelta_t))/longaeye(3); zeros(3),zeros(3),exp(-1longadelta_t)eye(3)]; %控制量驱动矩阵gama G=[-10.5delta_t^2eye(3);-1delta_teye(3);zeros(3)]; N=3; %导航比（制导率） for i=1:50 %做50次蒙特卡洛仿真 x=zeros(9,T); x(:,1)=[3500,1500,1000,-1100,-150,-50,0,0,0]’;%初始状态X(0) ex=zeros(9,T); ex(:,1)=[3000,1200,960,-800,-100,-100,0,0,0]’;%滤波器状态Xekf(0) cigema=sqrt(0.1); w=[zeros(6,T);cigemarandn(3,T)];%过程噪声 Q=[zeros(6),zeros(6,3);zeros(3,6),cigema^2eye(3)]; z=zeros(2,T);%观测值 z(:,1)=[atan( x(2,1)/sqrt(x(1,1)2+x(3,1)2) ), atan(-1x(3,1)/x(1,1))]’; v=zeros(2,T);%观测噪声 for k=2:T-3 tgo=tf-k0.01+0.0000000000000001; c1=N/tgo^2;%制导率的系数 c2=N/tgo;%制导率的系数 c3=N*(exp(-longatgo)+longatgo-1)/(longatgo)^2;%制导率的系数 %X、Y、Z三个方向的导弹加速度 u(1,k-1)=[c1,c2,c3][x(1,k-1),x(4,k-1),x(7,k-1)]’; u(2,k-1)=[c1,c2,c3][x(2,k-1),x(5,k-1),x(8,k-1)]’; u(3,k-1)=[c1,c2,c3][x(3,k-1),x(6,k-1),x(9,k-1)]’; x(:,k)=Fx(:,k-1)+Gu(:,k-1)+w(:,k-1);%目标状态方程 d=sqrt(x(1,k)2+x(2,k)2+x(3,k)^2); D=[d,0;0,d]; R=inv(D)0.1eye(2)inv(D)’;%观测噪声方差 v(:,k)=sqrtm®randn(2,1);%观测噪声模拟 %目标观测方程 z(:,k)=[atan( x(2,k)/sqrt(x(1,k)2+x(3,k)2) ), … atan(-1x(3,k)/x(1,k))]’+v(:,k); end %根据观测值开始滤波 P0=[104*eye(6),zeros(6,3);zeros(3,6),102eye(3)];%协方差初始化 eP0=P0; stop=0.5/0.01; span=1/0.01; for k=2:T-3 dd=sqrt(ex(1,k-1)2+ex(2,k-1)2+ex(3,k-1)^2); DD=[dd,0;0,dd]; RR=0.1eye(2)/(DDDD’); tgo=tf-k0.01+0.0000000000000001; c1=N/tgo^2; c2=N/tgo; c3=N(exp(-longatgo)+longatgo-1)/(longatgo)^2; u(1,k-1)=[c1,c2,c3][ex(1,k-1),ex(4,k-1),ex(7,k-1)]’; u(2,k-1)=[c1,c2,c3][ex(2,k-1),ex(5,k-1),ex(8,k-1)]’; u(3,k-1)=[c1,c2,c3][ex(3,k-1),ex(6,k-1),ex(9,k-1)]’; %调用扩展卡尔曼算法子函数 [ex(:,k),eP0]=ekf(F,G,Q,RR,eP0,u(:,k-1),z(:,k),ex(:,k-1)); end for t=1:T-3 %求每个时间点误差的平方 Ep_ekfx(i,t)=sqrt((ex(1,t)-x(1,t))^2); Ep_ekfy(i,t)=sqrt((ex(2,t)-x(2,t))^2); Ep_ekfz(i,t)=sqrt((ex(3,t)-x(3,t))^2); Ep_ekf(i,t)=sqrt( (ex(1,t)-x(1,t))2+(ex(2,t)-x(2,t))2+(ex(3,t)-x(3,t))^2 ); Ev_ekf(i,t)=sqrt( (ex(4,t)-x(4,t))2+(ex(5,t)-x(5,t))2+(ex(6,t)-x(6,t))^2 ); Ea_ekf(i,t)=sqrt( (ex(7,t)-x(7,t))2+(ex(8,t)-x(8,t))2+(ex(9,t)-x(9,t))^2 ); end
for t=1:T-3 %求误差的均值，即RMS
error_x(t)=mean(Ep_ekfx(:,t));
error_y(t)=mean(Ep_ekfy(:,t));
error_z(t)=mean(Ep_ekfz(:,t));
error_r(t)=mean(Ep_ekf(:,t));
error_v(t)=mean(Ev_ekf(:,t));
error_a(t)=mean(Ea_ekf(:,t));
end
end t=0.01:0.01:3.67; %轨迹图 figure hold on;box on;grid on; plot3(x(1,:),x(2,:),x(3,:),’-k.’) plot3(ex(1,:),ex(2,:),ex(3,:),’-r.’,‘MarkerFace’,‘r’) legend(‘real’,‘ekf’); view(3) title(‘position’) %位置偏差图 figure hold on;box on;grid on; plot(t,error_r,‘b’); xlabel(‘飞行时间/s’); ylabel(‘弹目相对距离估计误差/m’); %加速度偏差图 figure hold on;box on;grid on; plot(t,error_v,‘b’); xlabel(‘飞行时间/s’); ylabel(‘速度估计误差m/s’);
%子函数 %ex为扩展卡尔曼估计得到的状态 function [ex,P0]=ekf(F,G,Q,R,P0,u,z,ex) %状态预测 Xn=Fex+Gu; %观测预测 Zn=[atan( Xn(2)/sqrt(Xn(1)2+Xn(3)2) ),atan(-1Xn(3)/Xn(1))]’; %协方差阵预测 P=FP0F’+Q; %计算线性化的H矩阵 dh1_dx=-1Xn(1)Xn(2)/(Xn(1)2+Xn(2)2+Xn(3)2)/sqrt(Xn(1)2+Xn(3)^2); dh1_dy=sqrt(Xn(1)2+Xn(3)2)/(Xn(1)2+Xn(2)2+Xn(3)^2); dh1_dz=-1Xn(2)Xn(3)/(Xn(1)2+Xn(2)2+Xn(3)2)/sqrt(Xn(1)2+Xn(3)^2); dh2_dx=Xn(3)/(Xn(1)2+Xn(3)2); dh2_dy=0; dh2_dz=-1Xn(1)/(Xn(1)2+Xn(3)2); H=[dh1_dx,dh1_dy,dh1_dz,0,0,0,0,0,0;dh2_dx,dh2_dy,dh2_dz,0,0,0,0,0,0]; %卡尔曼增益 K=PH’/(HPH’+R); %状态更新 ex=Xn+K(z-Zn); %协方差阵更新 P0=(eye(9)-K*H)*P; 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 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 寻的导弹EKF跟踪位置偏差
寻的导弹EKF跟踪速度偏差
寻的导弹EKF跟踪加速度偏差
无迹卡尔曼滤波 前面讨论的扩展卡尔曼滤波算法是对非线性的系统方程或者观测方程进行泰勒展开并保留其一阶近似项，这样不可避免地引入线性化误差。如果线性化假设不成立，采用这种算法则会导致滤波器性能下降以至于造成发散。另外，在一般情况下计算系统状态方程和观测方程的Jacobian矩阵是不易实现的，增加了算法的计算复杂度。
无迹卡尔曼滤波UKF摒弃了对非线性函数进行线性化的传统做法，采用卡尔曼线性滤波框架，对于一步预测方程，使用无迹变换UT来处理均值和协方差的非线性传递问题。UKF算法是对非线性函数的概率密度分布进行近似，用一系列确定样本来逼近状态的后验概率密度，而不是对非线性函数进行近似，不需要对Jacobian矩阵进行求导。UKF没有把高阶项忽略，因此对于非线性分布的娃统计量有较高的计算精度，有效地克服了扩展卡尔曼滤波的估计精度低、稳定性差的缺陷。
无迹卡尔曼滤波在单观测站目标跟踪中的应用 % 无迹Kalman滤波在目标跟踪中的应用 function UKF clc;clear; T=1; %雷达扫描周期 N=60/T; %总的采样次数 X=zeros(4,N);%目标真实位置，速度 X(:,1)=[-100,2,200,20]; %目标初始位置、速度 Z=zeros(1,N); %传感器对位置的观测 delta_w=1e-3; %如果增大这个参数，目标真实轨迹就是曲线 Q=delta_w*diag([0.5,1]) ;%过程噪声均值 G=[T2/2,0;T,0;0,T2/2;0,T];%过程噪声驱动矩阵 R=5;%观测噪声方差 F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵 x0=200; %观测站的位置，可以设为其他值 y0=300; Xstation=[x0,y0]; %雷达站的位置
w=sqrtm®randn(1,N); for t=2:N X(:,t)=FX(:,t-1)+Gsqrtm(Q)randn(2,1);%目标观测 end %UKF滤波，UT变换 for t=1:N Z(t)=Dist(X(:,t),Xstation)+w(t); end L=4; alpha=1; kalpha=0; belta=2; ramda=3-L; for j=1:2L+1 Wm(j)=1/(2(L+ramda)); Wc(j)=1/(2*(L+ramda)); end Wm(1)=ramda/(L+ramda); Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;%权值计算
Xukf=zeros(4,N); Xukf(:,1)=X(:,1);%无迹卡尔曼滤波状态初始化 P0=eye(4);%协方差阵初始化 for t=2:N xestimate= Xukf(:,t-1); P=P0; %第一步：获得一组Sigma点集 cho=(chol(P*(L+ramda)))’; for k=1:L xgamaP1(:,k)=xestimate+cho(:,k); xgamaP2(:,k)=xestimate-cho(:,k); end Xsigma=[xestimate,xgamaP1,xgamaP2]; %第二步：对Sigma点集进行一步预测 Xsigmapre=FXsigma; %第三步：利用第二步的结果计算均值和协方差 Xpred=zeros(4,1);%均值 for k=1:2L+1 Xpred=Xpred+Wm(k)Xsigmapre(:,k); end Ppred=zeros(4,4);%协方差阵预测 for k=1:2L+1 Ppred=Ppred+Wc(k)(Xsigmapre(:,k)-Xpred)(Xsigmapre(:,k)-Xpred)’; end Ppred=Ppred+GQG’; %第四步：根据预测值，再一次使用UT变换，得到新的sigma点集 chor=(chol((L+ramda)Ppred))’; for k=1:L XaugsigmaP1(:,k)=Xpred+chor(:,k); XaugsigmaP2(:,k)=Xpred-chor(:,k); end Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2]; %第五步：观测预测 for k=1:2L+1%观测预测 Zsigmapre(1,k)=hfun(Xaugsigma(:,k),Xstation); end %第六步：计算观测预测均值和协方差 Zpred=0;%观测预测的均值 for k=1:2L+1 Zpred=Zpred+Wm(k)Zsigmapre(1,k); end Pzz=0; for k=1:2L+1 Pzz=Pzz+Wc(k)(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)’; end Pzz=Pzz+R;%得到协方差Pzz
Pxz=zeros(4,1);
for k=1:2*L+1
Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)';
end
%第七步：计算卡尔曼增益
K=Pxz*inv(Pzz);
%第八步：状态和方差更新
xestimate=Xpred+K*(Z(t)-Zpred);%状态更新
P=Ppred-K*Pzz*K';%方差更新
P0=P;
Xukf(:,t)=xestimate;
end %误差分析 for i=1:N Err_KalmanFilter(i)=Dist(X(:,i),Xukf(:,i));%滤波后的误差 end %画图 figure hold on;box on; plot(X(1,:),X(3,:),’-k.’);%真实轨迹 plot(Xukf(1,:),Xukf(3,:),’-r+’);%无迹卡尔曼滤波轨迹 legend(‘真实轨迹’,‘UKF轨迹’) figure hold on; box on; plot(Err_KalmanFilter,’-ks’,‘MarkerFace’,‘r’)
%子函数：求两点间的距离 function d=Dist(X1,X2) if length(X2)<=2 d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 ); else d=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 ); end %观测子函数：观测距离 function [y]=hfun(x,xx) y=sqrt((x(1)-xx(1))2+(x(3)-xx(2))2); 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 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 跟踪轨迹
误差曲线
UKF在匀加速直线运动目标跟踪中的应用 % 功能说明： UKF在目标跟踪中的应用 % 参数说明： 1、状态6维，x方向的位置、速度、加速度； % y方向的位置、速度、加速度； % 2、观测信息为距离和角度； function ukf_for_track_6_div_system n=6;%状态维数 t=0.5;%采用点数 Q=[1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 0.01 0 0 0; 0 0 0 0.01 0 0; 0 0 0 0 0.0001 0; 0 0 0 0 0 0.0001];%过程噪声协方差阵 R = [100 0; 0 0.001^2];%量测噪声协方差阵 %状态方程 f=@(x)[x(1)+tx(3)+0.5t2*x(5);x(2)+t*x(4)+0.5*t2x(6);… x(3)+tx(5);x(4)+t*x(6);x(5);x(6)]; %x1为X轴位置，x2为Y轴位置，x3、x4分别是X、Y轴的速度，x5、x6为X、Y两方向的加速度 %观测方程 h=@(x)[sqrt(x(1)2+x(2)2);atan(x(2)/x(1))]; s=[1000;5000;10;50;2;-4]; x0=s+sqrtm(Q)*randn(n,1);%初始化状态 P0 =[100 0 0 0 0 0; 0 100 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 0.1 0; 0 0 0 0 0 0.1];%初始化协方差 N=50;%总仿真时间步数，即总时间 Xukf = zeros(n,N);%UKF滤波状态初始化 X = zeros(n,N);%真实状态 Z = zeros(2,N);%测量值 for i=1:N X(:,i)= f(s)+sqrtm(Q)*randn(6,1);%模拟，产生目标运动真实轨迹 s = X(:,i); end ux=x0;%ux为中间变量 for k=1:N Z(:,k)= h(X(:,k)) + sqrtm®*randn(2,1);%测量值，保存观测 [Xukf(:,k), P0] = ukf(f,ux,P0,h,Z(:,k),Q,R);%调用ukf滤波算法 ux=Xukf(:,k); end %跟踪误差分析 %这里只分析位置误差，速度、加速度误差分析在此略 for k=1:N RMS(k)=sqrt( (X(1,k)-Xukf(1,k))2+(X(2,k)-Xukf(2,k))2 ); end %画图 figure t=1:N; hold on;box on; plot( X(1,t),X(2,t), ‘k-’) plot(Z(1,t).*cos(Z(2,t)),Z(1,t).*sin(Z(2,t)),’-b.’) plot(Xukf(1,t),Xukf(2,t),’-r.’) legend(‘实际值’,‘测量值’,‘ukf估计值’); xlabel(‘x方向位置/米’) ylabel(‘y方向位置/米’) %误差分析图 figure box on; plot(RMS,’-ko’,‘MarkerFace’,‘r’) xlabel(‘t/秒’) ylabel(‘偏差/米’)
%UKF子函数 function [X,P]=ukf(ffun,X,P,hfun,Z,Q,R) %非线性系统中UKF算法 L=numel(X);%状态维数 m=numel(Z);%观测维数 alpha=1e-2;%默认系数，参看UT变换，下同 ki=0;%默认系数 beta=2;%默认系数 lambda=alpha^2*(L+ki)-L;%默认系数 c=L+lambda;%默认系数 Wm=[lambda/c 0.5/c+zeros(1,2L)];%权值 Wc=Wm; Wc(1)=Wc(1)+(1-alpha^2+beta);%权值 c=sqrt©; %第一步，获取一组sigma点集 %sigma点集，在状态X附近的点集，X是613矩阵，每列为1样本 Xsigmaset=sigmas(X,P,c); %第二、第三、四步，对sigma点集进行一步预测，得到均值XImeans和方差P1和新sigma点集X1 %对状态UT变换 [X1means,X1,P1,X2]=ut(ffun,Xsigmaset,Wm,Wc,L,Q);
%第五、六步，得到观测预测，Z1为X1集合的预测，Zpre为Z1的均值。 %Pzz为协方差 [Zpre,Z1,Pzz,Z2]=ut(hfun,X1,Wm,Wc,m,R);%对观测UT变换 Pxz=X2diag(Wc)Z2’;%协方差Pxz %第七步，计算卡尔曼增益 K=Pxzinv(Pzz); %第八步，状态和方差更新 X=X1means+K(Z-Zpre);%状态更新 P=P1-K*Pxz’;%协方差更新
%UT变换子函数 % 输入：fun为函数句柄，Xsigma为样本集，Wm和Wc为权值，n为状态维数（n=6）,COV为方差 % 输出：Xmeans为均值 function [Xmeans,Xsigma_pre,P,Xdiv]=ut(fun,Xsigma,Wm,Wc,n,COV) LL=size(Xsigma,2);%得到Xsigma样本个数 Xmeans=zeros(n,1);%均值 Xsigma_pre=zeros(n,LL); for k=1:LL Xsigma_pre(:,k)=fun(Xsigma(:,k));%一步预测 Xmeans=Xmeans+Wm(k)Xsigma_pre(:,k); end Xdiv=Xsigma_pre-Xmeans(:,ones(1,LL));%预测减去均值 P=Xdivdiag(Wc)*Xdiv’+COV;%协方差
%产生Sigma点集函数 function Xset=sigmas(X,P,c) A = c*chol§’;%Cholesky分解 Y = X(:,ones(1,numel(X))); Xset = [X Y+A Y-A]; 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 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 运动轨迹
跟踪误差
交互多模型卡尔曼滤波 在卡尔曼滤波算法中用到了状态转移方程和观测方程，被估计量随时间变化，它是一种动态估计。在目标跟踪中，不必知道目标的运动模型就能够实时的修正目标的状态参量（位置、速度等），具有良好的适应性。但是当目标实施机动时，仅采用基本的卡尔曼滤波算法往往得不到理想的结果。这时需要采用自适应算法。交互多模型IMM是一种软切换算法，目前在机动目标跟踪领域中得到了广泛的应用。IMM算法使用两个或更多的模型来描述工作过程中可能的状态，最后通过有效的加权融合进行系统状态估计，很好地克服了单模型估计误差较大的问题。
% 交互多模Kalman滤波在目标跟踪中的应用 % function ImmKalman clear; T=2;%雷达扫描周期，即采样周期 M=50;%滤波次数 N=900/T;%总采样点数 N1=400/T;%第一转弯处采样起点 N2=600/T;%第一匀速处采样起点 N3=610/T;%第二转弯处采样起点 N4=660/T;%第二匀速处采样起点 Delta=100;%测量噪声标准差 %对真实的轨迹和观测轨迹数据的初始化 Rx=zeros(N,1); Ry=zeros(N,1); Zx=zeros(N,M); Zy=zeros(N,M); % 1-沿y轴匀速直线 t=2:T:400; x0=2000+0t’; y0=10000-15t’; % 2-慢转弯 t=402:T:600; x1=x0(N1)+0.075*((t’-400).^2)/2; y1=y0(N1)-15*(t’-400)+0.075*((t’-400).^2)/2; % 3-匀速 t=602:T:610; vx=0.075*(600-400); x2=x1(N2-N1)+vx*(t’-600); y2=y1(N2-N1)+0t’; % 4-快转弯 t=612:T:660; x3=x2(N3-N2)+(vx(t’-610)-0.3*((t’-610).^2)/2); y3=y2(N3-N2)-0.3*((t’-610).^2)/2; % 5-匀速直线 t=662:T:900; vy=-0.3*(660-610); x4=x3(N4-N3)+0t’; y4=y3(N4-N3)+vy(t’-660); % 最终将所有轨迹整合成为一个列向量，即真实轨迹数据，Rx为Real-x,Ry为Real-y Rx=[x0;x1;x2;x3;x4]; Ry=[y0;y1;y2;y3;y4]; % 对每次蒙特卡洛仿真的滤波估计位置的初始化 Mt_Est_Px=zeros(M,N); Mt_Est_Py=zeros(M,N); % 产生观测数据，要仿真M次，必须有M次 nx=randn(N,M)Delta;%产生观测噪声 ny=randn(N,M)Delta; Zx=Rxones(1,M)+nx;%真实值的基础上叠加噪声，即构成计算机模拟的观测值 Zy=Ryones(1,M)+ny; for m=1:M %滤波初始化 Mt_Est_Px(m,1)=Zx(1,m);%初始数据 Mt_Est_Py(m,1)=Zx(2,m); xn(1)=Zx(1,m);%滤波初值 xn(2)=Zx(2,m); yn(1)=Zy(1,m); yn(2)=Zy(2,m); %非机动模型参数 phi=[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]; g=[T/2,0;1,0;0,T/2;0,1]; q=[Delta2,0;0,Delta2]; vx=(Zx(2)-Zx(1,m))/2; vy=(Zy(2)-Zy(1,m))/2; %初始状态估计 x_est=[Zx(2,m);vx;Zy(2,m);vy]; p_est=[Delta2,Delta2/T,0,0;Delta2/T,2*Delta2/(T^2),0,0; 0,0,Delta2,Delta2/T;0,0,Delta2/T,2*Delta2/(T^2)]; Mt_Est_Px(m,2)=x_est(1); Mt_Est_Py(m,2)=x_est(3); %滤波开始 for r=3:N z=[Zx(r,m);Zy(r,m)]; if r<20 x_pre=phix_est;%预测 p_pre=phip_estphi’;%预测误差协方差 k=p_preh’inv(hp_preh’+q);%卡尔曼增益 x_est=x_pre+k(z-hx_pre);%滤波 p_est=(eye(4)-kh)*p_pre;%滤波协方差 xn®=x_est(1);%记录采样点滤波数据 yn®=x_est(3); Mt_Est_Px(m,r)=x_est(1);%记录第m次仿真滤波估计数据 Mt_Est_Py(m,r)=x_est(3); else if r==20 X_est=[x_est;0;0];%扩充维数 P_est=p_est; P_est(6,6)=0; for i=1:3 Xn_est{i,1}=X_est; Pn_est{i,1}=P_est; end u=[0.8,0.1,0.1];%模型概率初始化 end %调用IMM算法 [X_est,P_est,Xn_est,Pn_est,u]=IMM(Xn_est,Pn_est,T,z,Delta,u); xn®=X_est(1); yn®=X_est(3); Mt_Est_Px(m,r)=X_est(1); Mt_Est_Py(m,r)=X_est(3); end end end %结束第一次滤波 %%%%%%%%%%%%%%%%%%% %滤波结果的数据分析 err_x=zeros(N,1); err_y=zeros(N,1); delta_x=zeros(N,1); delta_y=zeros(N,1); %计算滤波的误差均值及标准差 for r=1:N %估计误差均值 ex=sum(Rx®-Mt_Est_Px(:,r)); ey=sum(Ry®-Mt_Est_Py(:,r)); err_x®=ex/M; err_y®=ey/M; eqx=sum((Rx®-Mt_Est_Px(:,r)).^2); eqy=sum((Ry®-Mt_Est_Py(:,r)).^2); %估计误差标准差 delta_x®=sqrt(abs(eqx/M-(err_x®^2))); delta_y®=sqrt(abs(eqy/M-(err_y®^2))); end %画图 figure(1); plot(Rx,Ry,‘k-’,Zx,Zy,‘g:’,xn,yn,‘r-.’); legend(‘真实轨迹’,‘观测样本’,‘估计轨迹’); figure(2); subplot(2,1,1); plot(err_x); %axis([1,N,-300,300]); title(‘x方向估计误差均值’); subplot(2,1,2); plot(err_y); %axis([1,N,-300,300]); title(‘y方向估计误差均值’); figure(3); subplot(2,1,1); plot(delta_x); %axis([1,N,0,1]); title(‘x方向估计误差标准差’); subplot(2,1,2); plot(delta_y); %axis([1,N,0,1]); title(‘y方向估计误差标准差’);
%% 子函数 %% X_est,P_est返回第m次仿真第r个采样点的滤波结果 %% Xn_est,Pn_est记录每个模型对应的第m次仿真第r次仿真第r个采样点的滤波结果 %% u为模型概率 function [X_est,P_est,Xn_est,Pn_est,u]=IMM(Xn_est,Pn_est,T,Z,Delta,u) %% 控制模型转换的马尔科夫链的转移概率矩阵 P=[0.95,0.025,0.025;0.025,0.95,0.025;0.025,0.025,0.95]; %所采用的第三个模型参数，模型一位非机动，模型二、三均为机动模型 %模型一 PHI{1,1}=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; PHI{1,1}(6,6)=0; PHI{2,1}=[1,T,0,0,T2/2,0;0,1,0,0,T,0;0,0,1,T,0,T2/2; 0,0,0,1,0,T;0,0,0,0,1,0;0,0,0,0,0,1];%模型二 PHI{3,1}=PHI{2,1};%模型三 G{1,1}=[T/2,0;1,0;0,T/2;0,1];%模型一 G{1,1}(6,2)=0; G{2,1}=[T2/4,0;T/2,0;0,T2/4;0,T/2;1,0;0,1];%模型二 G{3,1}=G{2,1};%模型三 Q{1,1}=zeros(2);%模型一 Q{2,1}=0.001eye(2);%模型二 Q{3,1}=0.0114eye(2);%模型三 H=[1,0,0,0,0,0;0,0,1,0,0,0]; R=eye(2)*Delta^2;%观测噪声协方差阵 mu=zeros(3,3);%混合概率矩阵 c_mean=zeros(1,3);%归一化常数 for i=1:3 c_mean=c_mean+P(i,:)*u(i); end for i=1:3 mu(i,:)=P(i,:)u(i)./c_mean; end %输入交互 for j=1:3 X0{j,1}=zeros(6,1); P0{j,1}=zeros(6); for i=1:3 X0{j,1}=X0{j,1}+Xn_est{i,1}mu(i,j); end for i=1:3 P0{j,1}=P0{j,1}+mu(i,j)( Pn_est{i,1}… +(Xn_est{i,1}-X0{j,1})(Xn_est{i,1}-X0{j,1})’); end end %模型条件滤波 a=zeros(1,3); for j=1:3 %观测预测 X_pre{j,1}=PHI{j,1}*X0{j,1}; %协方差预测 P_pre{j,1}=PHI{j,1}*P0{j,1}*PHI{j,1}’+G{j,1}*Q{j,1}G{j,1}’; %计算卡尔曼增益 K{j,1}=P_pre{j,1}H’inv(HP_pre{j,1}H’+R); %状态更新 Xn_est{j,1}=X_pre{j,1}+K{j,1}(Z-HX_pre{j,1}); %协方差更新 Pn_est{j,1}=(eye(6)-K{j,1}H)P_pre{j,1}; end %模型概率更新 for j=1:3 v{j,1}=Z-HX_pre{j,1};%新息 s{j,1}=HP_pre{j,1}H’+R;%观测协方差矩阵 n=length(s{j,1})/2; a(1,j)=1/((2pi)^nsqrt(det(s{j,1})))exp(-0.5v{j,1}’… *inv(s{j,1})*v{j,1});%观测相对于模型j的似然函数 end c=sum(a.*c_mean);%归一化常数 u=a.c_mean./c;%模型概率更新 %输出交互 Xn=zeros(6,1); Pn=zeros(6); for j=1:3 Xn=Xn+Xn_est{j,1}.u(j); end for j=1:3 Pn=Pn+u(j).(Pn_est{j,1}+(Xn_est{j,1}-Xn)(Xn_est{j,1}-Xn)’); end %返回滤波结果 X_est=Xn; P_est=Pn; 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 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 目标的真实轨迹、观测轨迹、估计轨迹
滤波误差的均值曲线
滤波误差的标准差曲线
作者：Joeyos 来源：CSDN 原文：https://blog.csdn.net/zhangquan2015/article/details/79264540 版权声明：本文为博主原创文章，转载请附上博文链接！