EKF.rar_EKF_EKF跟踪_trackingEKF_卡尔曼滤波_扩展目标跟踪

共1个文件

m：1个

版权申诉

5星 · 超过95%的资源 140 浏览量 2022-07-15 09:42:31 上传评论 1 收藏 2KB RAR 举报

**正文** 《扩展卡尔曼滤波（EKF）在目标跟踪中的应用》扩展卡尔曼滤波（Extended Kalman Filter, 简称EKF）是经典卡尔曼滤波（Kalman Filter, KF）在非线性系统状态估计中的延伸，它在目标跟踪领域具有广泛的应用。本篇将详细介绍EKF的工作原理以及在目标跟踪中的具体实现。 1. **卡尔曼滤波基础** 卡尔曼滤波是一种统计方法，用于在线估计动态系统的状态。其核心思想是利用系统的先验知识（即预测）和实际观测值（即更新）来不断优化对系统状态的估计，从而达到最小化估计误差的目的。卡尔曼滤波假设系统是线性的，并且存在高斯白噪声。 2. **扩展卡尔曼滤波** 当实际系统模型是非线性时，EKF应运而生。EKF通过线性化非线性函数，将其转换为近似的线性系统，进而应用卡尔曼滤波的框架。线性化通常采用泰勒级数展开，通常取一阶泰勒展开，即将非线性函数在当前估计值处进行切线近似。 3. **EKF工作流程** - **预测步骤**：根据上一时刻的状态估计和系统动力学模型，预测下一时刻的状态。 - **更新步骤**：将预测结果与传感器观测值进行比较，通过观测模型更新状态估计。 4. **目标跟踪应用** 在目标跟踪中，EKF可以处理目标的位置、速度等多维状态的估计。例如，对于移动的目标，我们可以建立包含位置和速度的非线性状态模型，通过EKF来连续估计目标的实时状态。在实际应用中，EKF可以利用雷达或摄像头等传感器的数据来不断修正目标的位置估计。 5. **MATLAB实现** 提供的MATLAB文件`EKF.m`应该是一个EKF目标跟踪算法的实现。此代码可能包含了定义系统模型、非线性函数的线性化、预测和更新过程等关键步骤。通过运行此代码，可以模拟目标的运动轨迹，并观察EKF如何在每次迭代中逐步改进状态估计。 6. **EKF的局限性和改进** 尽管EKF在许多情况下表现出色，但其线性化过程可能导致误差积累，尤其是在非线性非常强的情况下。为了克服这一问题，后续出现了许多改进的滤波器，如无迹卡尔曼滤波（UKF）、粒子滤波（PF）等，它们能更好地处理非线性问题。 EKF是目标跟踪领域的重要工具，通过对非线性系统的近似处理，能够在动态环境中提供有效的状态估计。MATLAB实现的EKF程序为我们提供了直观的理解和实践这种算法的机会。通过深入学习和理解EKF，我们可以更好地应用于实际的跟踪问题，提高系统性能。

资源详情

资源评论

资源推荐

收起资源包目录

EKF.rar （1个子文件）

EKF.m 11KB

