Parameter Identification HW2

TUGAS 2
AE6001 KAPITA SELEKTA DIRGANTARA B
Oleh
Rizqy Agung (23621018)
PROGRAM STUDI MAGISTER TEKNIK DIRGANTARA

FAKULTAS TEKNIK MESIN DAN DIRGANTARA
INSTITUT TEKNOLOGI BANDUNG
2021
No. 1
Suatu sistem dinamik dimodelkan dalam bentuk state space berikut
𝑥̇ −2.21 2.0 𝑥1 0.7
[ 1] = [ ]{ } + [ ].𝑢
𝑥̇ 2 −3.50 −0.5 𝑥2 2.0
Dimana kondisi awalnya adalah
𝑥1 0.73
[𝑥 ] = [ ]
2 0 0.27
Tentukan!
a. Plot respon dinamik sistem dengan metode Euler, model disimulasikan dengan input
doublet yang telah diberikan!
b. Lakukan hal yang sama, namun dengan metode Runge-kutta orde 4th .
c. Lakukan hal yang sama, namun dengan metode Diskritisasi
d. Bandingkan hasil simulasi antara ketiga metode di atas!
Solusi No. 1
Sebelum membuat program pada python, beberapa modul perlu diimpor untuk menyediakan
beberapa fungsi untuk perhitungan matematika.
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
import pandas as pd
a. Dilanjutkan dengan mendefenisikan fungsi general untuk mendapatkan respon sistem

dengan Metode Euler. Fungsi general yang berisikan beberapa variabel dan parameter pada
model matematika dibuat dengan memberikan perintah return untuk melakukan integrasi
dari kondisi awal yang ditambahkan dengan hasil fungsi dikali dengan interval waktunya
def euler(statemodel, xstate, dt, u, param):

""" Euler Integration Method """
f1 = statemodel(xstate, u, param)
return xstate + f1 * dt
Baca data dari file .csv yang diberikan
df = pd.read_csv("doublet.csv")
df
Lalu output dari code tersebut menunjukkan bahwa terdapat 201 baris dan 2 kolom dari data
berbentuk doublet tersebut
Apabila dibuat kedalam bentuk program, maka akan tervisualisasikan seperti berikut
uinput = pd.read_csv('doublet.csv')
plt.figure(figsize=(8,4))
plt.plot(uinput.time_s, uinput.doublet)
_ = plt.xlim(0, 20)
_ = plt.xlabel('time (s)', fontsize=12)
_ = plt.ylabel('input', fontsize=12)
Gambar 1. Plot Input Doublet pada Sistem
Pemodelan persamaan state-space seperti yang diketahui dari soal, dimodelkan diprogramkan
dalam python seperti berikut
def state_model(x, u, param):

""" State Model """
x1_dot = param[0] * x[0] + param[1] * x[1] + param[4] * u
xdot = [x1_dot, x2_dot]
return np.reshape(np.array(xdot), (-1,1))
Dimana param merupakan komponen nilai pada matriks A dan B. Data dari parameter dan
input pada sistem diprogram seperti berikut ini
param = np.array([-2.21, 2.0, -3.5, -0.5, 0.7, 2.0])

x_init = np.reshape(np.array([0.73, 0.27]), (-1,1))
uinp = uinput.doublet.to_numpy()
ndata = len(uinp)
dt = 0.1
Implementasi dari mencari solusi persamaan state-space tersebut dengan Metode Euler,
diprogram dengan code berikut ini
x_eu = np.zeros(shape=(ndata, 2))

x_eu[0, :] = x_init.T
for i in range(ndata):
if i > 0:
xprev = x_eu[i-1, :][:, np.newaxis]
x_eu[i, :] = euler(state_model, xprev, dt, uinp[i], param).T
Program code dan hasil visualisasi plot dari respon sistem dengan metode ini adalah
fig, ax = plt.subplots(1,2, figsize=(15, 4))
for i, axes in enumerate(ax):
axes.grid(ls='--', lw=0.5, color='black')
axes.plot(uinput.time_s, x_eu[:, i], lw=2.5, c='red')
axes.set_xlabel('time (s)', fontsize=18)
axes.set_ylabel(f'$x_{i+1}$', fontsize=18)
axes.set_xlim(0, 20)
Gambar 2. Plot Respon Sistem Dinamik dengan Metode Euler

b. Berdasarkan referensi [1], Metode Runge-Kutta orde ke-4, secara matematis dapat
dirumuskan sebagai berikut
Function Evaluations Solution

𝑓1 = 𝑓(𝑥, 𝑢, 𝛽)
𝑓2 = 𝑓[𝑥 + 𝑓1 (∆𝑡⁄2), 𝑢̅ , 𝛽]
𝑥(𝑘 + 1) = 𝑥(𝑘) + [𝑓1 + 2𝑓2 + 2𝑓3 + 𝑓4 ]( ∆𝑡/6)
𝑓3 = 𝑓[𝑥 + 𝑓2 (∆𝑡⁄2), 𝑢̅ , 𝛽]
𝑓4 = 𝑓[𝑥 + 𝑓3 (∆𝑡), 𝑢̅, 𝛽]
Sehingga pada program python, dapat dituliskan seperti berikut
def ruku4(statemodel, xstate, dt, u1, u2, param):

