waterwaytrace.m

北京到广州铁路路线仿真

clear all;
cq=[106.45,29.57,0]; % 重庆
ych=[111.3,30.7,0]; % 宜昌
wh=[114.32,30.52,0]; % 武汉
jj=[115.97,29.72,0]; % 九江
shh=[121.4,31.2,0]; % 上海
%选取重庆、宜昌、武汉、九江、上海五个城市为站点；
%设轮船出站从零开始加速，速度达到20m/s，开始匀速运动；
%快要到达下一站点时，开始减速，设减速与加速时间相同；
%假设加速时间为10分钟；
%按段划分，重庆到宜昌0，宜昌到武汉1，武汉到九江2，九江到上海3；
%每一段又分为加速0，匀速1，减速2；
%下划线表示某一点的数值，非下划线则表示这一段的所有采样点的矩阵
%例如T0_0,T0_1,T0_2分别表示重庆到宜昌这一段上加速、匀速、减速的起始时间点
%例如lat11,lat12,lat13分别表示宜昌到武汉的加速、匀速、减速段上的所有的采样点的纬度矩阵
%==============================START=======================================
%-------------------------------重庆到宜昌----------------------------------
[Xu_cq,Yu_cq,Zu_cq]=user_ecef(cq(1),cq(2),cq(3));
[Xu_ych,Yu_ych,Zu_ych]=user_ecef(ych(1),ych(2),ych(3));
Discq_ych=sqrt((Xu_cq-Xu_ych)^2+(Yu_cq-Yu_ych)^2+(Zu_cq-Zu_ych)^2);
[Xug_ych,Yug_ych,Zug_ych]=ecef_g(cq(1),cq(2),Xu_ych,Yu_ych,Zu_ych,...
                                    Xu_cq,Yu_cq,Zu_cq);
theta0 = atan(Yug_ych/Xug_ych);

%-------加速
T0_0=0;
dt=10;
[long01,lat01,v01]=trace_acc_water(cq(1),cq(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_water(long0_1,lat0_1,theta0,V0_1,T0_1,...
                          Xu_cq,Yu_cq,Zu_cq,Discq_ych);
%-------减速进站
long0_2=long02(length(long02));
lat0_2=lat02(length(lat02));
T0_2=dt*(length(long02)+length(long01));
[long03,lat03]=trace_dece_water(long0_2,lat0_2,theta0,T0_2);
%-----------------------------宜昌到武汉------------------------------------
[Xu_wh,Yu_wh,Zu_wh]=user_ecef(wh(1),wh(2),wh(3));
Disych_wh=sqrt((Xu_ych-Xu_wh)^2+(Yu_ych-Yu_wh)^2+(Zu_ych-Zu_wh)^2);
[Xug_wh,Yug_wh,Zug_wh]=ecef_g(ych(1),ych(2),Xu_wh,Yu_wh,Zu_wh,...
                                 Xu_ych,Yu_ych,Zu_ych);
theta1=atan(Yug_wh/Xug_wh);
%------ 加速
T1_0=0;%起始时间可以任意设置，这里我们假设为0，对结果不会造成影响
[long11,lat11,v11]=trace_acc_water(ych(1),ych(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_water(long1_1,lat1_1,theta1,V1_1,T1_1,...
                          Xu_ych,Yu_ych,Zu_ych,Disych_wh);
%------减速进站
long1_2=long12(length(long12));
lat1_2=lat12(length(lat12));
T1_2=dt*(length(long11)+length(long12));
[long13,lat13]=trace_dece_water(long1_2,lat1_2,theta1,T1_2);
%----------------------------武汉到九江------------------------------------
[Xu_jj,Yu_jj,Zu_jj]=user_ecef(jj(1),jj(2),jj(3));
Diswh_jj=sqrt((Xu_wh-Xu_jj)^2+(Yu_wh-Yu_jj)^2+(Zu_wh-Zu_jj)^2);
[Xug_jj,Yug_jj,Zug_jj]=ecef_g(jj(1),jj(2),Xu_jj,Yu_jj,Zu_jj,...
                                  Xu_wh,Yu_wh,Zu_wh);
theta2= atan(Yug_jj/Xug_jj);
%-------加速
T2_0=0;
[long21,lat21,v21]=trace_acc_water(wh(1),wh(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_water(long2_1,lat2_1,theta2,V2_1,T2_1,...
                          Xu_wh,Yu_wh,Zu_wh,Diswh_jj);
%--------减速进站
long2_2=long22(length(long22));
lat2_2=lat22(length(lat22));
T2_2=dt*(length(long21)+length(long22));
[long23,lat23]=trace_dece_water(long2_2,lat2_2,theta2,T2_2);
%---------------------------九江到上海-------------------------------------
[Xu_shh,Yu_shh,Zu_shh]=user_ecef(shh(1),shh(2),shh(3));
Disjj_shh=sqrt((Xu_jj-Xu_shh)^2+(Yu_jj-Yu_shh)^2+(Zu_jj-Zu_shh)^2);
[Xug_shh,Yug_shh,Zug_shh]=ecef_g(shh(1),shh(2),Xu_shh,Yu_shh,Zu_shh,...
                                  Xu_jj,Yu_jj,Zu_jj);
theta3=atan(Yug_shh/Xug_shh);
%-------加速
T3_0=0;
[long31,lat31,v31]=trace_acc_water(jj(1),jj(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_water(long3_1,lat3_1,theta3,V3_1,T3_1,...
                          Xu_jj,Yu_jj,Zu_jj,Disjj_shh);
%-------减速进站
long3_2=long32(length(long32));
lat3_2=lat32(length(lat32));
T3_2=dt*(length(long31)+length(long32));
[long33,lat33]=trace_dece_water(long3_2,lat3_2,theta3,T3_2);
%=====根据程序计算出宜昌、武汉、九江、上海的经纬度（也可以计算途中任意采样点的经纬度）=======
waterlong = [long01,long02,long03,long11,long12,long13,long21,long22,long23,long31,long32,long33];
waterlat = [lat01,lat02,lat03,lat11,lat12,lat13,lat21,lat22,lat23,lat31,lat32,lat33];
ychjs=[long03(length(long03)),lat03(length(lat03))]
whjs=[long13(length(long13)),lat13(length(lat13))]
jjjs=[long23(length(long23)),lat23(length(lat23))]
shhjs=[long33(length(long33)),lat33(length(lat33))]