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

Transkripsi

1 Sidang Tugas Akhir TM PENGEMBANGAN ALGORITMA UNTUK ESTIMASI POSISI PADA SISTEM NAVIGASI DAN TRAYEKTORI WAHANA NIR AWAK BAWAH AIR ITS AUV 01 Oleh : Gustiyadi Fathur R. ( ) Dosen Pembimbing: Hendro Nurhadi, Dipl.-Ing., Ph.D ( ) Subchan, Ssi., MSc., PhD ( )

2 Contents BAB 1 BAB 2 BAB 3 BAB 4 BAB 5 PENDAHULUAN TINJAUAN PUSTAKA METODOLOGI ANALISA DAN PEMBAHASAN KESIMPULAN DAN SARAN DAFTAR PUSTAKA

3 BAB 1 PENDAHULUAN RUMUSAN DAN BATASAN MASALAH

4 LATAR BELAKANG Unmanned Underwater Vehicle AUV ROV Aplikasi di beberapa sektor bidang (sains, lingkungan, militer dll) Gangguan internal dan eksternal Metode Ensemble Kalman Filter (EnKF) Perancangan algoritma estimasi posisi pada sistem navigasi & trayektori

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 Tinjauan Pustaka Pengembangan Algoritma

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 Algoritma Rigid Body Kinematika AUV Hydrodynamics

10 Desain ITS AUV Gambar 1. ITS AUV 01

11 Kinematika AUV Posisi dan Sudut Euler 1 T T [, 2 ] T 1 [ x, y, z] T T 2 [,, ] Kecepatan linier dan Anguler T T [ v, v T 1 2 ] 1 [ u, v, w] T 2 [ p, q, r] T Gaya dan Momen 1 T T [, 2 ] T 1 [ X, Y, Z] T 1 [ K, M, N] T

12 Koordinat Rotasi pada EFF C x, cos sin Kecepatan Linier 0 sin cos C y, cos 0 sin J 0 sin cos C C C C z T T T 1 ( 2 ) z, y, x, cos sin, sin cos J 1 ( 2 ) cos cos sin cos sin sin cos cos sin sin sin sin sin cos cos cos sin sin sin cos sin cos cos sin sin sin cos cos cos X 3 X 2 ψ X 1 Y 1 Y 0 ϕ θ X 2 Y 3 Y 2 ψ ϕ Z 1 Z 0 θ Z 2 Z 1

13 Rigid Body Equation AUV m m m I x 1. Surge 2 2 u vr wq x ( q r ) y ( pq r ) z ( pr q ) X 2. Sway 3. Heave 4. Roll G 2 2 v wp ur y ( r pr ) z ( qr p ) x ( qp r ) Y G 2 2 w uq vp z ( p qp ) x ( rp q ) y ( rq p ) Z p ( I z I y G ) qr ( r pq) I xz Persamaan gerak 6 DOF (SNAME 1950) ( r 2 G G G q 2 ) I yz G G G ( pr q ) I xy m y ( w uq vp) z ( v wp ur) K G G 5. Pitch I y q ( I x I z ) rp ( p qr) I xy ( p 2 r 2 ) I zx ( qp r ) I yz m z ( u vr wq) x ( w uq vp) M G G I z 6. Yaw r ( I I ) pq ( q rp) I y x yz ( q 2 p 2 ) I xy ( rq p ) I zx m x ( v wp ur) y ( u vr wq) N G G

14 Hidrodinamika AUV Gaya dan Momen Luar () hidrostatis added mass drag lift thrust

15 Gravitasi B B G [0 0 W] G J l 1 1 ( 2 ) G T C G C B Bouyancy W W B [ 0 0 B] l 1 1 ( 2 ) B J B T B B B c B C B hidrostatis C G C G W W W Posisi tidak stabil sehingga timbul momen agar stabil

16 T added mass 2 Bidang Sama

17 added mass

18 x x y u z δ s β e u δ r β e V fluida v W fin V fluida re r re se s se drag

19 A R wingspan planform l A 2 s C L A 2 R l A S C. L C L L C L Av e 2 lift

20 thrust D ω Gaya dan Momen Thruster p p T t p p T t J K D J K D T ) ( 2 1 ) ( D V J P A V A )u 1 ( n in i ti ti x n i i x xt r M T F dimana Persamaan Umum

21 Total Gaya dan Momen Translasi sepanjang arah x : Translasi sepanjang arah y : Translasi sepanjang arah z :

22 Rotasi sepanjang arah x : Rotasi sepanjang arah y : Rotasi sepanjang arah z :

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) Tahap koreksi (measurement update step).