clear; q1=0.25;%目标运动扰动参数 q2=0.25;%目标运动扰动参数 T=0.5; fai=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; %状态转移矩阵,Φ(k) Q(:,:,1)=[T^3/3,T^2/2,0,0;T^2/2,T,0,0;0,0,T^3/3,T^2/2;0,0,T^2/2,T]*q1; %过程噪声方差,Q1(k) Q(:,:,2)=[T^3/3,T^2/2,0,0;T^2/2,T,0,0;0,0,T^3/3,T^2/2;0,0,T^2/2,T]*q2; %过程噪声方差,Q2(k) x0(:,1)=[100;9.62;200;5.56]; %目标1初始状态 x0(:,2)=[150;9.62;300;5.56]; %目标2初始状态 p0(:,:,1)=0*[32.4,0,0,0;0,0,0,0;0,0,36.2,0;0,0,0,0]; %目标1初始误差协方差 p0(:,:,2)=0*[32.4,0,0,0;0,0,0,0;0,0,36.2,0;0,0,0,0]; %目标2初始误差协方差 NT=2;%设置目标个数 step=100;%设置仿真步数 H=zeros(2,4,step,NT);%扩展Klamn滤波中的等价测量矩阵 I=eye(4); D=1000;%两个基阵相距的距离，单位m R(:,:,1)=[2,0;0,0.5*pi/180];%主对角元分别为基阵A的测距误差(单位：米)和测向误差(单位：弧度) R(:,:,2)=[1.5,0;0,0.3*pi/180];%主对角元分别为基阵B的测距误差(单位：米)和测向误差(单位：弧度) gama=9;%跟踪波门参数γ P_gate=0.9997;%门概率 % Produce basic state and measurement for k=1:step for i=1:NT w(:,k,i)=sqrtm(Q(:,:,i))*randn(4,1);%产生过程噪声 a=[0;0]; for t=1:1000 a=a+R(:,:,i)*randn(2,1); t=t+1; end v(:,k,i)=a/1000; % v(:,k,i)=R(:,:,i)*randn(2,1);%产生各个观测站的测量噪声 end end %生成目标状态和测量值 for k=1:step for i=1:NT if k==1 x(:,k,i)=fai*x0(:,i)+w(:,k,i);%生成目标i的状态 else x(:,k,i)=fai*x(:,k-1,i)+w(:,k-1,i);%生成目标i的状态 end ra(k,i)=sqrt(x(1,k,i)^2+x(3,k,i)^2)+v(1,k,1);%基阵A对目标i的距离 rb(k,i)=sqrt((x(1,k,i)-D)^2+x(3,k,i)^2)+v(1,k,2);%基阵B对目标i的距离 A(k,i)=acos(x(1,k,i)/ra(k,i))+v(2,k,1);%基阵A对目标i的测向角αi B(k,i)=acos((x(1,k,i)-D)/rb(k,i))+v(2,k,2);%基阵B对目标i的测向角βi za(:,k,i)=[ra(k,i);A(k,i)];%基阵A对目标i的量测 zb(:,k,i)=[rb(k,i);B(k,i)];%基阵B对目标i的量测 end end for k=1:step for j=1:2 z1a(:,k,j)=[za(1,k,j)*cos(za(2,k,j));za(1,k,j)*sin(za(2,k,j))];%涉及基阵A的极坐标量测转换成直角坐标量测 z1b(:,k,j)=[zb(1,k,j)*cos(zb(2,k,j))+D;zb(1,k,j)*sin(zb(2,k,j))];%涉及基阵B的极坐标量测转换成直角坐标量测 end end %%%%%%%%最近邻域算法，NNSF %%%%%目标1的关联情况 for k=1:step if k==1 x1_yc(:,k)=fai*x0(:,1); p1_yc(:,:,k)=fai*p0(:,:,1)*fai'+Q(:,:,1); for i=1:2 %%%分别考察两个观测基站 if i==1 H1(:,:,k,i)=[x1_yc(1,k)/(x1_yc(1,k)^2+x1_yc(3,k)^2),0,x1_yc(3,k)/(x1_yc(1,k)^2+x1_yc(3,k)^2),0;-x1_yc(3,k)/(x1_yc(1,k)^2+x1_yc(3,k)^2),0,x1_yc(1,k)/(x1_yc(1,k)^2+x1_yc(3,k)^2),0]; z1_yc(:,k,i)=[sqrt(x1_yc(1,k)^2+x1_yc(3,k)^2);acos(x1_yc(1,k)/sqrt(x1_yc(1,k)^2+x1_yc(3,k)^2))]; S1(:,:,k,i)=H1(:,:,k,i)*p1_yc(:,:,k)*H1(:,:,k,i)'+R(:,:,i)*R(:,:,i); K1(:,:,k,i)=p1_yc(:,:,k)*H1(:,:,k,i)'*inv(S1(:,:,k,i)); x1_gj(:,k,i)=x1_yc(:,k)+K1(:,:,k,i)*(za(:,k,i)-z1_yc(:,k,i)); p1_gj(:,:,k,i)=(I-K1(:,:,k,i)*H1(:,:,k,i))*p1_yc(:,:,k); else H1(:,:,k,i)=[(x1_yc(1,k)-D)/((x1_yc(1,k)-D)^2+x1_yc(3,k)^2),0,x1_yc(3,k)/((x1_yc(1,k)-D)^2+x1_yc(3,k)^2),0;-x1_yc(3,k)/((x1_yc(1,k)-D)^2+x1_yc(3,k)^2),0,(x1_yc(1,k)-D)/((x1_yc(1,k)-D)^2+x1_yc(3,k)^2),0]; z1_yc(:,k,i)=[sqrt((x1_yc(1,k)-D)^2+x1_yc(3,k)^2);acos((x1_yc(1,k)-D)/sqrt((x1_yc(1,k)-D)^2+x1_yc(3,k)^2))]; S1(:,:,k,i)=H1(:,:,k,i)*p1_yc(:,:,k)*H1(:,:,k,i)'+R(:,:,i)*R(:,:,i); K1(:,:,k,i)=p1_yc(:,:,k)*H1(:,:,k,i)'*inv(S1(:,:,k,i)); x1_gj(:,k,i)=x1_yc(:,k)+K1(:,:,k,i)*(zb(:,k,1)-z1_yc(:,k,i)); p1_gj(:,:,k,i)=(I-K1(:,:,k,i)*H1(:,:,k,i))*p1_yc(:,:,k); end end %%%融合 p1_cross(:,:,k)=(I-K1(:,:,k,1)*H1(:,:,k,1))*(fai*p0(:,:,1)*fai'+Q(:,:,1))*(I-K1(:,:,k,2)*H1(:,:,k,2))'; L1(:,:,k)=(p1_gj(:,:,1)-p1_cross(:,:,k))*inv(p1_gj(:,:,1)+p1_gj(:,:,2)-p1_cross(:,:,k)-p1_cross(:,:,k)'); X1_gj(:,k)=x1_gj(:,k,1)+L1(:,:,k)*(x1_gj(:,k,2)-x1_gj(:,k,1)); P1_gj(:,:,k)=p1_gj(:,:,k,1)-L1(:,:,k)*(p1_gj(:,:,k,1)-p1_cross(:,:,k)'); % X1_gj(:,k)=inv(p1_gj(:,:,k,1))*x1_gj(:,k,1)+inv(p1_gj(:,:,k,2))*x1_gj(:,k,2)-p1_yc(:,:,k)*x1_yc(:,k); % P1_gj(:,:,k)=inv(inv(p1_gj(:,:,k,1))+inv(p1_gj(:,:,k,2))-inv(p1_yc(:,:,k))); else x1_yc(:,k)=fai*X1_gj(:,k-1); p1_yc(:,:,k)=fai*P1_gj(:,:,k-1)*fai'+Q(:,:,1); for i=1:2 %%%分别考察两个观测基站 if i==1 H1(:,:,k,i)=[x1_yc(1,k)/(x1_yc(1,k)^2+x1_yc(3,k)^2),0,x1_yc(3,k)/(x1_yc(1,k)^2+x1_yc(3,k)^2),0;-x1_yc(3,k)/(x1_yc(1,k)^2+x1_yc(3,k)^2),0,x1_yc(1,k)/(x1_yc(1,k)^2+x1_yc(3,k)^2),0]; z1_yc(:,k,i)=[sqrt(x1_yc(1,k)^2+x1_yc(3,k)^2);acos(x1_yc(1,k)/sqrt(x1_yc(1,k)^2+x1_yc(3,k)^2))]; S1(:,:,k,i)=H1(:,:,k,i)*p1_yc(:,:,k)*H1(:,:,k,i)'+R(:,:,i)*R(:,:,i); K1(:,:,k,i)=p1_yc(:,:,k)*H1(:,:,k,i)'*inv(S1(:,:,k,i)); x1_gj(:,k,i)=x1_yc(:,k)+K1(:,:,k,i)*(za(:,k,i)-z1_yc(:,k,i)); p1_gj(:,:,k,i)=(I-K1(:,:,k,i)*H1(:,:,k,i))*p1_yc(:,:,k); else H1(:,:,k,i)=[(x1_yc(1,k)-D)/((x1_yc(1,k)-D)^2+x1_yc(3,k)^2),0,x1_yc(3,k)/((x1_yc(1,k)-D)^2+x1_yc(3,k)^2),0;-x1_yc(3,k)/((x1_yc(1,k)-D)^2+x1_yc(3,k)^2),0,(x1_yc(1,k)-D)/((x1_yc(1,k)-D)^2+x1_yc(3,k)^2),0]; z1_yc(:,k,i)=[sqrt((x1_yc(1,k)-D)^2+x1_yc(3,k)^2);acos((x1_yc(1,k)-D)/sqrt((x1_yc(1,k)-D)^2+x1_yc(3,k)^2))]; S1(:,:,k,i)=H1(:,:,k,i)*p1_yc(:,:,k)*H1(:,:,k,i)'+R(:,:,i)*R(:,:,i); K1(:,:,k,i)=p1_yc(:,:,k)*H1(:,:,k,i)'*inv(S1(:,:,k,i)); x1_gj(:,k,i)=x1_yc(:,k)+K1(:,:,k,i)*(zb(:,k,1)-z1_yc(:,k,i)); p1_gj(:,:,k,i)=(I-K1(:,:,k,i)*H1(:,:,k,i))*p1_yc(:,:,k); end end %%%融合 p1_cross(:,:,k)=(I-K1(:,:,k,1)*H1(:,:,k,1))*(fai*p1_cross(:,:,k-1)*fai'+Q(:,:,1))*(I-K1(:,:,k,2)*H1(:,:,k,2))'; L1(:,:,k)=(p1_gj(:,:,1)-p1_cross(:,:,k))*inv(p1_gj(:,:,1)+p1_gj(:,:,2)-p1_cross(:,:,k)-p1_cross(:,:,k)'); X1_gj(:,k)=x1_gj(:,k,1)+L1(:,:,k)*(x1_gj(:,k,2)-x1_gj(:,k,1)); P1_gj(:,:,k)=p1_gj(:,:,k,1)-L1(:,:,k)*(p1_gj(:,:,k,1)-p1_cross(:,:,k)'); % X1_gj(:,k)=inv(p1_gj(:,:,k,1))*x1_gj(:,k,1)+inv(p1_gj(:,:,k,2))*x1_gj(:,k,2)-p1_yc(:,:,k)*x1_yc(:,k); % P1_gj(:,:,k)=inv(inv(p1_gj(:,:,k,1))+inv(p1_gj(:,:,k,2))-inv(p1_yc(:,:,k))); end end %%%%%目标2的关联情况 for k=1:step if k==1 x2_yc(:,k)=fai*x0(:,2); p2_yc(:,:,k)=fai*p0(:,:,2)*fai'+Q(:,:,2); for i=1:2 %%%分别考察两个观测基站 if i==1 H2(:,:,k,i)=[x2_yc(1,k)/(x2_yc(1,k)^2+x2_yc(3,k)^2),0,x2_yc(3,k)/(x2_yc(1,k)^2+x2_yc(3,k)^2),0;-x2_yc(3,k)/(x2_yc(1,k)^2+x2_yc(3,k)^2),0,x2_yc(1,k)/(x2_yc(1,k)^2+x2_yc(3,k)^2),0]; z2_yc(:,k,i)=[sqrt(x2_yc(1,k)^2+x2_yc(3,k)^2);acos(x2_yc(1,k)/sqrt(x2_yc(1,k)^2+x2_yc(3,k)^2))]; S2(:,:,k,i)=H2(:,:,k,i)*p2_yc(:,:,k)*H2(:,:,k,i)'+R(:,:,i)*R(:,:,i); K2(:,:,k,i)=p2_yc(:,:,k)*H2(:,:,k,i)'*inv(S2(:,:,k,i)); x2_gj(:,k,i)=x2_yc(:,k)+K2(:,:,k,i)*(za(:,k,2)-z2_yc(:,k,i)); p2_gj(:,:,k,i)=(I-K2(:,:,k,i)*H2(:,:,k,i))*p2_yc(:,:,k); else H2(:,:,k,i)=[(x2_yc(1,k)-D)/((x2_yc(1,k)-D)^2+x2_yc(3,k)^2),0,x2_yc(3,k)/((x2_yc(1,k)-D)^2+x2_yc(3,k)^2),0;-x2_yc(3,k)/((x2_yc(1,k)-D)^2+x2_yc(3,k)^2),0,(x2_yc(1,k)-D)/((x2_yc(1,k)-D)^2+x2_yc(3,k)^2),0]; z2_yc(:,k,i)=[sqrt((x2_yc(1,k)-D)^2+x2_yc(3,k)^2);acos((x2_yc(1,k)-D)/sqrt((x2_yc(1,k)-D)^2+x2_yc(3,k)^2))]; S2(:,:,k,i)=H2(:,:,k,i)*p2_yc(:,:,k)*H2(:,:,k,i)'+R(:,:,i)*R(:,:,i); K2(:,:,k,i)=p2_yc(:,:,k)*H2(:,:,k,i)'*inv(S2(:,:,k,i)); x2_gj(:,k,i)=x2_yc(:,k)+K2(:,:,k,i)*(zb(:,k,2)-z2_yc(:,k,i)); p2_gj(:,:,k,i)=(I-K2(:,:,k,i)*H