position_x=app.positionxEditField.Value;
position_y=app.positionyEditField.Value;
velocity_x=app.velocityxEditField.Value;
velocity_y=app.velocityyEditField.Value;
%运动轨迹字符串获取 +vx 17362709369
% eg:20&-6*pi/180&cv 50&6*pi/180&ct 100&-6*pi/180&cv
% 20&0.2&cv 50&1*pi&ct 100&-6*pi/180&cv
% 20&0.2&cv 30&-0.05*pi&ct 50&-6*pi/180&cv 60&-0.05*pi&ct 80&-6*pi/180&cv 100&-0.05*pi&ct
% 20&0.2&cv 30&-0.05*pi&ct 50&-6*pi/180&cv 60&-0.05*pi&ct 80&-6*pi/180&cv 100&-0.05*pi&ct
% Track_text=“20&0.2&ct”;
Track_text=string(app.routesettingEditField.Value);
% 防守位置与个数更改
target_char_array = regexp(app.TargetpositionEditField.Value, '\d+', 'match' );%s是含有数字的字符串
N_char_array = length(target_char_array);
app.Target = {};
for i=1:N_char_array/2
i1=str2double(target_char_array(i));
i2=str2double(target_char_array(N_char_array/2+i));
temp = [i1,i2]';
app.Target=[app.Target, temp];
end
app.Target_vector=cell2mat(app.Target);
% 雷达位置更改
anchor_char_array = regexp(app.RadarpositionEditField.Value, '\d+', 'match' );%s是含有数字的字符串
N_char_array1 = length(anchor_char_array);
app.Anchor = {};
for i=1:N_char_array1/2
i1=str2double(anchor_char_array(i));
i2=str2double(anchor_char_array(N_char_array1/2+i));
temp = [i1,i2]';
app.Anchor=[app.Anchor, temp];
end
app.Anchor_vector=cell2mat(app.Anchor);
%选择追踪算法的更改
Method_EKF=app.EKFCheckBox.Value;
Method_UKF=app.UKFCheckBox.Value;
Method_CKF=app.CKFCheckBox.Value;
%运算处理过程
for i=1:app.MC
% 初始状态的均值和方差
x=[position_x,velocity_x,position_y,velocity_y]'; % 修改轨迹时，除了修改fun_track函数，还要修改这个初始状态：[x位置 x速度 y位置 y速度]
% P_0=diag([100,10,100,10]);
P_0=diag([1e0,10^-1,1e-0,10^-1]);
x0=mvnrnd(x,P_0); % 初始状态
% 各个滤波器初始化
xk_ekf1=x0'; Pk_ekf1=P_0; % P0|0 x0|0
xk_ukf=x0'; Pk_ukf=P_0; % P0|0 x0|0
xk_ckf=x0'; Pk_ckf=P_0; % P0|0 x0|0
%x0=(x+normrnd(0,0.001)')';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 目标运动轨迹产生
% 备注： 轨迹修改为下面fun_track函数，T采样时间（不用变），
% 出了修改轨迹之外，记得修改输入x（初始状态），N总时刻
% 记得还要修改节点布局，不然轨迹很远，节点拓扑很小，定位不上
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[xV] = app_fun_track(x,Track_text,app.T); % x：初始输入状态
app.sV(:,:,i)=xV;% 存储目标运动轨迹
% TDOA测量和AOA测量数生成:第一种AOA测量精度下的测量TDOA/AOA数据
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=1:app.N
tdoam=[];
for m=1:app.M-1 % 该循环表示一次产生三个节点的测量
v_tdoa=app.sigma_tdoa*randn;%noise of the tdoa
[tdoa] = measurements(app.sV(:,k,i), app.Anchor{m}, app.Anchor{app.M});% 最后一个节点作为TDOA的参考节点，即Anchor{M}为参考节点
tdoa=tdoa+v_tdoa;%TDOA测量
tdoam=[tdoam;tdoa];
end
%存储TDOA测量
mV(:,k,i,1)=[tdoam]; %
end
%% 滤波器定位算法
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if Method_EKF==1
for k=1:app.N
%% %%%%%%%%% EKF TDOA定位 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
[xk_ekf1,Pk_ekf1] = fun_2EKF(xk_ekf1,Pk_ekf1,app.Fk,app.Gk,mV(:,k,i,1),app.Qk,app.sigma_tdoa, app.Anchor);
app.PV(:,:,k,i,1)=Pk_ekf1;
app.eV(:,k,i,1)=xk_ekf1;
%%%%%%%%% end filter %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
end
if Method_UKF==1
for k=1:app.N
%% %%%%%%%%% UKF TDOA定位 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
[xk_ukf,Pk_ukf] = fun_2UKF(xk_ukf,Pk_ukf,app.Fk,app.Gk,mV(:,k,i,1),app.Qk,app.sigma_tdoa, app.Anchor);
app.PV(:,:,k,i,2)=Pk_ukf;
app.eV(:,k,i,2)=xk_ukf;
%%%%%%%%% end filter %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
end
if Method_CKF==1
for k=1:app.N
%% %%%%%%%%% CKF TDOA定位 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
[xk_ckf,Pk_ckf] = fun_2CKF(xk_ckf,Pk_ckf,app.Fk,app.Gk,mV(:,k,i,1),app.Qk,app.sigma_tdoa, app.Anchor);
app.PV(:,:,k,i,3)=Pk_ckf;
app.eV(:,k,i,3)=xk_ckf;
%%%%%%%%% end filter %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
end
end
runs=2;% 选择不同蒙特卡洛下的定位轨迹
% h1=plot(x_an(1,1),x_an(2,1),'rp','markersize',7,'LineWidth',0.7);hold on;
% h2=plot(x_un(1,1),x_un(2,1),'b*','markersize',7,'LineWidth',0.7);hold on;
%绘制动图曲线
hold(app.UIAxes ,"off");
h1=plot(app.UIAxes,app.Anchor_vector(1,:),app.Anchor_vector(2,:),'ks','MarkerFaceColor','k','Markersize',7,'LineWidth',1);
hold(app.UIAxes ,"on");
htarget=plot(app.UIAxes,app.Target_vector(1,:),app.Target_vector(2,:),'gs','MarkerFaceColor','g','Markersize',7,'LineWidth',1);
h2 = animatedline(app.UIAxes,'Color','k','Marker','.');
for index=1:N_char_array/2
text(app.UIAxes,app.Target_vector(1,index)-5,app.Target_vector(2,index),num2str(index),"Color",'r');
end
if Method_EKF==1
h3 = animatedline(app.UIAxes,'Color','b','Marker','x');
end
if Method_UKF==1
h4 = animatedline(app.UIAxes,'Color','c','Marker','x');
end
if Method_CKF==1
h5 = animatedline(app.UIAxes,'Color','r','Marker','x');
end
% h6 = animatedline('Color','y','Marker','.');
% 动态图标初始化
data = my_gritsbot_patch;
this.robot_body = data.vertices;
transformed = this.robot_body;
this.robot_handle{1} = patch(app.UIAxes,...
'Vertices', transformed(:, 1:2), ...
'Faces', data.faces, ...
'FaceColor', 'flat', ...
'FaceVertexCData', data.colors, ...
'EdgeColor','none');
xlim(app.UIAxes,[-200, 800]);
ylim(app.UIAxes,[-200, 800]);
for k = 1:app.N
% 动画显示帧率
app.frame_of_vidio=10.0/app.DisplayspeedSlider.Value;
%% %%%%%%%%% 动态图标绘制与更新 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
x = app.sV(1,k,runs);
y = app.sV(3,k,runs);
vx = app.sV(2,k,runs);
vy = app.sV(4,k,runs);
th = atan2(vy, vx)-pi/2;
rotation_matrix = [
cos(th) -sin(th) x;
sin(th) cos(th) y;
0 0 1];
transformed = this.robot_body*rotation_matrix';
set(this.robot_handle{1}, 'Vertices', transformed(:, 1:2));
% 运动方向向量可视化
% quiver(app.UIAxes,x,y,vx,vy,5,'LineWidth',1);
addpoints(h2,app.sV(1,k,runs),app.sV(3,k,runs));
drawnow
value = app.PAUSEButton.Value;
while value==1
pause(0.1);
value = app.PAUSEButton.Value;
end
pause(app.frame_of_vidio);
if Method_EKF==1
addpoints(h3,app.eV(1,k,runs,1),app.eV(3,k,runs,1));
end
if Method_UKF==1
addpoints(h4,app.eV(1,k,runs,2),app.eV(3,k,runs,2));
end
if Method_CKF==1
addpoints(h5,app.eV(1,k,runs,3),app.eV(3,k,runs,3));
end
% 对目标威胁程度
if mod(k,3)==0
app.num_Target=length(app.Target);
text_threat_factor='';
for i_target=1:app.num_Target
if Method_CKF==1
threat_factor=fun_threat_factor(app.eV(:,k,runs,3),app.Target{i_target},-3);
elseif Method_UKF==1
threat_factor=fun_threat_factor(app.eV(:,k,runs,2),app.Target{i_target},-3);
elseif Method_EKF==1
threat_factor=fun_threat_factor(app.eV(:,k,runs,1),app.Target{i_target},-3);
else
threat_factor=0;
end
text_threat_factor=[text_threat_factor,num2str(i_target),' ',num2str(threat_factor),' '];
% text_threat_factor((i_target-1)*10+1)=num2str(i_target);
% text_threat_factor((i_target-1)10+3:(i_target-1)10+7)=num2str(threat_factor);
end
app.ThreatofTargetEditField.Value=string(text_threat_factor);
end
% [distance2,speed2]=fun_threat_factor(eV(:,k,runs,1),Target{2});
% [distance3,speed3]=fun_threat_factor(eV(:,k,runs,1),Target{3});
% addpoints(h6,k,500threat_factor1);
%% %%%%%%%%%%%%%%%%%%%%%% end %%%%%%%%%%%%%%%%%%%%%%%%%%%%
end
% h2=plot(sV(1,:,runs),sV(3,:,runs),‘-ko’,‘LineWidth’,1);
% h3=plot(eV(1,:,runs,1),eV(3,:,runs,1),‘-.bd’,‘LineWidth’,1);
% h4=plot(eV(1,:,runs,2),eV(3,:,runs,2),‘:c^’,‘LineWidth’,1);
% h5=plot(eV(1,:,runs,3),eV(3,:,runs,3),'-r’,‘LineWidth’,1);
% axis([-3 23,-3,23])
% X Y 跟踪位置和速度轨迹
% 指定图的位置
ax1 = subplot(2, 2, 1, 'Parent', app.Panel);
ax2 = subplot(2, 2, 2, 'Parent', app.Panel);
ax3 = subplot(2, 2, 3, 'Parent', app.Panel);
ax4 = subplot(2, 2, 4, 'Parent', app.Panel);
hold([ax1,ax2,ax3,ax4],"off");
ii=1:app.N;
plot(ax1,ii,app.sV(1,:,1), '-ko');%x方向位移
plot(ax2,ii,app.sV(3,:,1),'-ko');%y方向位移
plot(ax3,ii,app.sV(2,:,1),'-ko');%x方向速度
plot(ax4,ii,app.sV(4,:,1),'-ko');%x方向速度
hold([ax1,ax2,ax3,ax4],"on");
title(ax1,'X轴的位置轨迹');
title(ax2,'Y轴的位置轨迹');
title(ax3,'X轴的速度轨迹');
title(ax4,'Y轴的速度轨迹');
详细请咨询 vx 17362709369
1053
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
