User:Egm6341.s10.team3.ynahn81/HW7-6

= (6) Solution of optimal control of bunt maneuver of aircraft using Hermite-Simpson (HS) algorithm and Newton-Raphson (NR) method for the given control inputs = Ref: Lecture notes [[media:Egm6341.s10.mtg40.djvu|p.40-3]] and [[media:Egm6341.s10.mtg41.djvu|p.41-1]]

Problem Statement
 The given equations of motion of aircraft are


 * {| style="width:100%" border="0" align="left"

\mathbf{\dot{z}} = \begin{Bmatrix} \dot{\gamma} \\ \dot{v} \\ \dot{x} \\ \dot{y} \end{Bmatrix} = \begin{Bmatrix} \frac{T-D}{mv}\sin\alpha + \frac{L}{mv}\cos\alpha - \frac{g}{v}\cos\gamma \\ \frac{T-D}{m}\cos\alpha + \frac{L}{m}\sin\alpha - g\sin\gamma \\ v\cos\gamma \\ v\sin\gamma \end{Bmatrix} = \begin{Bmatrix} f_{1}(\mathbf{z}) \\ f_{2}(\mathbf{z}) \\ f_{3}(\mathbf{z}) \\ f_{4}(\mathbf{z}) \end{Bmatrix} = \mathbf{f}(\mathbf{z}) $$ $$  Here, $$\displaystyle D$$ and $$\displaystyle L$$ are
 * $$\displaystyle
 * $$\displaystyle
 * $$\displaystyle (Eq. 1)
 * }
 * }
 * {| style="width:100%" border="0" align="left"

\begin{cases} D(y,v,\alpha) = \frac{1}{2}C_{d}\rho v^{2}S_{ref} \\ L(y,v,\alpha) = \frac{1}{2}C_{l}\rho v^{2}S_{ref} \end{cases} $$  where
 * $$\displaystyle
 * $$\displaystyle
 * }
 * }
 * {| style="width:100%" border="0" align="left"

\begin{cases} C_{d}(\alpha) = A_{1}\alpha^2 + A_{2}\alpha + A_{3} \\ C_{l}(\alpha) = B_{1}\alpha + B_{2} \\ \rho(y) = C_{1}y^2 + C_{2}y + C_{3} \end{cases} $$  
 * $$\displaystyle
 * $$\displaystyle
 * }
 * }

Solve the above equations of motion of aircraft when the below control inputs $$\displaystyle T(t)$$ and $$\displaystyle \alpha(t)$$ are given.


 * {| style="width:100%" border="0" align="left"

T(t) = \begin{cases} 6000 \; N, & \mbox{for }0 \leq t < 27 \; \mbox{and} \; 33 \leq t \leq 40 \\ 1000 \; N, & \mbox{for }27 \leq t < 33 \end{cases} $$ $$ 
 * $$\displaystyle
 * $$\displaystyle
 * $$\displaystyle (Eq. 2)
 * }
 * }


 * {| style="width:100%" border="0" align="left"

\alpha(t) = \begin{cases} 0.03 \; rad, & \mbox{for }0 \leq t < 21 \\ \frac{0.13-0.09}{27-21}(t-21) + 0.09 \; rad, & \mbox{for }21 \leq t < 27 \\ \frac{-0.2+0.13}{33-21}(t-27) - 0.13 \; rad, & \mbox{for }27 \leq t < 33 \\ \frac{-0.13+0.2}{40-33}(t-33) - 0.2\; rad, & \mbox{for }33 \leq t \leq 40 \end{cases} $$ $$  
 * $$\displaystyle
 * $$\displaystyle
 * $$\displaystyle (Eq. 3)
 * }
 * }

The solution should be obtained using Hermite-Simpson (HS) algorithm and Newton-Raphson (NR) method.  Solve the same equations of motion using "ode45" command in Matlab to verify the solution by HS and NR is correct.

Solution
All required parameters can be obtained from the Subchan and Zbikowski paper. (Subchan and Zbikowski, Optim. Control Appl. Meth. 2007; 28:311-353)
 * {| style="width:100%" border="0" align="left"

\begin{cases} m = 1005 \; kg \\ g = 9.81 \; m/s^2 \\ S_{ref} = 0.3376 \; m^2 \\ A_{1} = -1.9431 \\ A_{2} = -0.1499 \\ A_{3} = 0.2359 \\ B_{1} = 21.9 \\ B_{2} = 0 \\ C_{1} = 3.312 \times 10^{-9} \; kg/m^5 \\ C_{2} = -1.142 \times 10^{-4} \; kg/m^4 \\ C_{3} = 1.224 \; kg/m^3 \end{cases} $$   
 * $$\displaystyle
 * $$\displaystyle
 * }
 * }

The below Matlab function returns the given control input thrust $$\displaystyle T(t)$$.  <br\> The below Matlab function returns the given control input angle of attack $$\displaystyle \alpha(t)$$. <br\> <br\> <br\>

