clear all;
bj=[116.46,39.92,0];
zhzh=[113.7,34.5,0];
wch=[114.32,30.35,0];
chsh=[112.92,28.33,0];
gzh=[113.25,23.13,0];
%選取北京、鄭州、武昌、長(zhǎng)沙、廣州五個(gè)城市為站點(diǎn)；
%設(shè)火車出站從零開始加速，速度達(dá)到180km/h即50m/s時(shí)，開始勻速運(yùn)動(dòng)；
%快要到達(dá)下一站點(diǎn)時(shí)，開始減速，設(shè)減速與加速時(shí)間相同；
%假設(shè)加速時(shí)間為10分鐘；
%按段劃分，北京到鄭州0，鄭州到武昌1，武昌到長(zhǎng)沙2，長(zhǎng)沙到廣州3；
%每一段又分為加速0，勻速1，減速2；
%下劃線表示某一點(diǎn)的數(shù)值，非下劃線則表示這一段的所有采樣點(diǎn)的矩陣
%例如T0_0,T0_1,T0_2表示北京到鄭州
%-------------------------------北京到鄭州----------------------------------
[Xu_bj,Yu_bj,Zu_bj]=user_ecef(bj(1),bj(2),bj(3));
[Xu_zhzh,Yu_zhzh,Zu_zhzh]=user_ecef(zhzh(1),zhzh(2),zhzh(3));
Disbj_zhzh=sqrt((Xu_bj-Xu_zhzh)^2+(Yu_bj-Yu_zhzh)^2+(Zu_bj-Zu_zhzh)^2);
[Xug_zhzh,Yug_zhzh,Zug_zhzh]=ecef_g(bj(1),bj(2),Xu_zhzh,Yu_zhzh,Zu_zhzh,...
                                    Xu_bj,Yu_bj,Zu_bj);
theta0=pi+atan(Yug_zhzh/Xug_zhzh);
%先計(jì)算鄭州在北京的當(dāng)?shù)赜脩羲阶鴺?biāo)下的位置，然后計(jì)算出鄭州相對(duì)北京的角度theta

%-------加速
T0_0=0;
dt=10;
[long01,lat01,v01]=trace_acc(bj(1),bj(2),theta0,T0_0);
%-------勻速
T0_1=dt*length(long01);
V0_1=v01(length(long01));
long0_1=long01(length(long01));
lat0_1=lat01(length(lat01));
[long02,lat02]=trace_even(long0_1,lat0_1,theta0,V0_1,T0_1,...
                          Xu_bj,Yu_bj,Zu_bj,Disbj_zhzh);
%-------減速進(jìn)站
long0_2=long02(length(long02));
lat0_2=lat02(length(lat02));
T0_2=dt*(length(long02)+length(long01));
[long03,lat03]=trace_dece(long0_2,lat0_2,theta0,T0_2);
%-----------------------------鄭州到武昌------------------------------------
[Xu_wch,Yu_wch,Zu_wch]=user_ecef(wch(1),wch(2),wch(3));
Diszhzh_wch=sqrt((Xu_zhzh-Xu_wch)^2+(Yu_zhzh-Yu_wch)^2+(Zu_zhzh-Zu_wch)^2);
[Xug_wch,Yug_wch,Zug_wch]=ecef_g(zhzh(1),zhzh(2),Xu_wch,Yu_wch,Zu_wch,...
                                 Xu_zhzh,Yu_zhzh,Zu_zhzh);
theta1=atan(Yug_wch/Xug_wch);
%------ 加速
T1_0=0;%起始時(shí)間可以任意設(shè)置，這里我們假設(shè)為0，對(duì)結(jié)果不會(huì)造成影響
[long11,lat11,v11]=trace_acc(zhzh(1),zhzh(2),theta1,T1_0);
%------勻速
T1_1=dt*length(long11);
V1_1=v11(length(long11));
long1_1=long11(length(long11));
lat1_1=lat11(length(lat11));
[long12,lat12]=trace_even(long1_1,lat1_1,theta1,V1_1,T1_1,...
                          Xu_zhzh,Yu_zhzh,Zu_zhzh,Diszhzh_wch);
