PENGEMBANGAN ALGORITMA UNTUK ESTIMASI POSISI PADA SISTEM NAVIGASI DAN TRAYEKTORI WAHANA NIR AWAK BAWAH AIR ITS AUV 01

(1)

PENGEMBANGAN ALGORITMA UNTUK ESTIMASI

POSISI PADA SISTEM NAVIGASI DAN

TRAYEKTORI WAHANA NIR AWAK BAWAH AIR

ITS AUV 01

Oleh :

Gustiyadi Fathur R.

(2107 100 109)

Dosen Pembimbing:

Hendro Nurhadi, Dipl.-Ing., Ph.D (19751120 200212 1 002)

Subchan, Ssi., MSc., PhD

(19710513 199702 1 001)

Sidang Tugas Akhir

(2)

BAB 1

PENDAHULUAN

RUMUSAN DAN

BATASAN

MASALAH

(4)

LATAR BELAKANG

Unmanned

Underwater

Vehicle

AUV

ROV

Perancangan algoritma

estimasi posisi pada

sistem navigasi & trayektori

Metode

Ensemble Kalman Filter

(EnKF)

Aplikasi di beberapa

sektor bidang

(sains, lingkungan,

militer dll)

Gangguan internal

dan eksternal

(5)

RUMUSAN DAN BATASAN MASALAH

• Rumusan Masalah

Bagaimana mengimplementasi EnKF untuk mengestimasi posisi

AUV dengan trayektori yang sudah ditentukan ?

• Batasan Masalah

a. Sistem bersifat time invariant

b. AUV memiliki GPS

c. Trayektori sudah ditentukan dan rintangan diabaikan

d. Kecepatan AUV maksimal 4 knot (2 m/s)

e. Kedalaman maksimal 50 m

f. Dimensi AUV ialah panjang 1500 mm dan diameter 200 mm

g. Pemodelan gerak AUV diproyeksikan pada 2 dimensi

h. Simulasi hanya dilakukan untuk gerak translasi saja

i. Simulasi dilakukan dengan bantuan program Matlab

(6)

TUJUAN DAN MANFAAT

• TUJUAN :

Mendapatkan hasil estimasi posisi AUV dengan trayektori

yang telah ditentukan dan menggunakan metode Ensemble

Kalman Filter (EnKF).

• MANFAAT :

• Memberikan gambaran bagaimana EnKF mengestimasi

posisi AUV sesuai trayektori.

• Sebagai penunjang penelitian khusus pada bidang robotika.

• Dapat diterapkan dalam teknologi yang menunjang

pertahanan dan keamanan NKRI.

• Sebagai

tambahan

kepustakaan

untuk

penelitian

selanjutnya.

(7)

BAB 2

Hasil Studi Peneliti Terdahulu

Dasar Teori

Pengembangan Algoritma

Tinjauan

Pustaka

(8)

Hasil Studi Peneliti Terdahulu

• Penelitian mengenai estimasi lintasan AUV telah dilakukan

oleh Risa Fitria [4]. Metode estimasi yang digunakan adalah

Ensemble Kalman Filter (EnKF) dengan persamaan sistem

dinamika kapal selam berdasarkan persamaan Morrison [6].

• Penelitian yang dilakukan oleh Achmad Ichwan [3] adalah

mengestimasi posisi kapal selam dengan mengimplementasikan

metode Extended Kalman Filter (EKF) pada persamaan sistem

dinamika berdasarkan persamaan Morrison [6].

(9)

DASAR TEORI

Kinematika

AUV

Algoritma

Rigid Body

Hydro-dynamics

(10)

Desain ITS AUV

(11)

Kinematika AUV



Posisi dan Sudut Euler



Gaya dan Momen

T T T

]

,

[



₁



₂





T

z

y

x

,

]

[

1





T

]

,

[

2













_{Kecepatan linier dan Anguler}

T T T

v

,

]

[

₁ ₂





T

w

v

u

,

]

[

1





T

r

q

p

,

]

[

2





T T T

]

,

[



₁



₂





T

Z

Y

X

,

]

[

1





T

N

M

K

,

]