Let us use the notation $$\displaystyle \mathbf{z}(t_{i}) = \mathbf{z}_{i}$$ for simplification. In class meeting, we already obtained the below HS algorithm. (see [[media:Egm6341.s10.mtg37.djvu|p.37-2]])
 * {| style="width:100%" border="0" align="left"

\mathbf{z}_{i+1} = \mathbf{z}_{i} + \frac{h/2}{3}\left[\mathbf{f}(\mathbf{z}_{i})+4\mathbf{f}(\mathbf{g}(\mathbf{z}_{i},\mathbf{z}_{i+1}))+\mathbf{f}(\mathbf{z}_{i+1})\right] $$ $$ <br\> where
 * $$\displaystyle
 * $$\displaystyle
 * <p style="text-align:right;">$$\displaystyle (Eq. 4)
 * }
 * }
 * {| style="width:100%" border="0" align="left"

\mathbf{g}(\mathbf{z}_{i},\mathbf{z}_{i+1}) = \mathbf{z}_{i+1/2} = \frac{1}{2}\left(\mathbf{z}_{i} + \mathbf{z}_{i+1}\right) + \frac{h}{8}\left(\mathbf{f}(\mathbf{z}_{i}) - \mathbf{f}(\mathbf{z}_{i+1})\right) $$ $$ <br\>
 * $$\displaystyle
 * $$\displaystyle
 * <p style="text-align:right;">$$\displaystyle (Eq. 5)
 * }
 * }

The below Matlab function returns vector $$\displaystyle \mathbf{f}(\mathbf{z})$$ (see (Eq. 1)) for a given vector $$\displaystyle \mathbf{z}(t)$$. <br\> <br\> <br\>

By rearrange terms of (Eq.4), let us define $$\displaystyle \mathbf{F}(\mathbf{z}_{i+1})$$ as below.
 * {| style="width:100%" border="0" align="left"

\mathbf{F}(\mathbf{z}_{i+1}) = \mathbf{z}_{i+1} - \mathbf{z}_{i} - \frac{h/2}{3}\left[\mathbf{f}(\mathbf{z}_{i})+4\mathbf{f}(\mathbf{g}(\mathbf{z}_{i},\mathbf{z}_{i+1}))+\mathbf{f}(\mathbf{z}_{i+1})\right] = 0 $$ $$ <br\>
 * $$\displaystyle
 * $$\displaystyle
 * <p style="text-align:right;">$$\displaystyle (Eq. 6)
 * }
 * }

We can find the value of $$\displaystyle \mathbf{z}_{i+1}$$ which satisfies (Eq.6) using NR method. (see [[media:Egm6341.s10.mtg38.djvu|p.38-1]])
 * {| style="width:100%" border="0" align="left"

\mathbf{z}_{i+1}^{(k+1)} = \mathbf{z}_{i+1}^{(k)} - \left[\frac{d\mathbf{F}(\mathbf{z}_{i+1}^{(k)})}{d\mathbf{z}}\right]^{-1}\mathbf{F}(\mathbf{z}_{i+1}^{(k)}) $$ $$ <br\> Since time step is small, $$\displaystyle \mathbf{z}_{i}$$ can be used as the initial value of $$\displaystyle \mathbf{z}_{i+1}^{(k)}$$, i.e. $$\displaystyle \mathbf{z}_{i+1}^{(0)}=\mathbf{z}_{i}$$.
 * $$\displaystyle
 * $$\displaystyle
 * <p style="text-align:right;">$$\displaystyle (Eq. 7)
 * }
 * }

Now, note that we need to calculate $$\displaystyle \frac{d\mathbf{F}(\mathbf{z}_{i+1}^{(k)})}{d\mathbf{z}}$$ to use NR method.
 * {| style="width:100%" border="0" align="left"

\left[\frac{d\mathbf{F}(\mathbf{z}_{i+1}^{(k)})}{d\mathbf{z}}\right] = \mathbf{I} - \frac{h/2}{3}\left[4\frac{d\mathbf{f}(\mathbf{g})}{d\mathbf{g}}\frac{\mathbf{g}(\mathbf{z}_{i+1})}{d\mathbf{z}_{i+1}} + \frac{d\mathbf{f}(\mathbf{z}_{i+1})}{d\mathbf{z}_{i+1}}\right] $$ $$ <br\> where $$\displaystyle \mathbf{I}$$ is a identity matrix. From (Eq. 5), we can calculate $$\displaystyle \frac{\mathbf{g}(\mathbf{z}_{i+1})}{d\mathbf{z}_{i+1}}$$ as below.
 * $$\displaystyle
 * $$\displaystyle
 * <p style="text-align:right;">$$\displaystyle (Eq. 8)
 * }
 * }
 * {| style="width:100%" border="0" align="left"

\frac{\mathbf{g}(\mathbf{z}_{i+1})}{d\mathbf{z}_{i+1}} = \frac{1}{2}\mathbf{I} - \frac{h}{8}\frac{d\mathbf{f}(\mathbf{z}_{i+1})}{d\mathbf{z}_{i+1}} $$ $$ <br\>
 * $$\displaystyle
 * $$\displaystyle
 * <p style="text-align:right;">$$\displaystyle (Eq. 9)
 * }
 * }

