function [r,v,t] = keepAtitude(r0, v0, phi_ocf, psi_ocf, other)
% keepAltitude 迭代制导终止后保持原姿态控制角飞行，此函数计算迭代制导终止后tg时间内火箭的状态
% 输入输出量都是表示在发射惯性坐标系中
% Input：
%   x0    导航开始时火箭位置
%   v0    导航开始时火箭速度
%   control   相对于入轨点轨道坐标系的姿态角指令相关参数组成的结构体
%   other     其它参数组成的结构体
% Output：
%   r   导航计算的位置
%   v   导航计算的速度
%   w   导航计算的视加速度
%   t   时间向量
% 
% 2019.05.25

T_vac = other.T_vac;   % 真空推力
m0 = other.m0;         % 导航初始质量
dotm = other.dotm;     % 秒耗量
delta_t = other.delta_t;   % 导航递推时间
Ma = other.Ma;     % 发射惯性坐标系到入轨点轨道坐标系的转换矩阵
x0 = [r0; v0];     % 导航初始状态
if delta_t >1
    h = delta_t / 500;
elseif delta_t > 0.1
    h = delta_t / 100;
elseif delta_t > 0.01
    h = delta_t / 10;
else
    h = delta_t;
end
% 使用龙格库塔法积分动力学方程
[t,x] = rungeKutta4(@(t, x)func(t, x, T_vac, m0,dotm, Ma, phi_ocf, psi_ocf), 0, delta_t, x0, h);
r = x(:,1:3);
v = x(:,4:6);

end

function dxdt = func(t, x, T_vac, m0,dotm, Ma, phi_ocf, psi_ocf)
% dynamic equation for ode45
% \dot{r} = v
% \dot{v} = g + T_{vac} / m

mu = 3.9860044e14;   % 地心引力常数，单位：m^3/s^2
g = -(mu/norm(x(1:3))^3)*x(1:3);
Ibi = conAng(Ma, phi_ocf, psi_ocf);
m = m0 - dotm*t;
dxdt = nan(6,1);
dxdt(1) = x(4);    % \dot{x} = v_{x}
dxdt(2) = x(5);    % \dot{y} = v_{y}
dxdt(3) = x(6);    % \dot{z} = v_{z}
dxdt(4) = g(1) + T_vac*Ibi(1) / m;
dxdt(5) = g(2) + T_vac*Ibi(2) / m;
dxdt(6) = g(3) + T_vac*Ibi(3) / m;

end

function Ibi = conAng(Ma, phi_ocf, psi_ocf)
% 将入轨点轨道坐标系中表示的体轴矢量转换到发射惯性坐标系

Ib = [cos(phi_ocf)*cos(psi_ocf);
    sin(phi_ocf)*cos(psi_ocf);
    -sin(psi_ocf)];
Ibi = Ma\Ib;    % 将体轴矢量发射惯性坐标系下
% psi = asin(-Ibi(3));
% sinPhi = Ibi(2)/cos(psi);
% if sinPhi >= 0
%     vphi = acos(Ibi(1)/cos(psi));
% else
%     vphi = -acos(Ibi(1)/cos(psi));
% end

end