""" Runge-Kutta 4th Model Integration Method """
# Note: xstate, u1, u2, param must be in 2D array (n x 1)
f1 = statemodel(xstate, u1, param)
f2 = statemodel(xstate + f1 * dt * 0.5, (u1 + u2) * 0.5, param)
f4 = statemodel(xstate + f3 * dt, (u1 + u2) * 0.5, param)
return xstate + (f1 + 2*f2 + 2*f3 + f4) * dt/6.0
Inisialisasi data juga dilakukan untuk metode ini
param = np.array([-2.21, 2.0, -3.5, -0.5, 0.7, 2.0])

ndata = len(uinp)
dt = 0.1
Implementasi metode Runge-kutta orde ke-4 pada data yang inisialisasi dilakukan juga
dengan mengintegrasikan hasil setiap iterasi (dengan for loop)
x_rk = np.zeros(shape=(ndata, 2))

x_rk[0, :] = x_init.T
if i > 0:
xprev = x_rk[i-1, :][:, np.newaxis]
x_rk[i, :] = ruku4(state_model, xprev, dt, uinp[i-1], uinp[i], param).T
Program dan visualisasi hasil plot respon sistem dengan Metode Runge-Kutta orde-4
disajikan pada code dan gambar berikut
axes.plot(uinput.time_s, x_rk[:, i], lw=2.5, c='k')
Gambar 3. Plot Respon Sistem Dinamik dengan Metode Runge-Kutta orde ke-4
Selain itu, saya juga melalukan perbandingan antara metode Euler dan Runge-Kutta orde ke-4

axes.plot(uinput.time_s, x_eu[:, i], lw=2.5, c='r')
plt.legend(['Euler', 'Runge-Kutta'])
Gambar 4. Perbandingan Metode Eular dengan Metode Runge-kutta orde ke-4

c. Berdasarkan referensi [1], Metode Diskritisasi dapat diekspresikan secara matematik
seperti berikut
𝑡+∆𝑡
𝑥(𝑡 + ∆𝑡) = 𝑥(𝑡) + ∫ 𝑓[𝑥(𝑡), 𝑢(𝑡), 𝛩]𝑑𝑡
𝑡
Dimana maksud dari diskritisasi disini merupakan diskritisasi terhadap rentang waktu yang
sangat kecil. Pada python, code program tersebut dapat dibuat seperti berikut ini
def discretize_method(A, B, u, x_init, dt):

""" Discretization method for model integration """
phi = compute_phi(A, dt)
psi = phi * dt
x = np.zeros((len(u), 2))
x[0, :] = x_init.T
for i in range(len(u)):
if i > 0:
xi = np.dot(phi, x[i-1][np.newaxis].T) + np.dot(psi, B) * u[i]
x[i, :] = xi.flatten()
return x
def compute_phi(A, dt):

""" To compute state transition matrix """
phi = np.zeros(A.shape, dtype=float)
for i in range(10):
phi += np.linalg.matrix_power(A, i) * dt**i / np.math.factorial(i)
return phi
Inisiasi data parameter, variabel, serta interval waktu yang akan digunakan dibuat seperti
berikut ini
A = np.array([[-2.21, 2.0], [-3.5, -0.5]])

B = np.array([0.7, 2.0])[np.newaxis].T
x_init = np.array([0.73, 0.27])[np.newaxis].T
dt = 0.1
Implementasi dan visualisasi respon sistem dengan Metode Diskritisasi dibuat dan disajikan
pada code dan gambar berikut
x_di = discretize_method(A, B, uinp, x_init, dt)

axes.plot(uinput.time_s, x_di[:, i], lw=2.5, c='g')
Gambar 5. Plot Respon Sistem Dinamik dengan Metode Diskritisasi dengan interval waktu 0.1 detik
Gambar 6. Plot Respon Sistem Dinamik dengan Metode Diskritisasi dengan interval waktu 1 detik
Gambar 7. Plot Respon Sistem Dinamik dengan Metode Diskritisasi pada dua interval waktu yang berbeda
Dapat dilihat dari kedua plot di atas, bahwa interval waktu pada Metode Diskritisasi sangat
memengaruhi hasil dari solusi yang didapatkan (semakin kecil intervalnya, maka hasilnya akan
lebih baik). Namun tentunya dibutuhkan kebijaksanaan dalam memilih interval waktu yang
digunakan, agar kerja komputer tidak terlalu berat dan hasil yang didapat sudah cukup akurat.
d. Perbandingan antara ketiga metode diatas, diimplementasikan dengan program
seperti berikut ini

