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 AkhirTM091486
Contents
BAB 1 • PENDAHULUAN
BAB 2 • TINJAUAN PUSTAKA
BAB 3 • METODOLOGI
BAB 4 • ANALISA DAN PEMBAHASAN
BAB 5 • KESIMPULAN DAN SARAN
• DAFTAR PUSTAKA
BAB 1
PENDAHULUAN
RUMUSAN DAN
BATASAN
MASALAH
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
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
TUJUAN DAN MANFAAT
• TUJUAN :
Mendapatkan hasil estimasi posisi AUV dengan trayektoriyang telah ditentukan dan menggunakan metode EnsembleKalman Filter (EnKF).
• MANFAAT :
• Memberikan gambaran bagaimana EnKF mengestimasiposisi AUV sesuai trayektori.
• Sebagai penunjang penelitian khusus pada bidang robotika.
• Dapat diterapkan dalam teknologi yang menunjangpertahanan dan keamanan NKRI.
• Sebagai tambahan kepustakaan untuk penelitianselanjutnya.
BAB 2
Hasil Studi Peneliti Terdahulu
Dasar Teori
Pengembangan Algoritma
Tinjauan
Pustaka
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].
DASAR TEORI
Kinematika
AUV
Algoritma
Rigid Body
Hydro-
dynamics
Desain ITS AUV
Gambar 1. ITS AUV 01
Kinematika AUV
Posisi dan Sudut Euler
Gaya dan Momen
TTT ],[ 21 Tzyx ],,[1 T],,[2
Kecepatan linier dan AngulerTTT vv ],[ 21 Twvu ],,[1 Trqp ],,[2
TTT ],[ 21 TZYX ],,[1 TNMK ],,[1
cossin0
sincos0
001
,xC
cos0sin
010
sin0cos
,yC
100
0cossin
0sincos
,
zC
Koordinat Rotasi pada EFF
T
x
T
y
T
z CCCJ ,,,21 )(
coscossincossin
cossinsinsincoscoscossinsinsincossin
cossincossinsinsinsincoscossincoscos
)( 21J
Kecepatan Linier
θ
θ
X1
X2
Z2 Z1
ψ
ψ
Y2
X3
Y3
X2
Z1Z0
Y1
Y0
ϕ
ϕ
Rigid Body Equation AUV
1. Surge
2. Sway
3. Heave
4. Roll
5. Pitch
6. Yaw
Xqprzrpqyrqxwqvrum GGG )()()( 22
Yrqpxpqrzprryurwpvm GGG )()()( 22
Zprqyqrpxqppzvpuqwm GGG )()()( 22
KurwpvzvpuqwymIqprIqrIpqrqrIIpI GGxyyzxzyzx )()()()()()( 22
MvpuqwxwqvruzmIrqpIrpIqrprpIIqI GGyzzxxyzxy )()()()()()( 22
NwqvruyurwpvxmIprqIpqIrpqpqIIrI GGzxxyyzxyz )()()()()()( 22
Persamaan gerak 6 DOF (SNAME 1950)
Hidrodinamika AUV
Gaya dan Momen Luar ()
hidrostatis lift added mass drag thrust
hidrostatis
T
G W ]00[
G
l
G J )( 2
1
1
T
B B]00[
B
l
B J )( 2
1
1
Gravitasi
Bouyancy
CG
B
cB
CG
B
W
CB
W
B
W
CB
CG
B
W
B
W
•Posisi tidak stabil sehingga timbul momen agar stabil
T added mass
2
Bidang
Sama
added mass
drag
x
y
δr
βe
u
v
V fluida
u
W fin
x
zδs
βe
V fluida
rerre sesse
lift
l
AS
s
RA
l
planform
wingspanA
2
RL A
C
2
eL AvCL2
LL CC .
thrust
D
ωGaya dan Momen Thruster
ppTt
ppTt
JKD
JKDT
)(2
1
)(2
1
5
4
D
VJ
P
A
uVA )1(
n
in
ititix
n
i
ix
xTrM
TF
1
13
1
13
dimana
Persamaan Umum
Total Gaya dan Momen
• Translasi sepanjang arah x :
• Translasi sepanjang arah y :
• Translasi sepanjang arah z :
• Rotasi sepanjang arah x :
• Rotasi sepanjang arah y :
• Rotasi sepanjang arah z :
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).
Algoritma Ensemble Kalman Filter (EnKF)
Inisialisasi
Tentukan nilai awal
Tahap Prediksi
Estimasi
Kovarian Error
Tahap Koreksi
Kalman Gain
Estimasi
Kovarian Error
][ ,03,02,01,0,0 Ni xxxxx
N
i
ixN
x1
,00ˆ
1ˆ
ikkkk wuxfx ,11 ),ˆ(ˆ
T
kik
N
i
kikk xxxxN
P )ˆˆ()ˆˆ(1
1,
1
,
N
i
ikk xN
x1
,ˆ
1ˆ
ikkik vzz ,,
1)( k
T
k
T
kk RHHPHPK
kkk PHKIP ][
)ˆ(ˆ,,,,
ikikkikik xHzKxx
BAB 3
METODOLOGI
Penelitian
Program
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
END
FLOWCHART PROGRAM
BAB 4
ANALISA & PEMBAHASAN
4.1DiskritisasiModel AUV
4.2Penambahan
FaktorStokastik
4.3Implementasi
Model ITS AUV padaAlgoritma
EnKF
4.4Simulasi dan
Analisa
Diskritisasi Model AUV
Metode Pendiskritan Beda Hingga Maju
Translasi sepanjang arah x :
Translasi sepanjang arah y :
Translasi sepanjang arah z :
Penambahan Faktor Stokastik
Model Stokastik :
fungsi nonlinear
dimana
wk ialah noise sistem Qk (program)
vk ialah noise pengukuran Rk (program)
H ialah matriks data sampel/pengukuran
Implementasi Model ITS AUV pada Algoritma EnKF
Mendefinisikan x pada fungsi xk+1 :
penentuan nilai awal untuk masing-masing posisi (nol)
• Model Sistem
• 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
• Inisialisasi
Pembangkitan sejumlah ensemble awal + noise-nya :
Menghitung nilai mean dari setiap state :
Menghitung nilai error ensemble awal :
• Tahap Prediksi
Menghitung nilai prediksi awal + noise-nya :
Menghitung nilai mean ensemble :
Menghitung nilai ensemble error :
• Tahap Koreksi
Menghitung data pengukuran :
Menghitung Kalman Gain :
Perhitungan estimasi koreksi :
Perhitungan nilai mean estimasi koreksi :
Perhitungan kovariansi error :
Simulasi dan Analisa• 100 Ensemble (Simulasi ke-1)
• 200 Ensemble (Simulasi ke-3)
• 300 Ensemble (Simulasi ke-6)
• 400 Ensemble (Simulasi ke-8)
• RMSE Gabungan
“Semakin besar jumlah ensemble semakin kecil nilai RMSE”
BAB 5
5.1Kesimpulan
5.2Saran
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.
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.
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 TeknologiSepuluh 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
• Jwo, Dah-Jing dan Ta-Shun Cho.2010. 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. 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
IMU, Kompas, dan GPS Module.Institut Teknologi Sepuluh Nopember: Surabaya
www.themegallery.com
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)
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;
xcp=0.75; %momen arm wrt the origin~body (0.5*l)
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;
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
N=400; %jumlah ensemble yang dibangkitkan (100, 200, 300 & 400)
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;
end
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
% 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;
%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;
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];
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
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);
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);
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);
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
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
% 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);
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
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');
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');
% 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');
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');