devbox
Cpp五条
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

TH方程学习（4）

作者：Cpp五条 | 2024-06-01 16:16:17

赞

踩

TH方程学习（4）

一、背景介绍

在本节将会对TH方程打包成一个函数，通过输入目标星的轨道要素，追踪星在目标星VVLH坐标系下的相对位置和速度，以及预报的时间，得到预报时刻追踪星在VVLH坐标系下的位置和速度，以及整个状态转移矩阵。

合并（1）（2），得到归一化形式的XZ方向的状态转移矩阵\Phi_{XZ}，（3），得到归一化形式的Y方向的状态转移矩阵\Phi_{Y}。所以合并后的形式可以写为\tilde{\Phi}=\begin{bmatrix} \Phi_{XZ}(1,1) & 0 &\Phi_{XZ}(1,2) & \Phi_{XZ}(1,3) & 0&\Phi_{XZ}(1,4) \\0 & \Phi_{Y}(1,1) &0& 0 & \Phi_{Y}(1,2) &0 \\ \Phi_{XZ}(2,1) & 0 & \Phi_{XZ}(2,2) & \Phi_{XZ}(2,3) & 0 &\Phi_{XZ}(2,4) \\ \Phi_{XZ}(3,1) & 0 & \Phi_{XZ}(3,2) & \Phi_{XZ}(3,3)& 0 & \Phi_{XZ}(3,4)\\ 0 & \Phi_{Y}(2,1) & 0 & 0 &\Phi_{Y}(2,2) & 0 \\ \Phi_{XZ}(4,1) & 0 & \Phi_{XZ}(4,2) & \Phi_{XZ}(4,3) &0 & \Phi_{XZ}(4,4) \end{bmatrix}

根据归一化，可以写成的矩阵形式如下所示

\tilde{\boldsymbol{X}}=\Phi_{\boldsymbol{X}}\boldsymbol{X}

\Phi_{X}=\begin{bmatrix} \rho & & & & & \\ & \rho & & & & \\ & & \rho & & & \\ -esin \theta & & & \frac{1}{k^2\rho} & & \\ & -e sin \theta & & & \frac{1}{k^2\rho} & \\ & & -e sin \theta & & & \frac{1}{k^2\rho} \end{bmatrix}

\Phi_{X}^{-1}=\begin{bmatrix} \frac{1}{\rho} & & & & & \\ & \frac{1}{\rho} & & & & \\ & & \frac{1}{\rho} & & & \\ k^2e\sin\theta & & & k^2\rho & & \\ & k^2e\sin\theta & & & k^2\rho & \\ & & k^2e\sin\theta & & & k^2\rho \end{bmatrix}

根据式子

\tilde{X}_{t}=\tilde{\Phi}\tilde{X}_0\\ \Phi_{X_{t}}X_t=\tilde{\Phi}\Phi_{X_0}X_0

所以状态转移矩阵为

\Phi=\Phi_{X_{t}}^{-1}\tilde{\Phi}_{X}\Phi_{X0}