[

1





(12)

                 cos sin 0 sin cos 0 0 0 1 , x C                  cos 0 sin 0 1 0 sin 0 cos , y C             1 0 0 0 cos sin 0 sin cos ,      z C

Koordinat Rotasi pada EFF

T x T y T z

C

J

₁

(



₂

)



_,_ _,_ _,_                  



























































cos cos sin cos sin cos sin sin sin cos cos cos sin sin sin cos sin cos sin cos sin sin sin sin cos cos sin cos cos ) ( ₂ 1 J

Kecepatan Linier

θ θ X1 X2 Z2 Z1 ψ ψ Y2 X₃ Y₃ X2 Z₁ Z0 Y1 Y0 ϕ ϕ

(13)

Rigid Body Equation AUV

1. Surge 2. Sway 3. Heave 4. Roll 5. Pitch 6. Yaw



u

vr

wq

x

q

r

y

pq

r

z

pr

q



X

m









_G

(

2



2

)



_G

(





)



_G

(





)





v

wp

ur

y

r

pr

z

qr

p

x

qp

r



Y

m









_G

(

2



2

)



_G

(





)



_G

(





)





w

uq

vp

z

p

qp

x

rp

q

y

rq

p



Z

m









_G

(

2



2

)



_G

(





)



_G

(





)





y w uq vp z v wp ur



K m I q pr I q r I pq r qr I I p I_x  ( _z  _y) ( ) _xz ( 2  2) _yz (  ) _xy  _G(   ) _G(  ) 



z u vr wq x w uq vp



M m I r qp I r p I qr p rp I I q I_y ( _x  _z) (  ) _xy ( 2  2) _zx (  ) _yz  _G(  ) _G(   ) 



x v wp ur y u vr wq



N m I p rq I p q I rp q pq I I r Iz ( y  x) (  ) yz (  ) xy (  ) zx  G(   ) G(   )  2 2 _ _ _  

(14)

Hidrodinamika AUV

Gaya dan Momen Luar (



)

(15)



hidrostatis T G



[

0

0 W ]



G l G

J







₁1

(

₂

)

T B



[

0

0 

B]



B l B

J







₁1

(

₂

)

Gravitasi Bouyancy C_G B c_B C_G B W C_B W B W C_B C_G B W B W

(16)

T added mass

2 Bidang

Sama

(17)

(18)



drag x y δ_r β_e u v V fluida u W fin x z _δ s β_e V fluida re r re













_se





_s





_se

(19)



lift l AS s R

A

l

planform

wingspan

A

2



R L

A

C

2 







e L

Av

C

L

2 





_L

L

C



.

(20)



thrust

D

ω

Gaya dan Momen Thruster

p p T t p p T t

J

K

D

J

K

D

T











)

(

2

1 )

(

2

1

5 4



D

V

J

P A





u

V

_A



(

1 



)



 





n in i ti ti x n i i x

xT

r

M

T

F

1 1 3 1 1 3



dimana

Persamaan Umum

(21)

Total Gaya dan Momen

• Translasi sepanjang arah x :

• Translasi sepanjang arah y :

(22)

• Rotasi sepanjang arah x :

• Rotasi sepanjang arah y :

(23)

Ensemble Kalman Filter (EnKF)

Merupakan salah satu metode dalam asimilasi data yang telah

banyak digunakan untuk mengestimasi berbagai persoalan bentuk

model sistem nonlinear, dan mampu menyelesaikan model sistem

dinamik nonlinear dan ruang keadaan (state space) yang besar.

Ada tiga tahapan :

 Tahap inisialisasi

 Tahap prediksi (time update step)

(24)

Algoritma Ensemble Kalman Filter (EnKF)

Inisialisasi

Tentukan nilai awal

Tahap Prediksi

Estimasi

Kovarian Error

Tahap Koreksi

Kalman Gain

Estimasi

Kovarian Error

]

[

₀_,₁ ₀_,₂ ₀_,₃ ₀_, , 0i

x

N

x









N i i

x

N

x

1 , 0 0

ˆ

1 ˆ

i k k k k

f

x

u

w

x

ˆ





(

ˆ

_₁

,

_₁

)



_, T k i k N i k i k k x x x x N P (ˆ ˆ )(ˆ ˆ ) 1 1 , 1 ,       _ _  



   _ N i i k k x N x 1 , ˆ 1 ˆ i k k i k

z

v

z

_,





_, 1

)