axes.plot(uinput.time_s, x_rk[:, i], lw=2.5, c='b')
axes.plot(uinput.time_s, x_di[:, i], lw=2.5, c='y')
plt.legend(['Euler', 'Runge-Kutta', 'Discretization'])
Gambar 8. Perbandingan Plot Respon Sistem dengan Ketiga Metode
Dapat diobservasi dan dianalisa bahwa berdasarkan Gambar 8 diatas terlihat bahwa dengan
Metode Runge-Kutta orde ke-4 dan Metode Diskritisasi dengan dt = 0.1 menunjukan plot yang
hampir berdempetan. Hal ini menunjukkan bahwa, tingkat keakuratan kedua metode ini lebih
baik daripada dengan Metode Euler (merupakan metode yang sederhana). Dimana pada
Metode Runge-Kutta orde ke 4, terdapat 4 kali “Evaluation Function” yang didalamnya juga
terdapat faktor nilai tengah dari interval waktu tersebut. Artinya, metode ini selalu berusaha
untuk meminimkan nilai error dari mencari nilai tengah dari setiap hasil evaluasi fungsinya.
Sementara itu, Metode Diskritisasi yang berbasis integral tersebut menghasilkan respon sistem
yang mirip dengan Runge-Kutta. Hal ini dikarenakan interval waktu yang cukup sempit,
sehingga hasil integrasinya juga tidak akan memberikan error yang terlalu signifikan.
No. 2
Simulasi attitude propagation model dengan Metode Runge-Kutta orde ke-4. Persamaan yang
akan digunakan adalah
𝜙̇ = 𝑝 + 𝑡𝑎𝑛𝜃(𝑞. 𝑠𝑖𝑛𝜙 + 𝑟. 𝜙)
𝜃̇ = 𝑞. 𝑐𝑜𝑠𝜙 − 𝑟. 𝜙
𝑞. 𝑠𝑖𝑛𝜙̇ + 𝑟. 𝑐𝑜𝑠𝜙
𝜓=
𝑐𝑜𝑠𝜃
Dimana kondisi awalnya adalah
𝜙0 0.9𝑜
[ 𝜃0 ] = [1.7𝑜 ]
𝜓0 13𝑜
Dan interval waktu yang digunakan adalah 0.4 detik (dt = 0.4 s)
Solusi No.2
Seperti biasa, sebelum membuat program yang didalamnya terdapat fungsi matematika,
beberapa modul perlu diimpor agar fungsi matematikanya dapat digunakan
import numpy as np
%matplotlib inline
import pandas as pd
Baca data yang diberikan untuk mengecek bentuk data, dimana data gerakan dari wahana telah
tersimpan dalam file .csv yang diberikan
uinput = pd.read_csv('pqr_input.csv')
uinput
Gambar 9. Data Gerakan Wahana (p, q, r)

Untuk melihat plot dari p, q, dan r dari data yang telah diberikan dapat dibuat code program
seperti berikut
plt.plot(uinput["p_radps"],lw=2,color='r')
plt.plot(uinput["q_radps"],lw=2,color='b')
plt.plot(uinput["r_radps"],lw=2,color='g')
plt.ylabel('radps')
plt.title("PQR Input Model")
plt.grid(ls="--",lw=0.5,color="black")
plt.legend(['p','q','r'])
plt.xlim(0,len(uinput))
plt.show()
Visualisasi dari hasil program di atas adalah sebagai berikut
Gambar 10. Plot Data p,q, dan r pada selang waktu tertentu
Berdasarkan persamaan matematika yang telah dijelaskan pada soal, maka code pemrograman
untuk mendefenisikan persamaannya dibuat seperti berikut ini
from numpy import float64, cos, sin, tan
def state_model(xstate,u):
p = u[0]
q = u[1]
r = u[2]
phi = xstate[0]
theta = xstate[1]
phi_dot = p + tan(theta)*(q*sin(phi)+r*phi)
theta_dot = q*cos(phi) - r*phi
psi_dot = (q*sin(phi)+r*cos(phi))/cos(theta)
xdot = [phi_dot,theta_dot,psi_dot]
return np.reshape(np.array(xdot),(-1,1))
Dengan menggunakan Metode Runge-Kutta orde ke-4 yang tadi, proses yang kurang lebih
sama dituliskan pada program. Namun perbedaannya terletak pada setiap fungsi dikalikan dt
agar propagasi sikap wahananya diiterasi terhadap waktu.
def runge_kutta(statemodel,xstate,dt,u1,u2):
k1 = dt*statemodel(xstate,u1)
k2 = dt*statemodel(xstate+k1*dt/2,(u1+u2)/2)
k4 = dt*statemodel(xstate+k3*dt,(u1+u2)/2)
k = (k1+2*k2+2*k3+k4)/6
return xstate + k
Inisialisasi data untuk kasus ini dibuat dengan program berikut ini
dt = 0.04
uinp = uinput.to_numpy()
y_rk = np.zeros(shape=(len(uinp),3))
y_init = [0.0157, 0.0296, 0.227]
y_rk[0,:] = y_init
for i in range(len(uinp)):
if i>0:
yprev = y_rk[i-1][:,np.newaxis]
y_rk[i,:] = runge_kutta(state_model,yprev,dt,uinp[i-1],uinp[i]).T
Plot hasil simulasi numerik untuk propagasi sikap dari wahana yang dikaji pada selang waktu
25 detik
label = [r'$\phi$',r'$\theta$',r'$\psi$']
fig,ax = plt.subplots(1,3,figsize=(15,4))
axes.grid(ls="--",lw=0.5,color="black")
axes.plot(time_rk,np.degrees(y_rk[:,i]),lw=2.5,color='b')
axes.set_ylabel(label[i]+' (deg)')
axes.set_xlim(0,time_rk[-1])
axes.set_xlabel('time (s)')
Gambar 11. Hasil Simulasi “Attitude Propagation” pada model nonlinear