实现代码如下所示

  1. function [rvt,Phi,rvt_phi] = TH_solver(ecc,Perigee,TA,delta_r,delta_v,t)
  2. %TH_SOLVER 本函数旨在通过输入目标轨道的轨道要素，追踪星的初始相对位置和速度
  3. %  输入
  4. %  ecc    :  目标星的偏心率           0<ecc<1
  5. %  Perigee:  目标星的近地点高度       (km)
  6. %  TA     :  目标星的初始真近地点角   (deg) (0<TA<=360)
  7. %  delta_r:  追踪星在目标星的VVLH位置 (km)
  8. %  delta_v:  追踪星在目标星的VVLH速度 (km/s)
  9. %  t      :  预报时间长度             (s)
  10. %  输出
  11. %  rvt    :  t时刻后，追踪星在目标星VVLH坐标系的位置和速度 (km,km/s)
  12. %  Phi    :  状态转移矩阵               
  13. %  rvt_phi:  通过状态转移矩阵计算出来的追踪星在目标星VVLH坐标系的位置和速度（用来与rvt做一个对比）
  14. %% 参考文献<New State Transition Matrix for Relative Motion on an Aribitrary Elliptical Orbit>
  15. %% 作者 Yamanaka Koji
  16. global miu Re
  17. miu = 3.986e5;
  18. Re  = 6378.137;
  19. % 计算半长轴
  20. sma =    (Re+Perigee)/(1-ecc);
  21. % 计算半通径
  22. p  =     sma*(1-ecc^2);
  23. % 计算角动量
  24. h  =     sqrt(p*miu);
  25. % 计算归一化的相对位置速度
  26. rho     = 1+ecc*cosd(TA);
  27. r_norm  = rho*delta_r;                                   %式子87
  28. k       = sqrt(h/p^2);
  29. v_norm  = -ecc*sind(TA)*delta_r+(1/(k^2*rho))*delta_v;   %式子87 
  30.  
  31. % 计算不同时刻的真近地点角
  32. tanf=tand(TA/2);
  33. ee=sqrt((1+ecc)/(1-ecc));
  34. tanE=tanf/ee;
  35. % 求偏近地点角
  36. if TA<=180
  37.     E = atand(tanE)*2;
  38. else
  39.     E=  atand(tanE)*2+360;
  40. end
  41. % 求平近地点角
  42. M = rad2deg(deg2rad(E)-ecc*sind(E));
  43. % 求平均角速度
  44. n = sqrt(miu/sma^3);
  45. % 平近地点角
  46. M_all1=M+rad2deg(n*t);
  47. if M_all1>360
  48.     int=floor(M_all1/360);
  49.     M_all=M_all1-int*360;
  50. else
  51.     M_all=M_all1;
  52. end
  53.  
  54. % 利用开普勒方程求解每个时刻的偏近地点角,真近地点角
  55. MM=deg2rad(M_all);
  56. E_rad=Kepler_function(ecc,MM);
  57. tanf2=sqrt((1+ecc)/(1-ecc))*tan(E_rad/2);
  58. if E_rad<pi && E_rad>=0
  59.     f=rad2deg(atan(tanf2)*2);
  60.     f_all=f;
  61. elseif E_rad>=pi
  62.     f=rad2deg(atan(tanf2)*2)+360;
  63.     f_all=f;
  64. end
  65.  
  66. % 计算X-Z矩阵
  67. s0            = rho*sind(TA); c0= rho*cosd(TA);
  68. Phi0_inv      =  zeros(4,4);
  69. Phi0_inv(1,1) =  1-ecc^2;
  70. Phi0_inv(1,2) =  3*ecc*(s0/rho)*(1+1/rho);
  71. Phi0_inv(1,3) =  -ecc*s0*(1+1/rho);
  72. Phi0_inv(1,4) =  -ecc*c0+2;
  73. Phi0_inv(2,2) =  -3*(s0/rho)*(1+ecc^2/rho);
  74. Phi0_inv(2,3) =  s0*(1+1/rho);
  75. Phi0_inv(2,4) =  c0-2*ecc;
  76. Phi0_inv(3,2) =  -3*(c0/rho+ecc);
  77. Phi0_inv(3,3) =  c0*(1+1/rho)+ecc;
  78. Phi0_inv(3,4) =  -s0;
  79. Phi0_inv(4,2) =  3*rho+ecc^2-1;
  80. Phi0_inv(4,3) =  -rho^2;
  81. Phi0_inv(4,4) =  ecc*s0;
  82. Phi0_inv1     =  Phi0_inv.*(1/(1-ecc^2));
  83.  
  84. % 根据f_all 计算每个时刻对应的转移矩阵 X-Z
  85. xz0     =   [r_norm(1);r_norm(3);v_norm(1);v_norm(3)];
  86. y0      =   [r_norm(2);v_norm(2)];
  87. % 计算转移初始值
  88. hatxz0  =   Phi0_inv1*xz0;
  89. % 转移矩阵
  90. % 已知 t 时刻的真近地点角
  91. theta   =    f_all;
  92. % 计算矩阵的参数
  93. rho1    =    1+ecc*cosd(theta);
  94. s1      =    rho1*sind(theta);
  95. c1      =    rho1*cosd(theta);
  96. ds1     =    cosd(theta)+ecc*cosd(2*theta);
  97. dc1     =    -(sind(theta)+ecc*sind(2*theta));
  98. J1      =    k^2*t;
  99. %  转移矩阵的参数
  100. Phi_theta       =  zeros(4);
  101. Phi_theta(1,1)  =  1;
  102. Phi_theta(1,2)  =  -c1*(1+1/rho1);
  103. Phi_theta(1,3)  =  s1*(1+1/rho1);
  104. Phi_theta(1,4)  =  3*rho1^2*J1;
  105. Phi_theta(2,2)  =  s1;
  106. Phi_theta(2,3)  =  c1;
  107. Phi_theta(2,4)  =  2-3*ecc*s1*J1;
  108. Phi_theta(3,2)  =  2*s1;
  109. Phi_theta(3,3)  =  2*c1-ecc;
  110. Phi_theta(3,4)  =  3*(1-2*ecc*s1*J1);
  111. Phi_theta(4,2)  =  ds1;
  112. Phi_theta(4,3)  =  dc1;
  113. Phi_theta(4,4)  =  -3*ecc*(ds1*J1+s1/rho1^2);
  114. %  利用XZ转移矩阵求出该时刻的位置和速度
  115. hatxz           =   Phi_theta*hatxz0;
  116. xt              =   hatxz(1,1)/rho1;
  117. zt              =   hatxz(2,1)/rho1;
  118. vxt             =   k^2*(hatxz(1,1)*ecc*sind(theta)+hatxz(3,1)*rho1);
  119. vzt             =   k^2*(hatxz(2,1)*ecc*sind(theta)+hatxz(4,1)*rho1);
  120. %  Y转移矩阵的参数
  121. Phi_thetay      =   zeros(2);
  122. Phi_thetay(1,1) =   cosd(theta-TA);
  123. Phi_thetay(1,2) =   sind(theta-TA);
  124. Phi_thetay(2,1) =   -sind(theta-TA);
  125. Phi_thetay(2,2) =    cosd(theta-TA);
  126. haty(1,:)       =    Phi_thetay*y0;
  127. yt              =    haty(1,1)/rho1;
  128. vyt             =    k^2*(haty(1,1)*ecc*sind(theta)+haty(1,2)*rho1);
  129. rvt             =    [xt,yt,zt,vxt,vyt,vzt];
  130. % 求出归一化形式的状态转移矩阵
  131. % 首先X-Z形式的矩阵
  132. Phi_XZ  = Phi_theta*Phi0_inv1;
  133. Phi_Y   = Phi_thetay;
  134. % 归一化的状态转移矩阵
  135. Phi_hat = zeros(6,6);
  136. Phi_hat(1,1) = Phi_XZ(1,1);
  137. Phi_hat(1,3) = Phi_XZ(1,2);
  138. Phi_hat(1,4) = Phi_XZ(1,3);
  139. Phi_hat(1,6) = Phi_XZ(1,4);
  140. Phi_hat(3,1) = Phi_XZ(2,1);
  141. Phi_hat(3,3) = Phi_XZ(2,2);
  142. Phi_hat(3,4) = Phi_XZ(2,3);
  143. Phi_hat(3,6) = Phi_XZ(2,4);
  144. Phi_hat(4,1) = Phi_XZ(3,1);
  145. Phi_hat(4,3) = Phi_XZ(3,2);
  146. Phi_hat(4,4) = Phi_XZ(3,3);
  147. Phi_hat(4,6) = Phi_XZ(3,4);
  148. Phi_hat(6,1) = Phi_XZ(4,1);
  149. Phi_hat(6,3) = Phi_XZ(4,2);
  150. Phi_hat(6,4) = Phi_XZ(4,3);
  151. Phi_hat(6,6) = Phi_XZ(4,4);
  152. % Y
  153. Phi_hat(2,2) = Phi_Y(1,1);
  154. Phi_hat(2,5) = Phi_Y(1,2);
  155. Phi_hat(5,2) = Phi_Y(2,1);
  156. Phi_hat(5,5) = Phi_Y(2,2);
  157. % 求出初始时间的状态转移矩阵
  158. PhiX0=zeros(6,6);
  159. PhiX0(1,1) = rho;
  160. PhiX0(2,2) = rho;
  161. PhiX0(3,3) = rho;
  162. PhiX0(4,1) = -ecc*sind(TA);
  163. PhiX0(5,2) = -ecc*sind(TA);
  164. PhiX0(6,3) = -ecc*sind(TA);
  165. PhiX0(4,4) = 1/(k^2*rho);
  166. PhiX0(5,5) = 1/(k^2*rho);
  167. PhiX0(6,6) = 1/(k^2*rho);
  168. % 求出末端时间的状态转移矩阵
  169. PhiXt = zeros(6,6);
  170. PhiXt(1,1) = 1/rho1;
  171. PhiXt(2,2) = 1/rho1;
  172. PhiXt(3,3) = 1/rho1;
  173. PhiXt(4,1) = k^2*ecc*sind(theta);
  174. PhiXt(5,2) = k^2*ecc*sind(theta);
  175. PhiXt(6,3) = k^2*ecc*sind(theta);
  176. PhiXt(4,4) = k^2*rho1;
  177. PhiXt(5,5) = k^2*rho1;
  178. PhiXt(6,6) = k^2*rho1;
  179. Phi        = PhiXt*Phi_hat*PhiX0;
  180. rvt_phi=Phi*[delta_r;delta_v];
  181. end
  182.  
  183.  
  184.  
  185.  
  186.  
  187.  
  188.