24 Algoritma Ensemble Kalman Filter (EnKF) Inisialisasi Tentukan nilai awal Tahap Prediksi Estimasi Kovarian Error Tahap Koreksi Kalman Gain Estimasi Kovarian Error ] [, 0 0,3 0,2 0,1, 0 N i x x x x x N i i x N x 1 0 ˆ0, 1 ˆ i k k k k w u x f x, 1 1 ), ( ˆ ˆ T k i k N i k i k k x x x x N P ) ˆ )( ˆ ˆ ( ˆ 1 1, 1, N i i k x k N x 1, ˆ 1 ˆ i k k i k v z z,, 1 ) ( k T k T k k R H HP H P K k k k P H K I P ] [ ) ˆ ( ˆ,,,, i k i k k i k i k x H z K x x

25 BAB 3 METODOLOGI Penelitian Program

26 FLOWCHART PENELITIAN START Kajian Pustaka Spesifikasi Dimensi dan Dinamika Gerak AUV Menentukan Desain AUV dan Trayektori Mendapatkan Persamaan Gerak AUV pada 6 DOF RMS Error 0 ~ 1? T Y Menganalisa Hasil Simulasi Menarik Kesimpulan dari Hasil Simulasi END

27 FLOWCHART PROGRAM

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 Translasi sepanjang arah x :

31 Translasi sepanjang arah y : Translasi sepanjang arah z :

32 Penambahan Faktor Stokastik Model Stokastik : fungsi nonlinear dimana w k v k H ialah noise sistem Q k (program) ialah noise pengukuran R k (program) ialah matriks data sampel/pengukuran

33 Implementasi Model ITS AUV pada Algoritma EnKF Mendefinisikan x pada fungsi x k+1 : Model Sistem penentuan nilai awal untuk masing-masing posisi (nol)

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 : Menghitung nilai mean dari setiap state :

36 Menghitung nilai error ensemble awal :

37 Tahap Prediksi Menghitung nilai prediksi awal + noise-nya : Menghitung nilai mean ensemble :

38 Menghitung nilai ensemble error :

39 Tahap Koreksi Menghitung data pengukuran : Menghitung Kalman Gain : Perhitungan estimasi koreksi : Perhitungan nilai mean estimasi koreksi : Perhitungan kovariansi error :

40 Simulasi dan Analisa 100 Ensemble (Simulasi ke-1)

41

42 200 Ensemble (Simulasi ke-3)

43

44 300 Ensemble (Simulasi ke-6)

45

46 400 Ensemble (Simulasi ke-8)

47

48 Semakin besar jumlah ensemble semakin kecil nilai RMSE RMSE Gabungan

49 5.1 Kesimpulan 5.2 Saran BAB 5

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, b. Ensemble 200 simulasi 3 RMSE surging = 0,01336; swaying = 0,09814; dan heaving = 0, c. Ensemble 300 simulasi 6 RMSE surging = 0,01342; swaying = 0,09449; dan heaving = 0, d. Ensemble 400 simulasi 8 RMSE surging = 0,013137; swaying = 0,098465; dan heaving = 0, 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 Sistem Permesinan Kapal Selam Evensen, G The ensemble Kalman filter: theoretical formulation and practical implementation, Ocean Dynamics, 53: Ichwan, A Estimasi posisi Kapal Selam Menggunakan metode Extended Kalman Filter. Tugas Akhir, Institut Teknologi Sepuluh Nopember, Surabaya Fitria, Risa Implementasi Ensemble Kalman Filter pada Estimasi Kecepatan Kapal Selam. Tugas Akhir, Institut Teknologi Sepuluh Nopember, Surabaya Lewis, F.L Optimal Estimation with an Introduction to Stochastic Control Theory, John Wiley & Sons., New York Purnomo, Kosala Dwidja Aplikasi Metode Ensemble Kalman Filter pada Model Populasi Plankton. Institut Teknologi Sepuluh Nopember: Surabaya Jeffi, Trio Pengembangan Sistem Kendali Robust AUV dengan Metode Sliding-PID. Institut Teknologi Sepuluh Nopember: Surabaya Walchko, K. J., Novick, David, and Nechyba, M. C Development of a Sliding Mode Control Sistem with Extended Kalman Filter Estimation for Subjugator, University of Florida Gainesville, FL, Nahon, Meyer A Simplified Dynamics Model for Autonomous Underwater Vehicle. University of Victoria: Canada

53 Jwo, Dah-Jing dan Ta-Shun Cho Critical remarks on the linearised and extended Kalman filters 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 Sistem Pengendali Gerak Pada Kapal Selam Menggunakan Metode Sliding Mode Control. Tugas Akhir. Institut Teknologi Sepuluh Nopember, Surabaya Yang, Chen 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 An Overview of Marine Systems Simulator (MSS): A Simulink Toolbox for Narine Control System. SIMS2005-Scandinavian Conference on Simulation and Modelling. T. I., Fossen A Nonlinear Unified State-space Model for Ship Maneuvering and Control in A Seaway. Journal of Bifurcation and Chaos. Louis Andrew Gonzalez 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 Dynamic Data Assimilation: A Least Squares Approach. University Press, Cambridge. Setiawan, Agus Rancang Bangun Prototype Kapal Selam Tanpa Awak (ITS AUV-01) dengan Aplikasi IMU, Kompas, dan GPS Module.Institut Teknologi Sepuluh Nopember: Surabaya

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= ; zg=0.05; xb=0.062; yb=0; zb= 0; volume= ; %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= / ; %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= ; %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; xcp=0.75; %momen arm wrt the origin~body (0.5*l)

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

58 Sn4= ; Sm4=1; St4=0; Sn5= ; Sm5=1; St5=2.7691e+33; Sn6= ; Sm6=1; St6= e+37; Sn7= ; Sm7=1; St7= e+39; u0=0; v0=0; w0=0; uawal=2; %(m/s = uawal*2 knot) [u dlm persamaan Va] I=1000; %jumlah iterasi N=400; %jumlah ensemble yang dibangkitkan (100, 200, 300 & 400)

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= *t; clalpha=1/((1/(2*pi*0.3))+(1/(pi*0.5))); for i=1:i/4 %sudut pitch di titik pitch1(1,i)=(-2)*pi/180; end for i=1:i/4 %sudut pitch di titik pitch2(1,i)=(0)*pi/180; end for i=1:i/4 %sudut pitch di titik pitch3(1,i)=(0)*pi/180; end

60 for i=1:i/4 %sudut pitch di titik 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 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');