Ketiga sudut diatas merupakan sudut roll, pitch, dan yaw dari suatu pesawat dengan input p, q,
dan r yang hubungan dapat dijelaskan dengan persamaan matematikanya. Dapat dilihat dari
trend-nya, sudut ptich dan yaw tersebut meningkat seiring dengan berjalannya waktu walaupun
sempat berosilasi pada kurun waktu tertentu. Sementara itu sudut roll seolah-olah akan kembali
ke posisi semulanya, walaupun sempat juga mengalami osilasi pada kurun waktu tertentu.
No. 3
Data penerbangan yang diberikan berupa flight_03.csv dan akan dilakukan “Trajectory
Reconstruction”.
a. Lakukan rekonstruksi lintas terbang dari data yang diberikan. Plot hasilnya baik dalam
Tata Acuan Koordinat (TAK) NED maupun ECEF WGS-84
b. Plot hasil lintas terbang pada Google Earth
c. Tentukan lokasi (bandara) tempat pendaratan berdasarkan data dan hasil rekonstruksi
Solusi No.3
Impor beberapa modul yang akan digunakan pada program ini
import numpy as np
import pandas as pd
# from matplotlib import style
Baca data yang diberikan untuk mengecek bentuk data, dimana data navigasi pesawat telah
tersimpan dalam file .csv yang diberikan
df = pd.read_csv("flight_03.csv")
df
Dan data dapat dilihat dari output code program di atas

Gambar 12. Data Navigasi Pesawat pada flight_03.csv
Apabila data diatas diplot terhadap waktu, maka akan didapatkan visualisasi seperti pada
dibawah.
time_s = np.linspace(0, df.shape[0] * 0.1, df.shape[0])
label = ["Ground Speed (m/s)", "Track angle (deg)", "Vertical Speed (m/s)"]
for i, axes in enumerate(ax.flatten()):
if i < 3:
if i == 1:
chi_deg = df.iloc[:, i+1].values * np.rad2deg(i) % 360
axes.plot(time_s, chi_deg, "--")
else:
axes.plot(time_s, df.iloc[:, i+1], "--")
axes.set_xlabel("time (s)", fontsize=12)
axes.set_ylabel(label[i], fontsize=12)
axes.set_xlim(0)
else:
axes.remove()
Gambar 13. Plot Data Navigasi Pesawat
Propagasi posisi pesawat pada TAK Kinematiknya dapat diformulasikan pada persamaan
berikut ini
𝑉𝑠 = 𝑉𝑘 . 𝑐𝑜𝑠(𝜒) . 𝑐𝑜𝑠(𝛾)
𝑉𝐸 = 𝑉𝑘 . 𝑠𝑖𝑛(𝜒) . 𝑐𝑜𝑠(𝛾)
ℎ = −𝑉𝑘 . 𝑠𝑖𝑛(𝛾)
Dari data yang disediakan, tidak ada sudut gamma. Sehingga kita perlu menghitungnya melalui
hubungan antara h dan VK seperti berikut ini
ℎ = −𝑉𝑘 . 𝑠𝑖𝑛(𝛾)
𝑉𝑘
𝛾 = −sin−1
ℎ
Code program yang dibuat untuk menghitung sudut gamma disajikan seperti berikut
df["gamma_rad"] = np.divide(df.hdot_mps, df.gs_mps)

df["gamma_rad"] = df["gamma_rad"].replace('0.0', 1E-8)
df["gamma_rad"] = df["gamma_rad"].replace(np.nan, 1E-8)
df["gamma_rad"] = df["gamma_rad"].replace(np.inf, 1E-8)
df["gamma_rad"] = df["gamma_rad"].replace(-np.inf, 1E-8)
df
Hasil perhitungan gamma dapat dilihat pada kolom paling kanan di gambar berikut
Gambar 14. Data Hasil Perhitungan Gamma
Untuk melihat plot data gamma pada selang waktu tertentu, dibuat code program seperti berikut

label = ["Ground Speed (m/s)", "Track angle (deg)", "Vertical Speed (m/s)",
"Gamma (deg)"]
if i == 1:
chi_deg = df.iloc[:, i+1].values * np.rad2deg(1) % 360
elif i == 4:
gamma_deg = df.iloc[:, i+1].values * np.rad2deg(1)
axes.plot(time_s, gamma_deg, "--")
else:
axes.set_xlim(0)
Visualisasi dari hasil perhitungan gamma berdasarkan data yang lainnya dapat dilihat pada
kurva di sebelah kanan bawah
Gambar 15. Plot Data Beberapa Parameter dari Data yang diberikan dan dari Perhitungan Sudut Gamma
a. Setelah semua data yang diperlukan telah tersedia, maka Trajectory Reconstruction dari
pesawat ini dapat dilakukan. Setelah itu dilanjutkan dengan membuat program untuk model
matematika berdasarkan persamaan propagasi posisi pada bagian sebelumnya
from numpy import float64, cos, sin
def xdot_kpm_ned(xstate, uinp, param):

"""
State Model of kinematic point mass model w.r.t NED Frame Reference
where uinp consists of 3x1 Numpy array:
ks_mps = kinematics speed (m/s)
chi_rad = track angle (rad)
gamma_rad = flight path angle (rad)
"""
ks_mps, chi_rad, gamma_rad = uinp
xdot = np.zeros(3, dtype=float64)
xdot[0] = ks_mps * cos(chi_rad) * cos(gamma_rad)
xdot[1] = ks_mps * sin(chi_rad) * cos(gamma_rad)
xdot[2] = ks_mps * sin(gamma_rad)
return xdot
def yout_kpm_ned(xstate, uinp, param):

