纯方位角目标跟踪 EKF状态估计

纯方位目标运动分析是一种隐蔽突击的有效方法，它是利用目标本身的有源辐射，如电磁波辐射、红外辐射、声波辐射、目标对照射的散射。甚至目标施放的干扰辐射等，采用机动单站测向机对目标定位与跟踪。这种对运动目标的隐蔽定位与跟踪是反电子对抗、反水声对抗、反侦察，实施对目标突然袭击的十分有效的手段。在现代战争实际环境里，通常测得的敌机（舰）特征数据是非常有限的。而目标的方位几乎成了唯一可靠的参数。因此可以
利用所测得的目标方位角信息估计目标的运动参数（位置、速度、加速度等），从而实施对敌机（舰）的有效打击和电子干扰。
假定观测站对某匀速直线运动的目标进行纯方位跟踪，假定观测站已知目标的初始状态。
基于纯方位的目标运动模型可以写成如下形式∶
X ( k ) = Φ X ( k − 1 ) + T U ( k ) X(k)= \Phi X(k-1)+\Tau U(k) X(k)=ΦX(k−1)+TU(k)
Z ( k ) = a r c t a n y ( k ) − y 0 x ( k ) − x 0 + V ( k ) Z(k)=arctan{\frac{y(k)-y_0}{x(k)-x_0}} +V(k) Z(k)=arctanx(k)−x0y(k)−y0+V(k)

% function EKF_Angle
T=1; %雷达扫描周期
N=40/T; %。总的采样次数
F=[ 1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1]; %状态转移矩阵
G=[T^2/2,0;T,0;0,T^2/2;0,T]; %过程噪声驱动矩阵
delta_w= 1e-4; %如果增大这个参数,目标真实轨迹就是曲线了
Q =delta_w * diag([1,1]); %过程噪声均值
R=0.01 * pi/180; %观测噪声方差，读者可以修改此值观察其对角度测量的影响
W = sqrtm( Q) * randn( 2,N); %过程噪声
V=sqrt( R) * randn( 1,N); %观测噪声
%观测站的位置,可以设为其他值
station.x=0;
station.y =1000;
%% %% %%%%%%%%%%%%%%%%%%%%% % %%%%%%%%%%%%%%%%% %%
X= zeros( 4,N); %目标真实位置、速度
X( :,1)=[0,2,1400,-10]; %目标初始位置、速度
Z=zeros( 1,N); %传感器对位置的观测
for k=2:NX( : ,k)=F*X( :,k-1)+G* W( :,k); %目标真实轨迹
end
for k= 1 :Ntarget.x=X( 1,k) ; target.y=X( 3,k);Z( k )= hfun( target , station)+V(k); % 对目标观测
end
% EKF滤波
Xekf = zeros( 4,N );
Xekf( : ,1)= X( :,1 ); %Kalman滤波状态初始化
P0=eye( 4); %协方差阵初始化
for i= 2:NXn=F * Xekf( : , i-1) ; %。预测P1=F* P0 * F'+G*Q* G'; %预测误差协方差target.x = Xn( 1 ) ; target.y = Xn( 3 ) ;dd = hfun( target , station) ; %。观测预测%求雅可比矩阵HD= Dist( target , station ) ;H=[-( Xn(3, 1) -station.y)/ D,0,(Xn(1,1)-station.x)/D,0];K=P1*H'* inv(H*P1* H'+R); %增益Xekf( : ,i)= Xn+K*(Z(:,i )-dd ) ; %状态更新P0= ( eye(4)-K* H) * P1; %滤波误差协方差更新
end
%误差分析
for i= 1 :Ntarget.x=X( 1 , i ) ;target.y=X( 3, i );s.x = Xekf( 1 , i ) ;s. y =Xekf( 3, i);Err_KalmanFilter( i)= Dist ( target , s ) ;%滤波后误差
end
% %%%% %%%%%%%%%%%%% %% %%%% %%%%%%%%%%%%%%%% %%%%%%%画图
figure
hold on ; box on;
plot( X( 1,:),X( 3, : ) , '-k.' ); %真实轨迹
hold on;
plot( Xekf( 1 , : ) ,Xekf( 3, : ) , '-r+'); %扩展Kalman滤波轨迹
legend('真实轨迹','EKF轨迹');figure
hold on; box on;
plot( Err_KalmanFilter , '-ks' , 'MarkerFace' , 'r');figure
hold on; box on;
plot ( Z/ pi * 180-V/ pi * 180 , '-r. ' , 'MarkerFace' , 'r' );%真实角度值
plot( Z/ pi * 180 , '-ko' , 'MarkerFace' , 'g'); % 受噪声污染的观测值
legend('真实角度','观测角度');
%%%%%%%%% %%%%%%% %%%%%%%% %%%%% %%% %%% %% %%%%% %% %%
%子函数
function cita = hfun( X1 ,X0)
cita= atan2( X1.y-X0.y ,X1.x-X0.x);
end
function d= Dist( X1,X2)
d = sqrt(( X1.x-X2.x)^2+(X1.y-X2.y)^2);
end
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%