(

  

_



T _k k T k k

P

H

HP

H

R

K





_k _k k

I

K

H

P

[

]

) ˆ ( ˆ _, _, _, ,   _ _  _k _i _k _k_i _k _i i k x K z Hx x

(25)

BAB 3

METODOLOGI

Penelitian

(26)

FLOWCHART PENELITIAN

Kajian Pustaka Spesifikasi Dimensi dan Dinamika Gerak

AUV

Menentukan Desain AUV dan Trayektori Mendapatkan Persamaan Gerak AUV pada 6 DOF

RMS Error 0 ~ 1 ?

Menganalisa Hasil Simulasi

Menarik Kesimpulan dari Hasil Simulasi

T

Y START

(27)

(28)

(29)

BAB 4

ANALISA & PEMBAHASAN

4.1 Diskritisasi

Model AUV

4.2 Penambahan

Faktor

Stokastik

4.3 Implementasi

Model ITS

AUV pada

Algoritma

EnKF

4.4 Simulasi dan

Analisa

(30)

Diskritisasi Model AUV

Metode Pendiskritan  Beda Hingga Maju

(31)

Translasi sepanjang arah y :

(32)

Penambahan Faktor Stokastik

Model Stokastik :

 f

ungsi nonlinear

dimana

w

_k

ialah noise sistem  Q

_k

(program)

v

_k

ialah noise pengukuran  R

_k

(program)

(33)

Implementasi Model ITS AUV pada Algoritma EnKF

Mendefinisikan x pada fungsi x

_k+1

:

 penentuan nilai awal untuk masing-masing posisi (nol)

• Model Sistem

(34)

• Model Pengukuran

 satu jenis data ukur yaitu komponen u

 satu jenis data ukur yaitu komponen v

 satu jenis data ukur yaitu komponen w

 dua jenis data ukur yaitu komponen u dan v

 dua jenis data ukur yaitu komponen u dan w

 dua jenis data ukur yaitu komponen v dan w

Sebagai contoh : H = 2 sampel, yaitu v dan w

(35)

• Inisialisasi

 Pembangkitan sejumlah ensemble awal + noise-nya :

(36)

(37)

• Tahap Prediksi



Menghitung nilai prediksi awal + noise-nya :

(38)

(39)

• Tahap Koreksi

 Menghitung data pengukuran :

 Menghitung Kalman Gain :

 Perhitungan estimasi koreksi :

 Perhitungan nilai mean estimasi koreksi :

(40)

Simulasi dan Analisa

(41)

(42)

(43)

(44)

(45)

(46)

(47)

(48)

• RMSE Gabungan

(49)

BAB 5

5.1 Kesimpulan

5.2 Saran

(50)

Kesimpulan

• Metode Ensemble Kalman Filter (EnKF) yang digunakan telah berhasil diimplementasikan untuk sistem navigasi pada bagian estimasi posisi pada gerak translasi yaitu surging, swaying, heaving. Hal ini terlihat dari besarnya RMSE yang relatif kecil pada tiap statenya.

• Hasil terbaik yang didapatkan dari hasil simulasi dengan menggunakan empat parameter data pengukuran dan 1000x iterasi adalah :

a. Ensemble 100  simulasi 1  RMSE  surging = 0,013127; swaying = 0,09812; dan heaving = 0,10039.

b. Ensemble 200  simulasi 3  RMSE  surging = 0,01336; swaying = 0,09814; dan heaving = 0,10075.

c. Ensemble 300  simulasi 6  RMSE  surging = 0,01342; swaying = 0,09449; dan heaving = 0,09739.

d. Ensemble 400  simulasi 8  RMSE  surging = 0,013137; swaying = 0,098465; dan heaving = 0,098871.

• Dari hasil nilai RMSE rata-rata pada tabel 4.9  teori hubungan antara jumlah ensemble yang digunakan terhadap nilai RMSE yang diperoleh seharusnya berbanding terbalik dimana semakin bertambah jumlah ensemble-nya maka semakin kecil nilai RMSE yang diperoleh, tidak terbukti. Kemungkinan  distribusi normal pada noise dan tidak adanya sistem kontrol.

(51)

Saran

• Ketika menurunkan persamaan dinamika AUV, diharapkan untuk menghitung

semua komponen-komponen gayanya sehingga dalam penyesuaian parameter

(penerapan asumsi yang harus ditiadakan) pada saat implementasi suatu metode

menghasilkan kondisi yang sebenarnya.

• Metode EnKF juga dapat diterapkan untuk estimasi posisi ketika AUV melakukan

gerak rotasi, yaitu pitching, yawing, dan rolling.

(52)

DAFTAR PUSTAKA

• Budiyanto,D. 2001. Sistem Permesinan Kapal Selam

• Evensen, G. 2003. The ensemble Kalman filter: theoretical formulation and practical implementation, Ocean Dynamics, 53: 343-3

• Ichwan, A. 2010. Estimasi posisi Kapal Selam Menggunakan metode Extended Kalman Filter. Tugas Akhir, Institut Teknologi Sepuluh Nopember, Surabaya

• Fitria, Risa. 2011. Implementasi Ensemble Kalman Filter pada Estimasi Kecepatan Kapal Selam. Tugas Akhir, Institut Teknologi Sepuluh Nopember, Surabaya

• Lewis, F.L. 1986. Optimal Estimation with an Introduction to Stochastic Control Theory, John Wiley & Sons., New York

• Purnomo, Kosala Dwidja. 2008. Aplikasi Metode Ensemble Kalman Filter pada Model Populasi Plankton. Institut Teknologi Sepuluh Nopember: Surabaya

• Jeffi, Trio. 2011. Pengembangan Sistem Kendali Robust AUV dengan Metode Sliding-PID. Institut Teknologi Sepuluh Nopember: Surabaya

• Walchko, K. J., Novick, David, and Nechyba, M. C. 2003. Development of a Sliding Mode Control Sistem with

Extended Kalman Filter Estimation for Subjugator, University of Florida Gainesville, FL, 32611-6200

• Nahon, Meyer. 1998. A Simplified Dynamics Model for Autonomous Underwater Vehicle. University of Victoria: Canada

(53)

• Jwo, Dah-Jing dan Ta-Shun Cho.2010. Critical remarks on the linearised and extended Kalman ﬁlters with

geodetic navigation examples. Journal, measurement XXX (1-13). Taiwan.

• Luknanto, Djoko. Hidraulika Komputasi, Metoda Beda Hingga. Jurusan Teknik sipil. Universitas Gadjah Mada.

• Nisa, Fitri Parsa. 2009. Sistem Pengendali Gerak Pada Kapal Selam Menggunakan Metode Sliding Mode

Control. Tugas Akhir. Institut Teknologi Sepuluh Nopember, Surabaya

• Yang, Chen. 2007. Modular Modeling and Control for Autonomous Underwater Vehicle (AUV), Departement of Mechanical Engineering National University of Singapore. Singapore

• T. Perez, Ø.N. Smogeli, T.I. Fossen and A.J. Sørensen. 2005. An Overview of Marine Systems Simulator (MSS):

A Simulink Toolbox for Narine Control System. SIMS2005-Scandinavian Conference on Simulation and

Modelling.

• T. I., Fossen. 2005. A Nonlinear Unified State-space Model for Ship Maneuvering and Control in A Seaway. Journal of Bifurcation and Chaos.

• Louis Andrew Gonzalez. 2004. Design, Modelling and Control of an Autonomous Underwater Vehicle, mobile Robotics Laboratory, center for Intelligent Information Processing Systems, School of Electrical, Electronic and Computer engineering, The University of Western Australia.

• Lewis, M. J, dkk. 2006. Dynamic Data Assimilation: A Least Squares Approach. University Press, Cambridge.

• Setiawan, Agus. 2012. Rancang Bangun Prototype Kapal Selam Tanpa Awak (ITS AUV-01) dengan Aplikasi

(54)

(55)

LAMPIRAN

PROGRAM SIMULASI M.FILE

clear all; clc; clf;

% Data Awal:

m=19.8; %vehicle mass in (kg)

g=9.81; %gravitaty acceleration in (m/s^2) rho=1025; %fluid density in (kg/m^3)

dt=0.1; xg=0.062; yg=-0.0013; zg=0.05; xb=0.062; yb=0; zb= 0;

volume=0.00219444; %vehicle volum in (m^3) Dprop=0.01; %propeller diameter in (m)

(56)

Kt=0.2265; %thrust coefficient

alpha1=0.4; %wake fraction number (0.1 ~ 0.4) omegap=2400; %rotating rate propeller in (rpm) css=0.07; %axial drag

Ap=661873.992/1000000; %top view projected area in (m^2)

Af=(0.5*4*pi*0.1^2)+(4*0.18*0.01); %front view projected area in (m^2) cdc=0.01; %drag koefisien silinder [S.F. Hoerner]

Dv=0.2; %dimeter vehivle in (m) l=1.5; %vehicle length in (m)

alpha2=0.03794; %empirical parameter (interpolasi) Dh=0.2; %hull diameter in (m)

t=0.571; %fin taper ratio

afin=0.25; %max fin height above the vehicle centerline in (m) rmean=0.35; %mean fin height above the vehicle centerline in (m)

xfin=0.638; %momen arm wrt the origin~fin cyb=0.003; %6.7<=(l/Dh)<=10

cydb=(l/Dh)*cyb;

(57)

dr=(30)*pi/180; %vertical fin angles referenced to the vehicle hull ds=(30)*pi/180; %horizontal fin angles referenced to the vehicle hull Ix=0.08583; Iy=1.11575; Iz=1.11575; xn0=0.5; %(m) xm0=0.4; %(m) xt0=0.4; %(m) Sfin=0.00825; %(m^2) Sn1=5.141; Sm1=1; St1=0; Sn2=338.119; Sm2=1; St2=0; Sn3=23010.065; Sm3=1; St3=0;

(58)

Sn4=1801752.357; Sm4=1; St4=0; Sn5=-1315.546; Sm5=1; St5=2.7691e+33; Sn6=-27363.569; Sm6=1; St6=1.884012468e+37; Sn7=-2479376.013; Sm7=1; St7=5.497680779e+39; u0=0; v0=0; w0=0;

uawal=2; %(m/s = uawal*2 knot) [u dlm persamaan Va] I=1000; %jumlah iterasi

(59)

W=m*g; B=rho*g*volume; Va=(1-alpha1)*uawal; cd=(css*pi*Ap*(l+((60*(Dv/l)^3))+((0.25*Dv)/l)))/Af; J=Va/(omegap*Dprop); cdf=0.1+0.7*t; clalpha=1/((1/(2*pi*0.3))+(1/(pi*0.5)));

for i=1:I/4 %sudut pitch di titik 1-250 pitch1(1,i)=(-2)*pi/180;

end

for i=1:I/4 %sudut pitch di titik 251-500 pitch2(1,i)=(0)*pi/180;

end

for i=1:I/4 %sudut pitch di titik 501-750 pitch3(1,i)=(0)*pi/180;

(60)

for i=1:I/4 %sudut pitch di titik 751-1000 pitch4(1,i)=(0)*pi/180; end pitch=[pitch1,pitch2,pitch3,pitch4]; for k=1:i sdtpit1=sin(pitch(k)); sdtpit2=cos(pitch(k)); end

for j=1:I/2 %sudut roll di titik 1-50 roll1(1,j)=(0)*pi/180;

end

for j=1:I/2 %sudut roll di titik 51-100 roll2(1,j)=(0)*pi/180; end roll=[roll1, roll2]; for o=1:j sdtrol1=sin(roll(o)); sdtrol2=cos(roll(o)); end

(61)

% Gaya Hidrostatis: Xres=(B-W)*sdtpit1;

Yres=(W-B)*sdtpit2*sdtrol1; Zres=(W-B)*sdtpit2*sdtrol2;

% Axial~Crossflow~Rolling Drag Coefficient: Xuu=-(0.5*rho*cd*Af); Yvv=(-rho*cdc*(Sn1+Sm1+St1))-(rho*Sfin*cdf); Zww=Yvv; Yrr=(-rho*cdc*(Sn3+Sm3+St3))-... (2*rho*cdc*((xn0*Sn2)+(xm0*Sm2)+(xt0*St2)))-... (rho*cdc*((xn0^2*Sn1)+(xm0^2*Sm1)+(xt0^2*St1)))-... (rho*Sfin*cdf*xfin*abs(xfin)); Zqq=-Yrr; % Body Lift: Yuvl=-(0.5*rho*Dh^2*cydb); Zuwl=Yuvl;

(62)

%Axial~Crossflow~Rolling Added Mass Coefficient: Xudot=-(alpha2*rho*pi*l*Dh^2)/6; Yvdot=-(Sn5+Sm5+St5); Zwdot=Yvdot; Mwdot=(Sn6+Sm6+St6)+((xn0*Sn5)+(xm0*Sm5)+(xt0*St5)); Zqdot=Mwdot; Nvdot=-Mwdot; Yrdot=Nvdot; % fin Lift: Yuudr=rho*clalpha*Sfin; Yuvf=-Yuudr; Zuuds=-Yuudr; Zuwf=-Zuuds;

% Propeller Force & Moment:

Xprop=0.5*rho*(Dprop^4)*Kt*J*omegap; Kprop=0.5*rho*(Dprop^5)*Kt*J*omegap;

(63)

u(1)=u0; v(1)=v0; w(1)=w0; Q1=0.001*pi/180; Q2=0.001; Q3=0.001; Q11=[Q1 0 0;0 Q2 0;0 0 Q3]; R1=0.01*pi/180; R2=0.01; R3=0.01; R11=[R1 0 0;0 R2 0;0 0 R3];

(64)

H1=eye(3);

% H1=[1 0 0]; %u sebagai data pengukuran % H1=[0 1 0]; %v sebagai data pengukuran % H1=[0 0 1]; %w sebagai data pengukuran

% H1=[1 0 0;0 1 0]; %u dan v sebagai data pengukuran % H1=[1 0 0;0 0 1]; %u dan w sebagai data pengukuran %H1=[0 1 0;0 0 1]; %v dan w sebagai data pengukuran

x001=[u(1) v(1) w(1)]';

% Membangkitkan Ensemble Awal for ens=1:N u2d=x001(1,1)+normrnd(0,sqrt(Q1),1,1); v2d=x001(2,1)+normrnd(0,sqrt(Q2),1,1); w2d=x001(3,1)+normrnd(0,sqrt(Q3),1,1); x01(:,ens)=[u2d v2d w2d]'; end

(65)

x01mean=mean(x01,2); xpre01=x01mean; xcor1=x01mean; rek1(1,1)=x001(1,1); rek1(2,1)=x001(2,1); rek1(3,1)=x001(3,1); Rekam1(1,1)=xcor1(1,1); Rekam1(2,1)=xcor1(2,1); Rekam1(3,1)=xcor1(3,1); u2(1)=x01mean(1,1); v2(1)=x01mean(2,1); w2(1)=x01mean(3,1);

(66)

for k=1:I % Sistem Real : Uu(k)=((Xres)+(Xuu*u(k)*abs(u(k)))+(Xprop))/(m-Xudot); Vv(k)=((Yres)+(Yvv*v(k)*abs(v(k)))+((Yuvl+Yuvf)*u(k)*v(k))+... (Yuudr*u(k)^2*dr))/(m-Yvdot); Ww(k)=((Zres)+(Zww*w(k)*abs(w(k)))+((Zuwl+Zuwf)*u(k)*w(k))+... (Zuuds*u(k)^2*ds))/(m-Zwdot); u(k+1)=Uu(k)*dt+u(k)+normrnd(0,sqrt(Q1),1,1); v(k+1)=Vv(k)*dt+v(k)+normrnd(0,sqrt(Q2),1,1); w(k+1)=Ww(k)*dt+w(k)+normrnd(0,sqrt(Q3),1,1); xreal1=[u(k+1) v(k+1) w(k+1)]'; rek1(1,k+1)=u(k+1); rek1(2,k+1)=v(k+1); rek1(3,k+1)=w(k+1);

(67)

z1=H1*xreal1+sqrt(R11)*randn(3,1); % untuk H identitas

% z1 = H1*xreal1+sqrt(R1)*randn(1,1); % untuk H u [1 0 0] % z1 = H1*xreal1+sqrt(R2)*randn(1,1); % untuk H v [0 1 0] % z1 = H1*xreal1+sqrt(R3)*randn(1,1); % untuk H w [0 0 1]

% z1 = H1*xreal1+sqrt([R1 0;0 R2])*randn(2,1); % untuk H u dan v [1 0 0;0 1 0] % z1 = H1*xreal1+sqrt([R1 0;0 R3])*randn(2,1); % untuk H u dan w [1 0 0;0 0 1] %z1 = H1*xreal1+sqrt([R2 0;0 R3])*randn(2,1); % untuk H v dan w [0 1 0;0 0 1]

%---> TAHAP PREDIKSI % Estimasi Ensemble Uu2(k)=((Xres)+(Xuu*u(k)*abs(u(k)))+(Xprop))/(m-Xudot); Vv2(k)=((Yres)+(Yvv*v(k)*abs(v(k)))+((Yuvl+Yuvf)*u(k)*v(k))+... (Yuudr*u(k)^2*dr))/(m-Yvdot); Ww2(k)=((Zres)+(Zww*w(k)*abs(w(k)))+((Zuwl+Zuwf)*u(k)*w(k))+... (Zuuds*u(k)^2*ds))/(m-Zwdot);

(68)

u2(k+1)=Uu2(k)*dt+u2(k)+normrnd(0,sqrt(Q1),1,1); v2(k+1)=Vv2(k)*dt+v2(k)+normrnd(0,sqrt(Q2),1,1); w2(k+1)=Ww2(k)*dt+w2(k)+normrnd(0,sqrt(Q3),1,1); for ens=1:N u2d_new=u2(k+1)+normrnd(0,sqrt(Q1),1,1); v2d_new=v2(k+1)+normrnd(0,sqrt(Q2),1,1); w2d_new=w2(k+1)+normrnd(0,sqrt(Q3),1,1);

xpre1(:,ens)=[u2d_new v2d_new w2d_new]'; end

% Mean (rata-rata) Ensemble xpre_rt1=mean(xpre1,2);

% Error Ensemble for ens=1:N

xpre_rt2(:,ens)=xpre_rt1; end

(69)

ek1=xpre1-xpre_rt2;

% Kovarian Error Ensemble C7=ek1*ek1';

Ppre1=(1/(N-1))*C7;

%----> TAHAP KOREKSI for ens=1:N

z1_new(:,ens) = z1+sqrt(R11)*randn(3,1); % untuk H identitas % z1_new(:,ens) = z1+sqrt(R1)*randn(1,1); % untuk H u [1 0 0] % z1_new(:,ens) = z1+sqrt(R2)*randn(1,1); % untuk H v [0 1 0] % z1_new(:,ens) = z1+sqrt(R3)*randn(1,1); % untuk H w [0 0 1]

% z1_new(:,ens) = z1+sqrt([R1 0;0 R2])*randn(2,1); % untuk H u dan v [1 0 0;0 1 0] % z1_new(:,ens) = z1+sqrt([R1 0;0 R3])*randn(2,1); % untuk H u dan w [1 0 0;0 0 1] %z1_new(:,ens) = z1+sqrt([R2 0;0 R3])*randn(2,1); % untuk H v dan w [0 1 0;0 0 1] end

(70)

% Kalman Gain

K1=Ppre1*H1'*inv(H1*Ppre1*H1'+R11); % untuk H identitas % K1=Ppre1*H1'*inv(H1*Ppre1*H1'+R1); % untuk H u [1 0 0] % K1=Ppre1*H1'*inv(H1*Ppre1*H1'+R2); % untuk H v [0 1 0] % K1=Ppre1*H1'*inv(H1*Ppre1*H1'+R3); % untuk H w [0 0 1]