""" Output Model of Kinematic Point Mass w.r.t NED Frame"""
return xstate
Pada bagian akhir code program diatas, variabel xstate dibuatkan dalam fungsi yang nantinya
variabel tersebut ada dalam TAK NED. Lalu, untuk menyelesaikan persamaan diatas,
digunakan metode Runge-Kutta orde ke-4. Implementasi metode tersebut pada code program
dibuat seperti berikut ini
Inisialisasi data yang akan digunakan pada simulasi kali ini, dibuat dalam code program seperti
berikut ini
param, dt = 0.0, 1.0
uinput = df.loc[:, ["gs_mps", "chi_rad", "gamma_rad"]].to_numpy()
xstate = np.zeros(3, dtype=np.float64)

ystorage = np.zeros(shape=(uinput.shape[0], 3), dtype=np.float64)
for i in range(uinput.shape[0]):
ystorage[i, :] = yout_kpm_ned(xstate, uinput[i], param)
if i > 0:
u1 = uinput[i-1, :]
u2 = uinput[i, :]
xstate = ruku4(xdot_kpm_ned, xstate, dt, u1, u2, param)
Plot hasil Trajectory Reconstruction pada TAK NED dan ketinggian dari pesawat tersebut
dapat dilihat pada gambar berikut ini
Gambar 16. Plot Hasil Trajectory Reconstruction dan Ketinggian Terbang Pesawat
Selanjutnya dilakukan transformasi dari TAK NED ke ECEF-WGS84. Dengan beberapa
kondisi awal yang diketahui pada TAK WGS84
• Latitude = -0.106821841 rad
• Longitude = 1.861220837 rad
• h = 182.88 m
Code program untuk mentransformasikan TAK ini dibuat seperti berikut ini
# Conversion from NED to LLA (WGS-84)

r2d = np.degrees(1)
lat_init_deg = -0.106821841 * np.rad2deg(1)
lon_init_deg = 1.861220837 * np.rad2deg(1)
hgps_init_m = 182.88
lat_ref, lon_ref, alt_ref = lat_init_deg, lon_init_deg, hgps_init_m
Impor modul “navpy” ke dalam program. Modul ini diambil dari website :
https://github.com/NavPy/NavPy
from navpy import ned2lla

lat_deg, lon_deg, _ = ned2lla(ystorage, lat_ref, lon_ref, alt_ref)
Dalam mengimpor modul, lokasi folder modul ini harus berada dalam directory yang sama
dengan lokasi penyimpanan file program. Hal ini sangat penting karena ketika akan melakukan
running pada program, komputer hanya akan bekerja pada satu file directory yang sama.
Apabila file modul tidak berada dalam satu directory, maka program tidak akan dapat membaca
modul tersebut. Plot hasil trajectrory reconstruction pada TAK ECEF-WGS 84 yang telah
didapat
fig, axes = plt.subplots(2, 1, figsize=(8,10))

axes[0].plot(lon_deg, lat_deg)
axes[0].set_xlabel("Longitude (deg)", fontsize=14)
axes[0].set_ylabel("Latitude (deg)", fontsize=14)
hgps_m = ystorage[:, 2]
axes[1].plot(hgps_m)
axes[1].set_xlabel("Time (s)", fontsize=14)
axes[1].set_ylabel("Altitude (m)", fontsize=14)
Visualisasi hasil plot dapat dilihat pada gambar berikut

Gambar 17. Hasil Trajectory Rekonstruction pada TAK ECEF-WGS 84
Dapat dilihat bahwa plot hasil rekonstruksi pada TAK NED dan TAK ECEF-WGS 84,
keduanya sudah sangat mirip. Namun perbedaannya hanya terdapat pada besaran pada sumbu
vertikal dan horizontal. Hal ini menunjukkan bahwa, code program yang telah dibuat sudah
benar dan tervalidasi. Selain itu, plot ketinggian terbang pesawat juga sudah menunjukkan hasil
yang masuk akal, dimana terlihat jelas ketinggian pesawat ketika take-off, cruise, hingga
landing dalam kurun waktu tertentu.
b. Agar hasil simulasi lebih menarik dan realistis, hasil rekonstruksi diatas juga diplot pada
Google Earth. Sebelum melakukan plot, modul kml yang yang telah disediakan perlu diimpor
ke dalam program.
from utilib.kml import create_kml

create_kml(lat_deg, lon_deg, hgps_m, "deg", "trajectory-sim")
Data program tersebut diambil dari data pada TAK ECEF-WGS 84. Dengan bantuan aplikasi
Google Earth, output dari simulasi diatas yang diberi nama “trajectory-sim” dimasukkan.
Sehingga didapatlah plot pada Google Earth seperti berikut
Gambar 18. Rekonstruksi Terbang Pesawat pada Goolge Earth
Code program untuk men-generate file .kml yang menjadi file input ke aplikasi Google
Earth dibuat oleh Pak Javen tahun 2019. Penjelasan lebih mendalam mengenai rekonstruksi
terbang pesawat akan dibahas lebih mendalam pada bagian berikutnya.
c. Untuk menentukan pesawat berangkat dan tiba di lokasi yang mana, selain dari Gambar
18 diperlukan untuk menganalisa pesawat dari kurva latitude vs longitude-nya. Dengan
mengetahui kondisi awalnya
• Latitude = -6.07 derajat
• Longitude = 106.6 derajat
Sehingga titik itu akan berada pada tanda silang pada kurva dibawah ini
Gambar 19. Lokasi Kondisi Awal pada Kurva Latitude vs Longitude
Dan apabila dibandingkan dengan Gambar 18 pada plot Google Earth, lokasi tanda silang
tersebut berada di Bandara Soekarna-Hatta di Cengkareng. Lalu sesuai dengan kurvanya,
pesawat akan mendarat di kota Palembang, yaitu Bandara Internasional Sultan Mahmud
Badaruddin II. Namun apabila diteliti lebih dalam, posisi mendarat tidak persis tepat di
bandara, melainkan terdapat sedikit perbedaan yang ditandai dengan garis putih pada gambar
dibawah ini.
Gambar 20. Lokasi Mendarat Rekonstruksi Terbang Pesawat