2.利用主函数调动子函数的方法来实现功能

  1. clc;clear
  2. %% 本代码旨在验证TH方程的正确性
  3. %% 验证ecc=0.7的案例
  4. delta_r=[0.1;0.01;0.01];
  5. delta_v=[0.0001;0.0001;0.0001];
  6. t=0:60:1200*60;
  7. vr=zeros(length(t),6);
  8. vrt_phi=zeros(length(t),6);
  9. ecc=0.7;Perigee=500;
  10. for j=1:length(t)
  11.     [vrt,Phi,vrt_phi1]=TH_solver(0.7,500,45,delta_r,delta_v,t(j));
  12.     vr(j,:)=vrt;
  13.     vrt_phi(j,:)=vrt_phi1';
  14. end
  15. figure(1)
  16. load STKVVLh2.mat
  17. plot(Tar_x(1:1200),Tar_z(1:1200),'b--');
  18. axis([-20 5 -5 4])
  19. set(gca,'XDir','reverse');
  20. set(gca,'YDir','reverse');
  21. set(gca,'FontName','Times New Roman')
  22. xlabel('X axis(km)','FontName','Times New Roman')
  23. ylabel('Z axis(km)','FontName','Times New Roman')
  24. title(['e=',num2str(ecc),' Perigee=',num2str(Perigee),'km'],'FontName','Times New Roman')
  25. grid on
  26. hold on
  27. plot(vr(:,1),vr(:,3),'r-')
  28. plot(vrt_phi(:,1),vrt_phi(:,3),'g--')
  29. legend(['Numercial'],['TH-Numberical'],['TH-State Transition Matrix'],'Location','southeast')
  30. hold off
  31. figure(2)
  32. plot(Tar_x(1:1200),Tar_y(1:1200));
  33. axis([-20 5 -0.8 0.6])
  34. set(gca,'XDir','reverse');
  35. set(gca,'FontName','Times New Roman')
  36. xlabel('X axis(km)','FontName','Times New Roman')
  37. ylabel('Y axis(km)','FontName','Times New Roman')
  38. title(['e=',num2str(ecc),' Perigee=',num2str(Perigee),'km'],'FontName','Times New Roman')
  39. grid on
  40. hold on
  41. plot(vr(:,1),vr(:,2),'r-')
  42. plot(vrt_phi(:,1),vrt_phi(:,2),'g--')
  43. legend(['Numercial'],['TH-Numberical'],['TH-State Transition Matrix'],'Location','southeast')
  44. %% 验证ecc=0.1的案例
  45. t=0:60:220*60;
  46. vr=zeros(length(t),6);
  47. vrt_phi=zeros(length(t),6);
  48. ecc=0.1;Perigee=500;
  49. for j=1:length(t)
  50.     [vrt,Phi]=TH_solver(0.1,500,45,delta_r,delta_v,t(j));
  51.     vr(j,:)=vrt;
  52.     vrt_phi(j,:)=vrt_phi1';
  53. end
  54. figure(3)
  55. load STKVVLh.mat
  56. plot(Tar_x(1:220),Tar_z(1:220),'b--');
  57. axis([-3.5 0.5 -0.6 0.3])
  58. set(gca,'XDir','reverse');
  59. set(gca,'YDir','reverse');
  60. set(gca,'FontName','Times New Roman')
  61. xlabel('X axis(km)','FontName','Times New Roman')
  62. ylabel('Z axis(km)','FontName','Times New Roman')
  63. title(['e=',num2str(ecc),' Perigee=',num2str(Perigee),'km'],'FontName','Times New Roman')
  64. grid on
  65. hold on
  66. plot(vr(:,1),vr(:,3),'r-')
  67. plot(vrt_phi(:,1),vrt_phi(:,3),'g--')
  68. legend(['Numercial'],['TH-Numberical'],['TH-State Transition Matrix'],'Location','southeast')
  69. hold off
  70. figure(4)
  71. plot(Tar_x(1:220),Tar_y(1:220),'b--');
  72. axis([-3.5 0.5 -0.2 0.15])
  73. set(gca,'XDir','reverse');
  74. set(gca,'FontName','Times New Roman')
  75. xlabel('X axis(km)','FontName','Times New Roman')
  76. ylabel('Y axis(km)','FontName','Times New Roman')
  77. title(['e=',num2str(ecc),' Perigee=',num2str(Perigee),'km'],'FontName','Times New Roman')
  78. grid on
  79. hold on
  80. plot(vr(:,1),vr(:,2),'r-')
  81. plot(vrt_phi(:,1),vrt_phi(:,2),'g--')
  82. legend(['Numercial'],['TH-Numberical'],['TH-State Transition Matrix'],'Location','southeast')

其中 STKVVLh,STKVVLh2的数据通过STK计算得到的VVLH坐标系，其数据在作者的数据包里可以免费下载，最后得到四个图

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Cpp五条/article/detail/658409
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号