%------減速進(jìn)站
long1_2=long12(length(long12));
lat1_2=lat12(length(lat12));
T1_2=dt*(length(long11)+length(long12));
[long13,lat13]=trace_dece(long1_2,lat1_2,theta1,T1_2);
%----------------------------武昌到長(zhǎng)沙------------------------------------
[Xu_chsh,Yu_chsh,Zu_chsh]=user_ecef(chsh(1),chsh(2),chsh(3));
Diswch_chsh=sqrt((Xu_wch-Xu_chsh)^2+(Yu_wch-Yu_chsh)^2+(Zu_wch-Zu_chsh)^2);
[Xug_chsh,Yug_chsh,Zug_chsh]=ecef_g(chsh(1),chsh(2),Xu_chsh,Yu_chsh,Zu_chsh,...
                                  Xu_wch,Yu_wch,Zu_wch);
theta2=pi+atan(Yug_chsh/Xug_chsh);
%-------加速
T2_0=0;
[long21,lat21,v21]=trace_acc(wch(1),wch(2),theta2,T2_0);
%-------勻速
T2_1=dt*length(long21);
V2_1=v21(length(long21));
long2_1=long21(length(long21));
lat2_1=lat21(length(lat21));
[long22,lat22]=trace_even(long2_1,lat2_1,theta2,V2_1,T2_1,...
                          Xu_wch,Yu_wch,Zu_wch,Diswch_chsh);
%--------減速進(jìn)站
long2_2=long22(length(long22));
lat2_2=lat22(length(lat22));
T2_2=dt*(length(long21)+length(long22));
[long23,lat23]=trace_dece(long2_2,lat2_2,theta2,T2_2);
%---------------------------長(zhǎng)沙到廣州-------------------------------------
[Xu_gzh,Yu_gzh,Zu_gzh]=user_ecef(gzh(1),gzh(2),gzh(3));
Dischsh_gzh=sqrt((Xu_chsh-Xu_gzh)^2+(Yu_chsh-Yu_gzh)^2+(Zu_chsh-Zu_gzh)^2);
[Xug_gzh,Yug_gzh,Zug_gzh]=ecef_g(gzh(1),gzh(2),Xu_gzh,Yu_gzh,Zu_gzh,...
                                  Xu_chsh,Yu_chsh,Zu_chsh);
theta3=atan(Yug_gzh/Xug_gzh);
%-------加速
T3_0=0;
[long31,lat31,v31]=trace_acc(chsh(1),chsh(2),theta3,T3_0);
%-------勻速
T3_1=dt*length(long31);
V3_1=v31(length(long31));
long3_1=long31(length(long31));
lat3_1=lat31(length(lat31));
[long32,lat32]=trace_even(long3_1,lat3_1,theta3,V3_1,T3_1,...
                          Xu_chsh,Yu_chsh,Zu_chsh,Dischsh_gzh);
%-------減速進(jìn)站
long3_2=long32(length(long32));
lat3_2=lat32(length(lat32));
T3_2=dt*(length(long31)+length(long32));
[long33,lat33]=trace_dece(long3_2,lat3_2,theta3,T3_2);
%=====根據(jù)程序計(jì)算出鄭州、武昌、長(zhǎng)沙、廣州的經(jīng)緯度（也可以計(jì)算途中任意采樣點(diǎn)的經(jīng)緯度）=======
zhzhjs=[long03(length(long03)),lat03(length(lat03))];
wchjs=[long13(length(long13)),lat13(length(lat13))];
chshjs=[long23(length(long23)),lat23(length(lat23))];
gzhjs=[long33(length(long33)),lat33(length(lat33))];