Hal ini merupakan suatu kejawaran yang bisa saja didapatkan karena data yang didapat tidak
terlalu akurat, atau juga bisa didapat dari error hasil pengintegrasian dari simulasi numerik
dengan Metode Runge-Kutta orde ke-4 tadi. Error dari setiap langkah iterasi akan
menambahkan error pada iterasi berikutnya, meskipun metode ini sudah menggunakan nilai
tengah dari setiap iterasi dan dievaluasi dengan 4 Evaluation Function. Namun, secara
engineering hal ini tidak terlalu bermasalah karena kita tetap dapat membuat kesimpulan yang
cukup baik dari hasil simulasi yang kita buat.
Jadi dapat disimpulkan bahwa pesawat tersebut terbang dari Bandara Soekarno-Hatta
di Cengkareng dan mendarat di Bandara Internasional Sultan Mahmud Badaruddin II
di Kota Palembang, sekalipun ada error yang terjadi.
References
[1] H. Muhammad and J. Sembiring, Mathematical Modeling of Flight Vehicle - Part 2,

Bandung: Institut Teknologi Bandung, 2022.
LAMPIRAN CODE
No. 1
import numpy as np
%matplotlib inline
import pandas as pd
def euler(statemodel, xstate, dt, u, param):

""" Euler Integration Method """
f1 = statemodel(xstate, u, param)
return xstate + f1 * dt
df = pd.read_csv("doublet.csv")
df
uinput = pd.read_csv('doublet.csv')
plt.figure(figsize=(8,4))
plt.plot(uinput.time_s, uinput.doublet)
_ = plt.xlim(0, 20)
_ = plt.xlabel('time (s)', fontsize=12)
_ = plt.ylabel('input', fontsize=12)
def state_model(x, u, param):

""" State Model """
xdot = [x1_dot, x2_dot]
return np.reshape(np.array(xdot), (-1,1))
param = np.array([-2.21, 2.0, -3.5, -0.5, 0.7, 2.0])

ndata = len(uinp)
dt = 0.1
x_eu = np.zeros(shape=(ndata, 2))

x_eu[0, :] = x_init.T
if i > 0:
xprev = x_eu[i-1, :][:, np.newaxis]
x_eu[i, :] = euler(state_model, xprev, dt, uinp[i], param).T

axes.plot(uinput.time_s, x_eu[:, i], lw=2.5, c='red')

param = np.array([-2.21, 2.0, -3.5, -0.5, 0.7, 2.0])

ndata = len(uinp)
dt = 0.1
x_rk = np.zeros(shape=(ndata, 2))

x_rk[0, :] = x_init.T
if i > 0:
xprev = x_rk[i-1, :][:, np.newaxis]
x_rk[i, :] = ruku4(state_model, xprev, dt, uinp[i-1], uinp[i], param).T


plt.legend(['Euler', 'Runge-Kutta'])

""" Discretization method for model integration """
psi = phi * dt
x[0, :] = x_init.T
if i > 0:
return x
for i in range(10):
return phi
A = np.array([[-2.21, 2.0], [-3.5, -0.5]])

dt = 1
x_di = discretize_method(A, B, uinp, x_init, dt)

axes.plot(uinput.time_s, x_di[:, i], lw=2.5, c='g')
## Iteration Number 1
""" Discretization method for model integration-1 """
psi = phi * dt
x[0, :] = x_init.T
if i > 0:
return x
for i in range(10):
return phi
A = np.array([[-2.21, 2.0], [-3.5, -0.5]])

dt = 0.1
x_di_a = discretize_method(A, B, uinp, x_init, dt)
## Iteration Number 2 ##
def discretize_method_i(A, B, u, x_init, dt):
""" Discretization method for model integration-2 """
phi_i = compute_phi(A, dt)
psi_i = phi_i * dt
x[0, :] = x_init.T
if i > 0:
xi = np.dot(phi_i, x[i-1][np.newaxis].T) + np.dot(psi_i, B) * u[i]
return x
def compute_phi_i(A, dt):

phi_i = np.zeros(A.shape, dtype=float)
for i in range(10):
phi_i += np.linalg.matrix_power(A, i) * dt**i / np.math.factorial(i)
return phi_i
A = np.array([[-2.21, 2.0], [-3.5, -0.5]])
dt = 0.2
x_di_b = discretize_method_i(A, B, uinp, x_init, dt)

axes.plot(uinput.time_s, x_di_a[:, i], lw=2.5, c='b')
axes.plot(uinput.time_s, x_di_b[:, i], lw=2.5, c='r')
plt.legend(['dt = 0.1', 'dt = 0.2'])

