function [xV] = app_fun_track_n(X,Track_text_n,T,n_uav)
% 备注： 轨迹修改为下面fun_track函数，T采样时间（不用变），
% 出了修改轨迹之外，记得修改输入x（初始状态），N总时刻
% 转弯模型，还要修改转弯角速度，即w1和w2
% 记得还要修改节点布局，不然轨迹很远，节点拓扑很小，定位不上
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 目标运动轨迹产生
%% 解析track_text
%模型2:CV模型
Fk_cv=[1 T 0 0;
0 1 0 0;
0 0 1 T;
0 0 0 1;];
q2=0.03; % 目标运动学标准差，过程噪声
Qk_cv=q2^2*[T^3/3, T^2/2, 0, 0;
T^2/2, T^2, 0, 0;
0, 0, T^3/3, T^2/2;
0, 0, T^2/2, T^2 ;];% 过程噪声协方差
Gk_cv=eye(4);
X_init=X;
Track_text = regexp(Track_text_n,'\ \|\ ','split');
for num=1:n_uav
X=X_init;
splitStr = regexp(Track_text(num),'\ ','split');
t_start=0;
t_end=0;
speed=0;
speed_last=sqrt(X(2)^2+X(4)^2);
N=length(splitStr);
for i=1:N
split_split_Str = regexp(splitStr(i),'\&','split');
% 提取时间
t_end=str2double(split_split_Str(1));
% 分离速度和旋转角速度
split_split_split_Str = regexp(split_split_Str(2),'\|','split');
% 提取运行速度
speed=str2double(split_split_split_Str(1));
factor=speed*1.0/speed_last;
X(2)=X(2)*factor;
X(4)=X(4)*factor;
% 提取旋转角速度
num_fraction_w1=regexp(split_split_split_Str(2), '\d+', 'match');
num_decimal_w1=regexp(split_split_split_Str(2), '\d+[.]\d+', 'match');
num_pi=regexp(split_split_split_Str(2), 'pi', 'match','ignorecase');
if length(num_decimal_w1)==1
xiaoshu=str2double(num_decimal_w1(1));
w1=1.0*xiaoshu;
elseif length(num_fraction_w1)==2
% 分数
fenzi=str2double(num_fraction_w1(1));
fenmu=str2double(num_fraction_w1(2));
w1=1.0*fenzi/fenmu;
else
% 整数
zhengshu=str2double(num_fraction_w1(1));
w1=1.0*zhengshu;
end
if ~isempty(num_pi)
w1=w1*pi;
end
temp=char(split_split_split_Str(2));
if isequal(temp(1),"-")
w1=w1*-1;
end
n=length(X);% 状态维数
%% 轨迹分阶段生成
if isequal(split_split_Str(3),'CT')||isequal(split_split_Str(3),'ct')
%% target model
% 模型1：CT模型参数
Fk_ct1=[1 sin(w1*T)/w1 0 -(1-cos(w1*T))/w1 ;
0 cos(w1*T) 0 -sin(w1*T) ;
0 (1-cos(w1*T))/w1 1 sin(w1*T)/w1 ;
0 sin(w1*T) 0 cos(w1*T) ;]; % 转弯模型
q1=0.03; % 目标运动学标准差，过程噪声
Qk_ct1= q1^2*[2*(w1*T-sin(w1*T))/w1^3 (1-cos(w1*T))/w1^2 0 (w1*T-sin(w1*T))/w1^2 ;
(1-cos(w1*T))/w1^2 T -(w1*T-sin(w1*T))/w1^2 0 ;
0 -(w1*T-sin(w1*T))/w1^2 2*(w1*T-sin(w1*T))/w1^3 (1-cos(w1*T))/w1^2 ;
(w1*T-sin(w1*T))/w1^2 0 (1-cos(w1*T))/w1^2 T;];
Gk_ct1=eye(4);
for k=t_start+1:t_end
X = Fk_ct1*X + Gk_ct1*sqrtm(Qk_ct1)*randn(n,1); %CT
% X = Fk_ct2*X; %CT
xV(:,k,num) = X;
end
elseif isequal(split_split_Str(3),'CV')||isequal(split_split_Str(3),'cv')
for k=t_start+1:t_end
X = Fk_cv*X + Gk_cv*sqrtm(Qk_cv)*randn(n,1); %CV， 轨迹生成加过程噪声
% % X = Fk_cv*X ; %CV， 轨迹生成不加过程噪声
xV(:,k,num)=X;
end
end
t_start=t_end;
speed_last=speed;
end
end
