#!/usr/bin/env python
# coding: utf-8
# 
# # ASE6029: Optimal guidance solutions
#
# $$
# \newcommand{\eg}{{\it e.g.}}
# \newcommand{\ie}{{\it i.e.}}
# \newcommand{\argmin}{\operatornamewithlimits{argmin}}
# \newcommand{\mc}{\mathcal}
# \newcommand{\mb}{\mathbb}
# \newcommand{\mf}{\mathbf}
# \newcommand{\minimize}{{\text{minimize}}}
# \newcommand{\diag}{{\text{diag}}}
# \newcommand{\cond}{{\text{cond}}}
# \newcommand{\rank}{{\text{rank }}}
# \newcommand{\range}{{\mathcal{R}}}
# \newcommand{\null}{{\mathcal{N}}}
# \newcommand{\tr}{{\text{trace}}}
# \newcommand{\dom}{{\text{dom}}}
# \newcommand{\dist}{{\text{dist}}}
# \newcommand{\R}{\mathbf{R}}
# \newcommand{\SM}{\mathbf{S}}
# \newcommand{\ball}{\mathcal{B}}
# \newcommand{\bmat}[1]{\begin{bmatrix}#1\end{bmatrix}}
# $$
#
# __ ASE6029: Linear Optimal Control, Inha University.
__
# _ Jong-Han Kim (jonghank@inha.ac.kr)
_
# _ Gyubin Park (gyubin@inha.edu)
_
#
# ## **Guidance problems**
# ### **Intercept Geometry**
#
# 
# 위와 같은 geometry에서, 제어 대상의 초기 속도벡터는 flight path angle $\gamma$와 $V$로 주어지며, $L$만큼 떨어진 표적을 요격하기 위해 속도벡터에 수직인 가속도 명령 $u$를 쓸 수 있다고 가정하자. 이 제어 대상의 동역학은 다음과 같이 나타낼 수 있다.
#
# $$
# \begin{aligned}
# \dot{l}&=V\cos\gamma \\
# \dot{r}&=V\sin\gamma \\
# V\dot{\gamma}&=u
# \end{aligned}
# $$
#
# 제어 대상이 $l$축 방향을 따라 일정한 속도로 움직인다고 가정하면, $\gamma\approx0$을 이용해 선형화하는 동시에 $r$축 방향의 성분만을 고려한 동역학을 아래와 같이 나타낼 수 있다.
#
# $$
# \begin{aligned}
# \dot{r}&=V\gamma \\
# V\dot{\gamma}&=u
# \end{aligned}
# $$
#
# 이제 상태변수 $x$을 $\begin{bmatrix}r\\V\gamma\end{bmatrix}$로 나타내면, 이 제어 대상의 상태방정식은 다음과 같다.
#
# $$
# \underbrace{\begin{bmatrix}\dot{r}\\V\dot{\gamma}\end{bmatrix}}_\dot{x} = \underbrace{\begin{bmatrix}0&1\\0&0\end{bmatrix}}_A
# \underbrace{\begin{bmatrix}r\\V\gamma\end{bmatrix}}_x+
# \underbrace{\begin{bmatrix}0\\1\end{bmatrix}}_Bu
# $$
#
#
#
# ---
#
#
# ### **Solving Riccati Differential Equation**
#
# 요격 시점 $T_f$가 $\displaystyle\frac{L}{V}$으로 정의된다면, continuous-time, finite horizon LQR problem의 비용함수 $J(u, x)$와 제어입력 $u(t)$을 다음과 같이 나타낼 수 있음을 보인 바 있다.
#
# $$
# \begin{aligned}
# J(u, x)&=\int_{0}^{T_f}\begin{pmatrix}x(t)^TQx(t) + u(t)^TRu(t)\end{pmatrix}dt + x(T_f)^TQ_fx(T_f) \\
# u(t) &= -R^{-1}B^TP_tx(t)
# \end{aligned}
# $$
#
# 여기서의 $P_t$은 Riccati differential equation $-\dot{P_t} = Q + A^TP_t + P_tA - P_tBR^{-1}B^TP_t$의 해로, 이 역시 Hamiltonian differential equation $\dot{z}(t) = \mathcal{H}z(t)$을 이용해 풀 수 있음을 보였다. Hamiltonian differential equation의 해는 $z(t)=e^{t\mathcal{H}}z(0)$으로 나타내어짐이 알려져 있으므로, $t=T_f$일 때의 $z(T_f)=e^{T_f\mathcal{H}}z(0)$을 이용해 $z(t) = e^{-\tau\mathcal{H}}z(T_f)$으로 나타낼 수 있으며, 이때 $\tau = T_f-t$은 요격까지 남은 시간, 즉 time-to-go을 의미한다.
#
# 자연로그의 밑 $e$의 정의에 따라, matrix exponential $e^{-\tau\mathcal{H}}$을 다음과 같이 나타낼 수 있다.
#
# $$
# e^{-\tau\mathcal{H}} = I - \tau\mathcal{H} + \frac{1}{2!}\tau^2\mathcal{H}^2 - \frac{1}{3!}\tau^3\mathcal{H}^3 + \frac{1}{4!}\tau^4\mathcal{H}^4 - \dotsm
# $$
#
# $R = I,~ Q = 0,~ Q_f = \begin{bmatrix}c_r & 0 \\ 0 & c_v\end{bmatrix}$로 주어지면 Hamiltonian $\mathcal{H}$은
# $\begin{bmatrix} A & -BR^{-1}B^T \\ -Q & - A^T\end{bmatrix}=
# \begin{bmatrix}\begin{array}{c|c}
# \begin{matrix}0&1\\0&0\end{matrix} &
# \begin{matrix}0&0\\0&-1\end{matrix} \\
# \hline
# \begin{matrix}0&0\\0&0\end{matrix} &
# \begin{matrix}0&0\\-1&0\end{matrix}
# \end{array}\end{bmatrix}$
# 이며, 그 sparse함으로 인해 $\mathcal{H}^4$ 이상의 거듭제곱이 영행렬임을 알 수 있다. 따라서 $e^{-\tau\mathcal{H}}$을 계산하면
#
# $$
# \begin{aligned}
# e^{-\tau\mathcal{H}} &= I - \tau\begin{bmatrix}0&1&0&0\\0&0&0&-1\\0&0&0&0\\0&0&-1&0\end{bmatrix} + \frac{1}{2}\tau^2\begin{bmatrix}0&0&0&-1\\0&0&1&0\\0&0&0&0\\0&0&0&0\end{bmatrix} - \frac{1}{6}\tau^3\begin{bmatrix}0&0&1&0\\0&0&0&0\\0&0&0&0\\0&0&0&0\end{bmatrix}\\
# &=\begin{bmatrix}1&-\tau&-\frac{1}{6}\tau^3&-\frac{1}{2}\tau^2\\0&1&\frac{1}{2}\tau^2&\tau\\0&0&1&0\\0&0&\tau&1\end{bmatrix}
# \end{aligned}
# $$
#
# 위와 같으며, $z(T_f)$은
# $\begin{bmatrix}I\\Q_f\end{bmatrix} = \begin{bmatrix}1&0\\0&1\\\hline c_r&0\\0&c_v\end{bmatrix}$이므로 Hamiltonian differential equation의 해 $z(t)$을 다음과 같이 나타낼 수 있다.
#
# $$
# \begin{aligned}
# z(t) &= \begin{bmatrix}1&-\tau&-\frac{1}{6}\tau^3&-\frac{1}{2}\tau^2\\0&1&\frac{1}{2}\tau^2&\tau\\0&0&1&0\\0&0&\tau&1\end{bmatrix}\begin{bmatrix}1&0\\0&1\\ c_r&0\\0&c_v\end{bmatrix}\\
# &= \begin{bmatrix}1-\frac{1}{6}c_r\tau^3 & -\tau-\frac{1}{2}c_v\tau^2 \\ \frac{1}{2}c_r\tau^2 & 1+c_v\tau \\ \hline c_r & 0 \\ c_r\tau & c_v\end{bmatrix}\\
# &= \begin{bmatrix}X(t)\\Y(t)\end{bmatrix}
# \end{aligned}
# $$
#
# Riccati differential equation의 해 $P_t$은 $Y(t)X(t)^{-1}$이므로,
#
# $$
# \begin{aligned}
# P_t &= Y(t)X(t)^{-1}\\
# &=\frac{1}{c_rc_v\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}c_rc_v\tau^4}\begin{bmatrix}c_r & 0 \\ c_r\tau & c_v\end{bmatrix}\begin{bmatrix}1+c_v\tau & \tau+\frac{1}{2}c_v\tau^2 \\ -\frac{1}{2}c_r\tau^2 & 1-\frac{1}{6}c_r\tau^3\end{bmatrix}
# \end{aligned}
# $$
#
# 제어입력 $u(t) = -R^{-1}B^TP_tx(t)$은 다음과 같다.
#
# $$
# \begin{aligned}
# u(t) &= -R^{-1}B^TP_tx(t)\\
# &=-\frac{1}{c_rc_v\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}c_rc_v\tau^4}\begin{bmatrix}0&1\end{bmatrix}\begin{bmatrix}c_r & 0 \\ c_r\tau & c_v\end{bmatrix}\begin{bmatrix}1+c_v\tau & \tau+\frac{1}{2}c_v\tau^2 \\ -\frac{1}{2}c_r\tau^2 & 1-\frac{1}{6}c_r\tau^3\end{bmatrix}\begin{bmatrix}r(t)\\v(t)\end{bmatrix}\\
# &= -\frac{1}{\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}\tau^4}\begin{bmatrix}\frac{1}{c_v}\tau+\frac{1}{2}\tau^2&\frac{1}{c_r}+\frac{1}{c_v}\tau^2+\frac{1}{3}\tau^3\end{bmatrix}\begin{bmatrix}r(t)\\v(t)\end{bmatrix}\\
# &= -\frac{\frac{1}{c_v}\tau+\frac{1}{2}\tau^2}{\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}\tau^4}r(t) - \frac{\frac{1}{c_r}+\frac{1}{c_v}\tau^2+\frac{1}{3}\tau^3}{\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}\tau^4}v(t)\\
# &=-K_r(\tau)r(t) - K_v(\tau)v(t)
# \end{aligned}
# $$
#
#
#
# ---
#
#
#
#
# ### **Flight path angle control: $c_r\rightarrow0,~ c_v\rightarrow\infty$**
#
# 종료 시점에서 $r$축 방향의 속도벡터 크기가 $0$이 되도록 제어하는 문제는 flight path angle $\gamma(t)$가 $0$이 되도록 하는 문제와 동일하다.
#
# $$
# \begin{aligned}
# K_r(\tau) &= \frac{\frac{1}{c_v}\tau+\frac{1}{2}\tau^2}{\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}\tau^4}\rightarrow0\\
# K_v(\tau) &= \frac{\frac{1}{c_r}+\frac{1}{c_v}\tau^2+\frac{1}{3}\tau^3}{\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}\tau^4}\rightarrow \frac{1}{\tau}
# \end{aligned}
# $$
#
# Feedback gain은 위와 같이 근사되며, 이에 따른 제어입력 $u(t)=-K_r(\tau)r(t) - K_v(\tau)v(t)$은 다음과 같다.
#
# $$
# \begin{aligned}
# u(t) &=-K_r(\tau)r(t) - K_v(\tau)v(t)\\
# &= -\frac{1}{\tau}v(t)\\
# &= -\frac{V}{\tau}\frac{v(t)}{V}\\
# &= -\frac{V}{\tau}\gamma(t)
# \end{aligned}
# $$
#
#
#
# ---
#
#
#
#
# ### **Proportional navigation guidance: $c_r\rightarrow\infty,~c_v\rightarrow0$**
#
# 종료 시점에서 $r$축 방향의 변위가 $0$이 되도록 제어하는 문제를 생각하자.
#
# $$
# \begin{aligned}
# K_r(\tau) &= \frac{\frac{1}{c_v}\tau+\frac{1}{2}\tau^2}{\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}\tau^4}\rightarrow\frac{3}{\tau^2}\\
# K_v(\tau) &= \frac{\frac{1}{c_r}+\frac{1}{c_v}\tau^2+\frac{1}{3}\tau^3}{\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}\tau^4}\rightarrow \frac{3}{\tau}
# \end{aligned}
# $$
#
# 제어입력 $u(t)$은 다음과 같다.
#
# $$
# \begin{aligned}
# u(t) &=-K_r(\tau)r(t) - K_v(\tau)v(t)\\
# &= -\frac{3}{\tau^2}r(t)-\frac{3}{\tau}v(t)
# \end{aligned}
# $$
# 
# 기존 geometry에서 제어 대상과 표적을 잇는 직선의 각도를 line of sight라고 정의하자. LOS의 각도를 $\lambda(t)$라 하면, $\tan(-\lambda(t))=\displaystyle\frac{r(t)}{V\tau}$가 성립한다. 이를 $\lambda\approx0$에서 선형화한다면 $\lambda(t)=\displaystyle-\frac{r(t)}{V\tau}$가 되며, $\dot{\lambda}$을 구하면 다음과 같다.
#
# $$
# \begin{aligned}
# \dot{\lambda}(t)&=-\frac{\dot{r}(t)}{V\tau} - \frac{r(t)}{V}\dot{\begin{pmatrix}\displaystyle\frac{1}{\tau}\end{pmatrix}}\\
# &=-\frac{v(t)}{V\tau} - \frac{r(t)}{V\tau^2}
# \end{aligned}
# $$
#
# 이렇게 구한 $\dot{\lambda}(t)$은 직전의 제어입력 $u(t)$와 동일한 형태이므로, 다음 결과를 얻는다.
#
# $$
# u(t) = 3V\dot{\lambda}(t)
# $$
#
# 이러한 guidance solution은 proportional navigation guidance라는 이름으로 알려져 있으며, PNG gain $3$이 제어 입력을 최소한으로 사용하도록 한다.
#
# $u(t)$을 $\dot{\lambda}(t)$ 대신 $\lambda(t)$와 $\gamma(t)$로 다음과 같이 나타낼 수도 있다.
#
# $$
# \begin{aligned}
# u(t) &= -\frac{3}{\tau^2}r(t)-\frac{3}{\tau}v(t)\\
# &= \frac{V}{\tau}\begin{pmatrix}\displaystyle-\frac{3r(t)}{V\tau}-\frac{3v(t)}{V}\end{pmatrix}\\
# &= \frac{V}{\tau}\begin{pmatrix}3\lambda(t) - 3\gamma(t)\end{pmatrix}
# \end{aligned}
# $$
#
#
#
# ---
#
#
# ### **Impact angle guidance: $c_r\rightarrow\infty,~ c_v\rightarrow\infty$**
#
# 종료 시점에서 $\gamma(t)$와 $r$축의 변위를 모두 $0$으로 수렴시키는 문제를 생각하자.
#
# $$
# \begin{aligned}
# K_r(\tau) &= \frac{\frac{1}{c_v}\tau+\frac{1}{2}\tau^2}{\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}\tau^4}\rightarrow\frac{6}{\tau^2}\\
# K_v(\tau) &= \frac{\frac{1}{c_r}+\frac{1}{c_v}\tau^2+\frac{1}{3}\tau^3}{\begin{pmatrix}\frac{1}{c_r}+\frac{1}{3}\tau^3\end{pmatrix}\begin{pmatrix}\frac{1}{c_v}+\tau\end{pmatrix}-\frac{1}{4}\tau^4}\rightarrow \frac{4}{\tau}
# \end{aligned}
# $$
#
# 제어입력 $u(t)$은 다음과 같으며,
#
# $$
# \begin{aligned}
# u(t) &=-K_r(\tau)r(t) - K_v(\tau)v(t)\\
# &= -\frac{6}{\tau^2}r(t)-\frac{4}{\tau}v(t)\\
# &= V\begin{pmatrix}\displaystyle-\frac{4r(t)}{V\tau^2}-\frac{4v(t)}{V\tau}-\frac{2r(t)}{V\tau^2}\end{pmatrix}\\
# &= V\begin{pmatrix}\displaystyle4\dot{\lambda}(t)+2\frac{\lambda(t)}{\tau}\end{pmatrix}
# \end{aligned}
# $$
#
# 아래와 같은 형태로도 나타낼 수 있다.
#
# $$
# \begin{aligned}
# u(t) &= -\frac{6}{\tau^2}r(t)-\frac{4}{\tau}v(t)\\
# &= \frac{V}{\tau}\begin{pmatrix}\displaystyle-\frac{6r(t)}{V\tau}-\frac{4v(t)}{V}\end{pmatrix}\\
# &= \frac{V}{\tau}\begin{pmatrix}\displaystyle6\lambda(t)-4\gamma(t)\end{pmatrix}
# \end{aligned}
# $$
#
# 이러한 guidance solution은 변위와 더불어 속도벡터 역시 최적 제어할 수 있으므로, rendezvous guidance, impact angle guidance, optimal guidance law 등의 이름으로 불린다.
# #### **Non-zero impact angle guidance**
#
# 
# 종료 시점에 제어 대상의 속도벡터가 $l'$축과 일치하도록 제어하는 문제를 생각하자. 기존 $l$축을 $\theta_f$만큼 회전시켜 $l'$축이 될 때, 이를 기준으로 하는 guidance solution은 flight path angle $\gamma$ 대신 $\gamma'=\gamma-\theta_f$을, line of sight $\lambda$ 대신 $\lambda'=\lambda-\theta_f$을 사용해 도출할 수 있다. 따라서 $\theta_f$의 각도로 표적을 요격하는 문제의 제어 입력은 다음과 같이 기술된다.
#
# $$
# \begin{aligned}
# u(t) &= \frac{V}{\tau}\begin{pmatrix}\displaystyle6\lambda(t)-4\gamma(t)\end{pmatrix}\\
# &\Downarrow\\
# u(t) &= \frac{V}{\tau}\begin{pmatrix}\displaystyle6(\lambda(t)-\theta_f)-4(\gamma(t)-\theta_f)\end{pmatrix}\\
# &= \frac{V}{\tau}\begin{pmatrix}\displaystyle6\lambda(t)-4\gamma(t)-2\theta_f\end{pmatrix}
# \end{aligned}
# $$
#
#
#
# ---
#
#
# ## **Numerical example**
#
# 
# 수평면 상에서 어떤 비행체의 속도를 $V$, heading angle을 $\chi$라 하고, 속도벡터의 수직 방향으로 제어입력 $u$를 사용해 표적을 요격하는 문제를 생각하자. 문제의 단순화를 위해 중력 및 별도의 항력은 무시한다. 이 비행체의 비선형 운동방정식은 다음과 같다.
#
# $$
# \begin{aligned}
# \dot{P}_N &= V\cos\chi\\
# \dot{P}_E &= V\sin\chi\\
# \dot{\chi} &= \frac{u}{V}
# \end{aligned}
# $$
#
# 상태변수 $x$을 $\begin{bmatrix}P_N\\P_E\\\chi\end{bmatrix}$으로 정의하면, 위 운동방정식을 $\dot{x} = f(x)$꼴의 미분방정식으로 나타낼 수 있다.
# In[ ]:
##### Guidance Solutions of Nonlinear Dynamical System #####
# Nonlinear Dynamical Equations
def state_dot(t, x, V, u):
pos_N, pos_E, chi = x
pos_N_dot = V*np.cos(chi)
pos_E_dot = V*np.sin(chi)
chi_dot = u/V
return np.array([pos_N_dot, pos_E_dot, chi_dot])
# Simulation Settings
tf = 1e3
dt = 1e-1
V = 10
x_init = [0, 0, -45*np.pi/180] # [Pos_N, Pos_E, Heading]
tgt_pos = [-100, 400] # [Pos_N, Pos_E]
Impact_Ang = 60*np.pi/180
# Loop for Various Guidance Solutions
for m in range(0, 4):
# Setting Variables
n = int(tf/dt) + 1
t = np.linspace(0, tf, n)
u = np.zeros([1, n])
x = np.zeros([3, n])
LOS = np.zeros([1, n])
dist = np.zeros([1, n])
x[:, 0] = x_init
# Simulation Begins
for i in range(0, n):
rgo = np.linalg.norm(x[0 : 2, i] - tgt_pos)
tgo = rgo/V
LOS[0, i] = np.arctan2(tgt_pos[1] - x[1, i], tgt_pos[0] - x[0, i])
dist[0, i] = np.linalg.norm(x[0 : 2, i] - tgt_pos)
# Guidance Solutions
if m == 0:
# PNG, Gain : 3
u[0, i] = V/tgo*3*(LOS[0, i] - x[2, i])
elif m == 1:
# PNG, Gain : 4
u[0, i] = V/tgo*4*(LOS[0, i] - x[2, i])
elif m == 2:
# OGL, Impact angle : 0
u[0, i] = V/tgo*(6*LOS[0, i] - 4*x[2, i])
elif m == 3:
# OGL, Impact angle : 60
u[0, i] = V/tgo*(6*LOS[0, i] - 4*x[2, i] - 2*Impact_Ang)
# State Derivatives
x_dot = state_dot(t[i], x[:, i], V, u[0, i])
# Terminating Simulation
if tgo < dt:
if m == 0:
t_PNG3 = t[0 : i]
x_PNG3 = x[:, 0 : i]
u_PNG3 = u[0, 0 : i]
LOS_PNG3 = LOS[0, 0 : i]
Dist_PNG3 = dist[0, 0 : i]
elif m == 1:
t_PNG4 = t[0 : i]
x_PNG4 = x[:, 0 : i]
u_PNG4 = u[0, 0 : i]
LOS_PNG4 = LOS[0, 0 : i]
Dist_PNG4 = dist[0, 0 : i]
elif m == 2:
t_OGL_1 = t[0 : i]
x_OGL_1 = x[:, 0 : i]
u_OGL_1 = u[0, 0 : i]
LOS_OGL_1 = LOS[0, 0 : i]
Dist_OGL_1 = dist[0, 0 : i]
elif m == 3:
t_OGL_2 = t[0 : i]
x_OGL_2 = x[:, 0 : i]
u_OGL_2 = u[0, 0 : i]
LOS_OGL_2 = LOS[0, 0 : i]
Dist_OGL_2 = dist[0, 0 : i]
break
# Numerical Integration
if i < n - 1:
x[:, i + 1] = x[:, i] + x_dot*dt
# Plot Results
plt.figure(figsize=(10,20))
plt.subplot(411)
plt.plot(x_PNG3[1, :], x_PNG3[0, :], label = 'PNG(Gain : 3)')
plt.plot(x_PNG4[1, :], x_PNG4[0, :], label = 'PNG(Gain : 4)')
plt.plot(x_OGL_1[1, :], x_OGL_1[0, :], label = 'OGL(Impact : 0 [deg])')
plt.plot(x_OGL_2[1, :], x_OGL_2[0, :], label = 'OGL(Impact : 60 [deg])')
plt.plot(x_init[1], x_init[0], 'bo', fillstyle = 'none')
plt.plot(tgt_pos[1], tgt_pos[0], 'xr')
plt.xlabel('East [m]')
plt.ylabel('North [m]')
plt.legend()
plt.grid()
plt.axis('equal')
plt.title('Various Guidance Solutions')
plt.subplot(412)
plt.plot(t_PNG3, x_PNG3[2, :]*180/np.pi, label = 'PNG(Gain : 3)')
plt.plot(t_PNG4, x_PNG4[2, :]*180/np.pi, label = 'PNG(Gain : 4)')
plt.plot(t_OGL_1, x_OGL_1[2, :]*180/np.pi, label = 'OGL(Impact : 0 [deg])')
plt.plot(t_OGL_2, x_OGL_2[2, :]*180/np.pi, label = 'OGL(Impact : 60 [deg])')
plt.xlabel('time [sec]')
plt.ylabel('Heading Angle $[deg]$')
plt.legend()
plt.grid()
plt.subplot(413)
plt.plot(t_PNG3, u_PNG3, label = 'PNG(Gain : 3)')
plt.plot(t_PNG4, u_PNG4, label = 'PNG(Gain : 4)')
plt.plot(t_OGL_1, u_OGL_1, label = 'OGL(Impact : 0 [deg])')
plt.plot(t_OGL_2, u_OGL_2, label = 'OGL(Impact : 60 [deg])')
plt.xlabel('time [sec]')
plt.ylabel('Acceleration Cmd $[m/s^2]$')
plt.legend()
plt.grid()
plt.subplot(414)
plt.plot(t_PNG3, LOS_PNG3*180/np.pi, label = 'PNG(Gain : 3)')
plt.plot(t_PNG4, LOS_PNG4*180/np.pi, label = 'PNG(Gain : 4)')
plt.plot(t_OGL_1, LOS_OGL_1*180/np.pi, label = 'OGL(Impact : 0 [deg])')
plt.plot(t_OGL_2, LOS_OGL_2*180/np.pi, label = 'OGL(Impact : 60 [deg])')
plt.xlabel('time [sec]')
plt.ylabel('Line of Sight [deg]')
plt.legend()
plt.grid()
plt.show()
# In[ ]:
plt.figure(figsize=(10,8))
plt.subplot(211)
plt.plot(t_PNG3, Dist_PNG3, label = 'PNG(Gain : 3)')
plt.plot(t_PNG4, Dist_PNG4, label = 'PNG(Gain : 4)')
plt.plot(t_OGL_1, Dist_OGL_1, label = 'OGL(Impact : 0 [deg])')
plt.plot(t_OGL_2, Dist_OGL_2, label = 'OGL(Impact : 60 [deg])')
plt.xlabel('time [sec]')
plt.ylabel('Miss Distance [m]')
plt.title('Miss Distance')
plt.legend()
plt.grid()
plt.subplot(212)
plt.plot(t_PNG3, Dist_PNG3, label = 'PNG(Gain : 3)')
plt.plot(t_PNG4, Dist_PNG4, label = 'PNG(Gain : 4)')
plt.plot(t_OGL_1, Dist_OGL_1, label = 'OGL(Impact : 0 [deg])')
plt.plot(t_OGL_2, Dist_OGL_2, label = 'OGL(Impact : 60 [deg])')
plt.xlabel('time [sec]')
plt.ylabel('Miss Distance [m]')
plt.xlim(50, 70)
plt.ylim(0, 3)
plt.legend()
plt.grid()
plt.show()