axes.plot(uinput.time_s, x_rk[:, i], lw=2.5, c='b')
axes.plot(uinput.time_s, x_di[:, i], lw=2.5, c='y')
plt.legend(['Euler', 'Runge-Kutta', 'Discretization'])
No. 2
import numpy as np
%matplotlib inline
import pandas as pd
uinput = pd.read_csv('pqr_input.csv')
uinput
plt.plot(uinput["p_radps"],lw=2,color='r')
plt.plot(uinput["q_radps"],lw=2,color='b')
plt.plot(uinput["r_radps"],lw=2,color='g')
plt.ylabel('radps')
plt.title("PQR Input Model")
plt.grid(ls="--",lw=0.5,color="black")
plt.legend(['p','q','r'])
plt.xlim(0,len(uinput))
plt.show()
from numpy import float64, cos, sin, tan
def state_model(xstate,u):
p = u[0]
q = u[1]
r = u[2]
phi = xstate[0]
theta = xstate[1]
phi_dot = p + tan(theta)*(q*sin(phi)+r*phi)
theta_dot = q*cos(phi) - r*phi
psi_dot = (q*sin(phi)+r*cos(phi))/cos(theta)
xdot = [phi_dot,theta_dot,psi_dot]
return np.reshape(np.array(xdot),(-1,1))
def runge_kutta(statemodel,xstate,dt,u1,u2):
k1 = dt*statemodel(xstate,u1)
k4 = dt*statemodel(xstate+k3*dt,(u1+u2)/2)
k = (k1+2*k2+2*k3+k4)/6
return xstate + k
dt = 0.04
uinp = uinput.to_numpy()
y_rk = np.zeros(shape=(len(uinp),3))
y_init = [0.0157, 0.0296, 0.227]
y_rk[0,:] = y_init
for i in range(len(uinp)):
if i>0:
yprev = y_rk[i-1][:,np.newaxis]
y_rk[i,:] = runge_kutta(state_model,yprev,dt,uinp[i-1],uinp[i]).T
time_rk = np.linspace(0,(len(uinput)-1)*dt,len(uinput))
label = [r'$\phi$',r'$\theta$',r'$\psi$']
fig,ax = plt.subplots(1,3,figsize=(15,4))
axes.grid(ls="--",lw=0.5,color="black")
axes.plot(time_rk,np.degrees(y_rk[:,i]),lw=2.5,color='b')
axes.set_ylabel(label[i]+' (deg)')
axes.set_xlim(0,time_rk[-1])
axes.set_xlabel('time (s)')
No. 3
import numpy as np
import pandas as pd
# from matplotlib import style
df = pd.read_csv("flight_03.csv")
df
label = ["Ground Speed (m/s)", "Track angle (deg)", "Vertical Speed (m/s)"]
if i < 3:
if i == 1:
chi_deg = df.iloc[:, i+1].values * np.rad2deg(i) % 360
else:
axes.set_xlim(0)
else:
axes.remove()
df["gamma_rad"] = np.divide(df.hdot_mps, df.gs_mps)

df["gamma_rad"] = df["gamma_rad"].replace('0.0', 1E-8)
df["gamma_rad"] = df["gamma_rad"].replace(np.nan, 1E-8)
df["gamma_rad"] = df["gamma_rad"].replace(np.inf, 1E-8)
df["gamma_rad"] = df["gamma_rad"].replace(-np.inf, 1E-8)
df

label = ["Ground Speed (m/s)", "Track angle (deg)", "Vertical Speed (m/s)", "Gamma (deg)"]
if i == 1:
chi_deg = df.iloc[:, i+1].values * np.rad2deg(1) % 360
elif i == 4:
gamma_deg = df.iloc[:, i+1].values * np.rad2deg(1)
axes.plot(time_s, gamma_deg, "--")
else:
axes.set_xlim(0)
from numpy import float64, cos, sin
def xdot_kpm_ned(xstate, uinp, param):

"""
State Model of kinematic point mass model w.r.t NED Frame Reference
where uinp consists of 3x1 Numpy array:
ks_mps = kinematics speed (m/s)
chi_rad = track angle (rad)
gamma_rad = flight path angle (rad)
"""
ks_mps, chi_rad, gamma_rad = uinp
xdot = np.zeros(3, dtype=float64)
xdot[0] = ks_mps * cos(chi_rad) * cos(gamma_rad)
xdot[1] = ks_mps * sin(chi_rad) * cos(gamma_rad)
xdot[2] = ks_mps * sin(gamma_rad)
return xdot
def yout_kpm_ned(xstate, uinp, param):

""" Output Model of Kinematic Point Mass w.r.t NED Frame"""
return xstate
param, dt = 0.0, 1.0

uinput = df.loc[:, ["gs_mps", "chi_rad", "gamma_rad"]].to_numpy()
xstate = np.zeros(3, dtype=np.float64)

ystorage = np.zeros(shape=(uinput.shape[0], 3), dtype=np.float64)
for i in range(uinput.shape[0]):
ystorage[i, :] = yout_kpm_ned(xstate, uinput[i], param)
if i > 0:
u1 = uinput[i-1, :]
u2 = uinput[i, :]
xstate = ruku4(xdot_kpm_ned, xstate, dt, u1, u2, param)

axes[0].plot(ystorage[:, 1], ystorage[:, 0])
axes[0].set_xlabel("East-Distance (m)", fontsize=14)
axes[0].set_ylabel("North-Distance (m)", fontsize=14)
axes[1].plot(ystorage[:, 2])
axes[1].legend(["Simulation"], fontsize=14)
# Conversion from NED to LLA (WGS-84)