% K1=Ppre1*H1'*inv(H1*Ppre1*H1'+[R1 0;0 R2]); % untuk H u dan v [1 0 0;0 1 0] % K1=Ppre1*H1'*inv(H1*Ppre1*H1'+[R1 0;0 R3]); % untuk H u dan w [1 0 0;0 0 1] %K1=Ppre1*H1'*inv(H1*Ppre1*H1'+[R2 0;0 R3]); % untuk H v dan w [0 1 0;0 0 1]

% Menghitung Estimasi Ensemble

xcor1 = xpre1+K1*(z1_new-H1*xpre1); % Menghitung Estimasi xcor1 = mean(xcor1,2); Rekam1(1,k+1) = xcor1(1); Rekam1(2,k+1) = xcor1(2); Rekam1(3,k+1) = xcor1(3);

(71)

pcor1 = [eye(3)-K1*H1]*Ppre1; % Kovarian Error

u2(k+1) = xcor1(1); v2(k+1) = xcor1(2); w2(k+1) = xcor1(3);

% estimasi RMSE(Root Mean Square Error/Rata-rata Error Akar Kuadrat) erru=abs(rek1(1,:)-Rekam1(1,:)); errou(k)=(erru(k))^2; erroru=sqrt(mean(errou,2)); errv=abs(rek1(2,:)-Rekam1(2,:)); errov(k)=(errv(k))^2; errorv=sqrt(mean(errov,2)); errw=abs(rek1(3,:)-Rekam1(3,:)); errow(k)=(errw(k))^2; errorw=sqrt(mean(errow,2)); end