Based on (Eq. 8) and (Eq. 9), we can calculate $$\displaystyle \frac{d\mathbf{F}(\mathbf{z}_{i+1}^{(k)})}{d\mathbf{z}}$$ once we know $$\displaystyle \frac{d\mathbf{f}(\mathbf{z}_{i+1})}{d\mathbf{z}_{i+1}}$$. Therefore, let us calculate $$\displaystyle \frac{d\mathbf{f}(\mathbf{z})}{d\mathbf{z}}$$ using (Eq. 1).
 * {| style="width:100%" border="0" align="left"

\begin{align} \frac{d\mathbf{f}(\mathbf{z})}{d\mathbf{z}} & =
 * $$\displaystyle
 * $$\displaystyle

\begin{bmatrix} \frac{df_{1}(\mathbf{z})}{d\gamma} & \frac{df_{1}(\mathbf{z})}{dv} & \frac{df_{1}(\mathbf{z})}{dx} & \frac{df_{1}(\mathbf{z})}{dy}    \\ \frac{df_{2}(\mathbf{z})}{d\gamma} & \frac{df_{2}(\mathbf{z})}{dv} & \frac{df_{2}(\mathbf{z})}{dx} & \frac{df_{2}(\mathbf{z})}{dy}    \\ \frac{df_{3}(\mathbf{z})}{d\gamma} & \frac{df_{3}(\mathbf{z})}{dv} & \frac{df_{3}(\mathbf{z})}{dx} & \frac{df_{3}(\mathbf{z})}{dy}    \\ \frac{df_{4}(\mathbf{z})}{d\gamma} & \frac{df_{4}(\mathbf{z})}{dv} & \frac{df_{4}(\mathbf{z})}{dx} & \frac{df_{4}(\mathbf{z})}{dy} \end{bmatrix} \\ & = \begin{bmatrix} \frac{g}{v}\sin\gamma & -\frac{T}{mv^2}\sin\alpha - \frac{C_{d}\rho S_{ref}}{2m}\sin\alpha + \frac{C_{l}\rho S_{ref}}{2m}\cos\alpha + \frac{\rho}{v^2}\cos\gamma & 0 & -\frac{C_{d}(2C_{1}y+C_{2})vS_{ref}}{2m}\sin\alpha + \frac{C_{l}(2C_{1}y+C_{2})vS_{ref}}{2m}\cos\alpha    \\ -g\cos\gamma & -\frac{C_{d}\rho vS_{ref}}{m}\cos\alpha - \frac{C_{d}\rho vS_{ref}}{m}\sin\alpha & 0 & -\frac{C_{d}(2C_{1}y+C_{2})v^2S_{ref}}{2m}\cos\alpha - \frac{C_{l}(2C_{1}y+C_{2})v^2S_{ref}}{2m}\sin\alpha    \\ -v\sin\gamma & \cos\gamma & 0 & 0    \\ v\cos\gamma & \sin\gamma & 0 & 0 \end{bmatrix} \\ \end{align} $$ $$ <br\> The below Matlab function returns vector $$\displaystyle \frac{d\mathbf{f}(\mathbf{z})}{d\mathbf{z}}$$ (see (Eq. 10)) for a given vector $$\displaystyle \mathbf{z}(t)$$. <br\> <br\>
 * <p style="text-align:right;">$$\displaystyle (Eq. 10)
 * }
 * }

Now, we can calculate (Eq. 6), (Eq. 8) and (Eq. 9) using the provided Matlab functions. Since we have all required information in order to perform NR method given in (Eq. 7), the solution of the equations of motion given in (Eq. 1) can be obtained.

The below Matlab function returns vector $$\displaystyle \mathbf{z}(t)$$ for the given time range $$\displaystyle t \in [0, 40]$$. <br\>

To verify the obtained solution from our own code, let us solve the same equations of motion using "ode45" command in Matlab. <br\>

Now, let us compare our results to the results from "ode45" command.

Plot $$\displaystyle \gamma(t) $$ vs $$\displaystyle t $$ <br\> <br\> Plot $$\displaystyle v(t) $$ vs $$\displaystyle t $$ <br\> <br\> Plot $$\displaystyle x(t) $$ vs $$\displaystyle t $$ <br\> <br\> Plot $$\displaystyle y(t) $$ vs $$\displaystyle t $$ <br\> <br\> Plot $$\displaystyle y(x) $$ vs $$\displaystyle x $$ <br\> <br\>

As shown above figures, the solution from our own code is really consistent with the result from Matlab ode45 command.