r2d = np.degrees(1)
lat_init_deg = -0.106821841 * np.rad2deg(1)
lon_init_deg = 1.861220837 * np.rad2deg(1)
hgps_init_m = 182.88
lat_ref, lon_ref, alt_ref = lat_init_deg, lon_init_deg, hgps_init_m
from navpy import ned2lla

lat_deg, lon_deg, _ = ned2lla(ystorage, lat_ref, lon_ref, alt_ref)

axes[0].plot(lon_deg, lat_deg)
axes[0].set_xlabel("Longitude (deg)", fontsize=14)
axes[0].set_ylabel("Latitude (deg)", fontsize=14)
hgps_m = ystorage[:, 2]
axes[1].plot(hgps_m)
from utilib.kml import create_kml

create_kml(lat_deg, lon_deg, hgps_m, "deg", "trajectory-sim")

Parameter Identification HW2

Diunggah oleh

Informasi Dokumen

Hak Cipta

Format Tersedia

Bagikan dokumen Ini

Bagikan atau Tanam Dokumen

Opsi Berbagi

Apakah menurut Anda dokumen ini bermanfaat?

Apakah konten ini tidak pantas?

Hak Cipta:

Format Tersedia

Parameter Identification HW2

Diunggah oleh

Hak Cipta:

Format Tersedia

TUGAS 2

AE6001 KAPITA SELEKTA DIRGANTARA B

PROGRAM STUDI MAGISTER TEKNIK DIRGANTARA

a. Dilanjutkan dengan mendefenisikan fungsi general untuk mendapatkan respon sistem

def euler(statemodel, xstate, dt, u, param):

Baca data dari file .csv yang diberikan

Gambar 1. Plot Input Doublet pada Sistem

def state_model(x, u, param):

param = np.array([-2.21, 2.0, -3.5, -0.5, 0.7, 2.0])

x_eu = np.zeros(shape=(ndata, 2))

Gambar 2. Plot Respon Sistem Dinamik dengan Metode Euler

Function Evaluations Solution

Sehingga pada program python, dapat dituliskan seperti berikut

def ruku4(statemodel, xstate, dt, u1, u2, param):

Inisialisasi data juga dilakukan untuk metode ini

param = np.array([-2.21, 2.0, -3.5, -0.5, 0.7, 2.0])

x_rk = np.zeros(shape=(ndata, 2))

fig, ax = plt.subplots(1,2, figsize=(15, 4))

Gambar 4. Perbandingan Metode Eular dengan Metode Runge-kutta orde ke-4

def discretize_method(A, B, u, x_init, dt):

def compute_phi(A, dt):

A = np.array([[-2.21, 2.0], [-3.5, -0.5]])

x_di = discretize_method(A, B, uinp, x_init, dt)

fig, ax = plt.subplots(1,2, figsize=(15, 4))

Gambar 8. Perbandingan Plot Respon Sistem dengan Ketiga Metode

Gambar 9. Data Gerakan Wahana (p, q, r)

Visualisasi dari hasil program di atas adalah sebagai berikut

Gambar 11. Hasil Simulasi “Attitude Propagation” pada model nonlinear

Dan data dapat dilihat dari output code program di atas

df["gamma_rad"] = np.divide(df.hdot_mps, df.gs_mps)

time_s = np.linspace(0, df.shape[0] * 0.1, df.shape[0])

def xdot_kpm_ned(xstate, uinp, param):

def yout_kpm_ned(xstate, uinp, param):

xstate = np.zeros(3, dtype=np.float64)

# Conversion from NED to LLA (WGS-84)

from navpy import ned2lla

fig, axes = plt.subplots(2, 1, figsize=(8,10))

Visualisasi hasil plot dapat dilihat pada gambar berikut

from utilib.kml import create_kml

Gambar 18. Rekonstruksi Terbang Pesawat pada Goolge Earth

Gambar 19. Lokasi Kondisi Awal pada Kurva Latitude vs Longitude

Gambar 20. Lokasi Mendarat Rekonstruksi Terbang Pesawat

[1] H. Muhammad and J. Sembiring, Mathematical Modeling of Flight Vehicle - Part 2,

def euler(statemodel, xstate, dt, u, param):

def state_model(x, u, param):

param = np.array([-2.21, 2.0, -3.5, -0.5, 0.7, 2.0])

x_eu = np.zeros(shape=(ndata, 2))

fig, ax = plt.subplots(1,2, figsize=(15, 4))

def ruku4(statemodel, xstate, dt, u1, u2, param):

param = np.array([-2.21, 2.0, -3.5, -0.5, 0.7, 2.0])

x_rk = np.zeros(shape=(ndata, 2))

fig, ax = plt.subplots(1,2, figsize=(15, 4))

fig, ax = plt.subplots(1,2, figsize=(15, 4))

def discretize_method(A, B, u, x_init, dt):

A = np.array([[-2.21, 2.0], [-3.5, -0.5]])

fig, ax = plt.subplots(1,2, figsize=(15, 4))

A = np.array([[-2.21, 2.0], [-3.5, -0.5]])

x_di_a = discretize_method(A, B, uinp, x_init, dt)

def compute_phi_i(A, dt):

x_di_b = discretize_method_i(A, B, uinp, x_init, dt)

fig, ax = plt.subplots(1,2, figsize=(15, 4))

fig, ax = plt.subplots(1,2, figsize=(15, 4))

from numpy import float64, cos, sin, tan

df["gamma_rad"] = np.divide(df.hdot_mps, df.gs_mps)