(72)

disp(['RMS Error pada u = ',num2str(erroru)]); disp(['RMS Error pada v = ',num2str(errorv)]); disp(['RMS Error pada w = ',num2str(errorw)]);

%Plot hasil simulasi

%Proyeksi Gerak antara uv, uw dan vw figure (1)

% subplot(1,3,1)

plot((rek1(1,:)),rek1(2,:),'-r',Rekam1(1,:),Rekam1(2,:),'-b') grid on;

title('Proyeksi Gerak AUV terhadap Bidang X-Y'); xlabel('X (m)');

ylabel('Y (m)'); legend('Reference','EnKF');

(73)

figure(2)

% subplot(1,3,2)

plot((rek1(1,:)),rek1(3,:),'-r',Rekam1(1,:),Rekam1(3,:),'-b') grid on;

title('Proyeksi Gerak AUV terhadap Bidang X-Z'); xlabel('X (m)'); ylabel('Z (m)'); legend('Reference','EnKF'); figure(3) % subplot(1,3,3) plot((rek1(2,:)),rek1(3,:),'-r',Rekam1(2,:),Rekam1(3,:),'-b') grid on;

title('Proyeksi Gerak AUV terhadap Bidang Y-Z'); xlabel('Y (m)');

ylabel('Z (m)'); legend('Reference','EnKF');

(74)

% Gerak antara u, v dan w terhadap iterasi figure (4) subplot(3,1,1) plot((1:I+1),rek1(1,:),'-r',(1:I+1),Rekam1(1,:),'-b') grid on; title('Estimasi u'); xlabel('iterasi');

ylabel('posisi (+) akselerasi (-) deselerasi (m)'); legend('Reference','EnKF'); subplot(3,1,2) plot((1:I+1),rek1(2,:),'-r',(1:I+1),Rekam1(2,:),'-b') grid on; title('Estimasi v'); xlabel('iterasi');

ylabel('posisi (+) kiri; (-) kanan (m)'); legend('Reference','EnKF');

(75)

subplot(3,1,3)

plot((1:I+1),rek1(3,:),'-r',(1:I+1),Rekam1(3,:),'-b') grid on;

title('Estimasi w'); xlabel('iterasi');

ylabel('posisi (+) naik; (-) turun (m)'); legend('Reference','EnKF');

PENGEMBANGAN ALGORITMA UNTUK ESTIMASI POSISI PADA SISTEM NAVIGASI DAN TRAYEKTORI WAHANA NIR AWAK BAWAH AIR ITS AUV 01