纯方位角目标跟踪 EKF状态估计相关推荐

（从零开始）基于检测前跟踪雷达目标跟踪技术的研究：第（7）周所学知识（PF-TBD感悟：纯干货，全网你绝对搜不到下面的干货）
若由本篇博文增加关注,就解封本篇博文的VIP权限哈,记得在下方留言哈本周上点对于基于PF-TBD的雷达目标跟踪的超级干货.下面的这些全网搜索都不一定找得到(至少我看了许多的博文与文献还没有下面的这些 ...
无迹卡尔曼滤波UKF—目标跟踪中的应用(仿真部分)
无迹卡尔曼滤波UKF-目标跟踪中的应用(仿真部分) 原创不易,路过的各位大佬请点个赞机动目标跟踪/非线性滤波/传感器融合/导航等探讨联系WX: ZB823618313 算法部分见博客: [无迹卡尔曼 ...
交互式多模型-粒子滤波IMM-PF—在机动目标跟踪中的应用/matlab实现
交互式多模型-粒子滤波IMM-PF-在机动目标跟踪中的应用/matlab实现原创不易,路过的各位大佬请点个赞 WX: ZB823618313 交互式多模型-粒子滤波IMM-PF-在机动目标跟踪中的应 ...
机动目标跟踪——匀加速运动CA模型(二维)
机动目标跟踪--匀加速运动CA模型(二维) 原创不易,路过的各位大佬请点个赞 WX: ZB823618313 机动目标跟踪--目标模型概述机动目标跟踪--匀加速运动CA模型(二维) 1. 对机动目标 ...
机动目标跟踪——匀加速运动模型（三维）
机动目标跟踪--匀加速运动模型(三维) 原创不易,路过的各位大佬请点个赞 WX: ZB823618313 机动目标跟踪--目标模型概述机动目标跟踪--匀加速运动模型(三维) 1. 对机动目标跟踪的理 ...
目标跟踪ObjectT综述介绍
此文也很详细:http://blog.csdn.net/maochongsandai110/article/details/11530045 原文链接:http://blog.csdn.net/pp5 ...
kalman滤波在雷达目标跟踪中的应用_简述Automotive radar中的多目标跟踪处理
毫米波雷达在目前的先进辅助驾驶(ADAS)中的应用主要体现在自适应巡航控制(Adaptive Cruise Control,ACC),预碰撞(PreCrash),泊车辅助(Parking Aid),变 ...
单目标跟踪算法调研（2020）
Deformable Siamese Attention Networks for Visual Object Tracking(SiamAttn,2020) 背景:基于孪生结构的跟踪器在视觉目标跟踪 ...
90+目标跟踪算法九大benchmark！基于判别滤波器和孪生网络的视觉目标跟踪：综述与展望...
点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[目标跟踪]技术交流群后台回复[视觉目标跟踪综述]获取本文论文! 准确和鲁棒的视觉目标 ...

纯方位角目标跟踪 EKF状态估计

纯方位角目标跟踪 EKF状态估计相关推荐

最新文章

热门文章