Archive

Archive for January, 2011

Baca Data Real-Time GPS Dengan Matlab

January 29, 2011 2 comments

%————————————————————————-
% Baca data GPS decara real time @2011 Radiotrack
% Port com no 1, baud 4800 bps, header $GPGGA
%————————————————————————-
s1 = serial(‘com1′,’baudrate’,4800,’InputBufferSize’,10000);
fopen(s1)
e=0;
while e<1;
data=”;
while isempty(strmatch(‘$GPGGA’,data))
data=fscanf(s1);
end
[header,data]=strtok(data,’,’);
[time,data]=strtok(data,’,’);
[Lat,data]=strtok(data,’,’);
[ns,data]=strtok(data,’,’);
[Lon,data]=strtok(data,’,’);
[ew,data]=strtok(data,’,’);
[posfix,data]=strtok(data,’,’);
[sats,data]=strtok(data,’,’);
[hdop,data]=strtok(data,’,’);
[Alt,data]=strtok(data,’,’);
disp([‘Latitude = ‘ Lat ‘ Londitude = ‘ Lon ‘ Altitude = ‘ Alt])
pause(0.001)
end
%————————————————————————-

Categories: imuinsgps

Merubah Parameter Quaternion ke Euler

January 22, 2011 Leave a comment

function eul = quat2euler(quat)
% This will take a input quaternion row vector,
% [q0 q1 q2 q3], and output a row vector of euler
% angles, [phi theta psi]
%
% [phi theta psi] = quat2euler([q0 q1 q2 q3])

%quat = normq(quat);

q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);

q02 = q0*q0;
q12 = q1*q1;
q22 = q2*q2;
q32 = q3*q3;

theta = real(-asin(2*(q1*q3 – q0*q2)));
phi = real(atan2( 2*(q2*q3 + q0*q1),(1-2*(q12 + q22)) ));
psi = real(atan2( 2*(q1*q2 + q0*q3),(1-2*(q22 + q32)) ));

eul = [phi theta psi];

Categories: imuinsgps

Algoritma INS Dengan Quaternion

January 22, 2011 Leave a comment

%————————————————————————–
%       INS Algoritma untuk sensor rate gyroscope (p,q,r)

%       dan sensor accelerometer (ax,ay,az) untuk aplikasi roket
%       menggunakan quaternion @Dr.Wahyu Widada
%————————————————————————–
% data sensor gyroscope [rad/sec]
clear all,close all
deg2rad = pi/180;

rad2deg = 180/pi;
G       = 9.81;
Dt = 1/1000; % sampling data
t  = 0:Dt:1;

% Data Simulasi Sensor rate-gyroscope 3 Axis
p  = 100*deg2rad*sin(2*pi*5*t);
q  = 2000*deg2rad*sin(2*pi*3*t);
r  = 100*deg2rad*sin(2*pi*4*t);

% data Simulasi Sensor Accelerometer 3 Axis
ax = 10*sin(2*pi*5*t);
ay = 0*sin(2*pi*5*t);
az = 0*sin(2*pi*5*t);

% Inisialisasi data
pitch(1) = 0; yaw(1) = 0; roll(1) = 0;
Roll(1)  = 0;
Pitch(1) = 0;
Yaw(1)   = 0;
e0(1) = 1; e1(1) = 0; e2(1) = 0; e3(1) = 0;
e(1)  = e0(1)^2 + e1(1)^2 + e2(1)^2 + e3(1)^2;
% Data sensor accelerometer [m/sec2]
Udot(1) = 0;
Vdot(1) = 0;
Wdot(1) = 0;
U(1) = 0;
V(1) = 0;
W(1) = 0;

% Data prosesing
for n = 1 : length(p)-1
% Rate-GYROSCOPE PROCESSING
% Quaternion rates
e_0(n) = 1/2*(e1(n)*p(n) + e2(n)*q(n) + e3(n)*r(n));
e_1(n) = 1/2*(e0(n)*p(n) + e2(n)*r(n) – e3(n)*q(n));
e_2(n) = 1/2*(e0(n)*q(n) + e3(n)*p(n) – e1(n)*r(n));
e_3(n) = 1/2*(e0(n)*r(n) + e1(n)*q(n) – e2(n)*p(n));
% Integral quaternion and normalization
e0(n+1) = e0(n)+e_0(n)*Dt;
e1(n+1) = e1(n)+e_1(n)*Dt;
e2(n+1) = e2(n)+e_2(n)*Dt;
e3(n+1) = e3(n)+e_3(n)*Dt;
% (p,q,r)–>(Phi,Theta,Psi)/(roll,pitch,yaw)
Eul        = quat2euler([e0(n+1) e1(n+1) e2(n+1) e3(n+1)]);
roll(n+1)  = Eul(1); pitch(n+1) = Eul(2); yaw(n+1)   = Eul(3);
% Normalisasi quaternion
e0(n+1)    = 1; e1(n+1) = 0; e2(n+1) = 0; e3(n+1) = 0;
% Euler Angles
Roll(n+1)  = Roll(n)+roll(n+1);
Pitch(n+1) = Pitch(n)+pitch(n+1);
Yaw(n+1)   = Yaw(n)+yaw(n+1);
%end
% DCM direct cosine matrix, untuk merubah koordinat body ke koordinat bumi
DCM = eulerDCM(Eul);
% ACCELEROMETER PROCESSING
Udot(n+1)=ax(n)+V(n)*r(n)-W(n)*q(n)+G*sin(Roll(n));
Vdot(n+1)=ay(n)-U(n)*r(n)+W(n)*p(n)-G*cos(Pitch(n))*sin(Roll(n));
Wdot(n+1)=az(n)+U(n)*q(n)-V(n)*p(n)-G*sin(Pitch(n))*cos(Roll(n));

U(n+1)=U(n)+Udot(n)*Dt;
V(n+1)=V(n)+Vdot(n)*Dt;
W(n+1)=W(n)+Wdot(n)*Dt;
end
%————————————————————————–
%close all
figure(1)
subplot(3,1,1)
plot(t,cumsum(p)*Dt*180/pi)
hold on
plot(t,Roll*rad2deg,’r’),grid
xlabel(‘Time (sec)’),ylabel(‘Roll (deg)’)
legend(‘body’,’reference’)
hold off
subplot(3,1,2)
plot(t,cumsum(q)*Dt*180/pi)
hold on
plot(t,Pitch*rad2deg,’r’),grid
xlabel(‘Time (sec)’),ylabel(‘Pitch (deg)’)
legend(‘body’,’reference’)
hold off
subplot(3,1,3)
plot(t,cumsum(r)*Dt*180/pi)
hold on
plot(t,Yaw*rad2deg,’r’),grid
xlabel(‘Time (sec)’),ylabel(‘Yaw (deg)’)
legend(‘body’,’reference’)
hold off

figure(2)
subplot(3,1,1)
plot(t,cumsum(cumsum(ax)*Dt)*Dt),grid
hold on
plot(t,cumsum(U)*Dt,’r’)
xlabel(‘Time (sec)’),ylabel(‘Roll (m)’)
legend(‘body’,’reference’)
hold off
subplot(3,1,2)
plot(t,cumsum(ay)*Dt),grid
hold on
plot(t,cumsum(V)*Dt,’r’)
xlabel(‘Time (sec)’),ylabel(‘Pitch (m)’)
legend(‘body’,’reference’)
hold off
subplot(3,1,3)
plot(t,cumsum(az)*Dt),grid
hold on
plot(t,cumsum(W)*Dt,’r’)
xlabel(‘Time (sec)’),ylabel(‘Yaw (m)’)
legend(‘body’,’reference’)
hold off

Categories: imuinsgps

Algoritma INS Dengan Euler

January 21, 2011 Leave a comment

%————————————————————————–
%       INS Algoritma untuk sensor rate gyroscope (p,q,r)
%       menggunakan Euler @2009 w_widada
%————————————————————————–
% data sensor gyroscope [rad/sec]
clear all
deg2rad = pi/180;
rad2deg = 180/pi;
G       = 9.81;
Dt = 1/10000; % sampling data
% Contoh data rate-gyroscope
t  = 0:Dt:1;
p  = 100*deg2rad*sin(2*pi*5*t);
q  = 100*deg2rad*sin(2*pi*3*t);
r  = 100*deg2rad*sin(2*pi*4*t);
% Inisialisasi data
pitch(1) = 0;
yaw(1)   = 0;
roll(1) = 0;
Roll(1)  = 0;
Pitch(1) = 0;
Yaw(1)   = 0;
rolldot(1)  = 0;
pitchdot(1) = 0;
yawdot(1)   = 0;

% Data prosesing
for n = 1 : length(p)-1
rolldot(n+1)=p(n)+q(n)*sin(roll(n))*tan(pitch(n))+r(n)*cos(roll(n))*tan(pitch(n));
pitchdot(n+1)=p(n)*0+q(n)*cos(roll(n))-r(n)*sin(roll(n));
yawdot(n+1)=p(n)*0+q(n)*sin(roll(n))*sec(pitch(n))+r(n)*cos(roll(n))*sec(pitch(n));
roll(n+1)=roll(n)+rolldot(n)*Dt;
pitch(n+1)=pitch(n)+pitchdot(n)*Dt;
yaw(n+1)=yaw(n)+yawdot(n)*Dt;
Roll(n+1)  = Roll(n)+(roll(n+1)-roll(n));
Pitch(n+1) = Pitch(n)+(pitch(n+1)-pitch(n));
Yaw(n+1)   = Yaw(n)+(yaw(n+1)-yaw(n));
end

figure(1)
subplot(3,1,1)
plot(t,cumsum(p)*Dt*180/pi)
hold on
plot(t,Roll*rad2deg,’r’),grid
xlabel(‘Time (sec)’),ylabel(‘Roll (deg)’)
legend(‘body’,’reference’)
hold off
subplot(3,1,2)
plot(t,cumsum(q)*Dt*180/pi)
hold on
plot(t,Pitch*rad2deg,’r’),grid
xlabel(‘Time (sec)’),ylabel(‘Pitch (deg)’)
legend(‘body’,’reference’)
hold off
subplot(3,1,3)
plot(t,cumsum(r)*Dt*180/pi)
hold on
plot(t,Yaw*rad2deg,’r’),grid
xlabel(‘Time (sec)’),ylabel(‘Yaw (deg)’)
legend(‘body’,’reference’)
hold off

Categories: imuinsgps

Kalkulasi Jarak GPS ke Meter

January 21, 2011 Leave a comment

Data dua titik dari sebuah GPS dapat dihitung jarak antara titk terebut. Berikut ini contoh kalkulasi.

clear all
lat1 = 06+22/60+30.12/3600  ; % kantor
lon1 = 106+37/60+43.73/3600;
lat2 = 06+33.6364/60                ; % rumah bogor
lon2 = 106+46.1083/60             ;
%Convert to radians
latrad1 = lat1*pi/180;
lonrad1 = lon1*pi/180;
latrad2 = lat2*pi/180;
lonrad2 = lon2*pi/180;
londif = abs(lonrad2-lonrad1);
raddis = acos(sin(latrad2)*sin(latrad1)+cos(latrad2)*cos(latrad1)*cos(londif));
nautdis = raddis * 3437.74677;
stdism = nautdis * 1.852*1000;
fprintf(‘Distance in meters: = %7.2f \n’, stdism); % Hasil = 25748.75 meter

Categories: imuinsgps

Sensor Accelerometer

January 19, 2011 Leave a comment

Accelerometer

Sensor untuk mengukur percepatan gerak dengan gaya gravitasi. Sensor ini dalam keadaan diam akan menunjukkan nilai 1 g (1 g = 9,8 m/s2) pada posisi tegak lurus melawan gravitasi, dan akan menunjukkan nilai -1 g saat arah sebaliknya. Jika diputar dalam sumbu tersebut maka akan menunjukkan nilai sinusoidal. Jika ditulis dalam persamaan, sudut roll adalah sebagai berikut:

Roll = arcsin(Yaccel)

Sedangkan untuk sudut pitch dapat dihitung dengan persamaan berikut:

Pitch = arcsin(Xaccel)

Tetapi jika dalam keadaan ada perubahan gerak, maka selain nilai gravitasi ini juga akan timbul percepatan gerak. Sayangnya sensor accelerometer tidak bisa untuk mengukur sudut Yawing, sehingga perlu sensor lainnya, sperti digital kompas.

Pada saat bergerak, maka gaya gravitasi harus dieliminasi dengan mengetahui posisi sudutnya untuk masing-masing sumbu, sehingga hanya percepatan saja yang terbaca. Nilai percepatan ini digunakan untuk menghitung kecepatan dan posisi. Bagaimana menghilangkan gaya gravitasi supaya bisa mengukur percepatan gerak…? sebagai contoh untuk sumbu X, percepatan = percepatan dari accelerometer dikurangi g*sin(pitch), lihat persamaan dibawah untuk masing-masing sumbu.

Udot(i) = Xaccel(i) + g×sin(pitch(i));
Vdot(i) = Yaccel(i) – g×cos(pitch(i))×sin(roll(i));
Wdot(i) = Zaccel(i) – g×cos(pitch(i))×cos(roll(i));

Jika nilai percepatan sensor accelerometer sudah dieliminasi pengaruh gaya gravitasi, maka hanya tinggal percepatan gerak. Faktor lain yang perlu dieliminasi adalah gaya sentrifugal. Perhitunganya adalah sbb:

Udot(i) = Xaccel(i) + V(i) * r(i) – W(i) * q(i) + g * sin(pitch(i));
Vdot(i) = Yaccel(i) – U(i) * r(i) + W(i) * p(i) – g * cos(pitch(i)) * sin(roll(i));
Wdot(i) = Zaccel(i) + U(i) * q(i) – V(i) * p(i) – g * cos(pitch(i)) * cos(roll(i));

Nilai percepatan diatas telah siap digunakan untuk deteksi gerak. Sudut roll dan pitch mempengaruhi gaya gravitasi pada accelerometer, sedangkan sudut Yawing tidak terlihat mempengaruhi. Kemudian kecepatan dapat dihitung dengan persamaan berikut.

% Velocity (m/s)
U(i+1)  = U(i) + Udot(i)dt;
V(i+1)  = V(i) + Vdot(i)dt;
W(i+1) = W(i) + Wdot(i)dt;

Kemudian jarak tempuhnya dapat dihutung sbb:

% Distance (m)
Sx(i+1) = Sx(i) + U(i)dt;
Sy(i+1) = Sy(i) + V(i)dt;
Sz(i+1) = Sz(i) + W(i)dt;

Nilai-nilai diatas masih dalam sistem koordinat sumbu body sensor. Sudut roll, pitch, dan yaw harus dihitung dari sensor lain (rate-gyroscope). Persoaal lain yang masih ada adalah adanya noise dan bias sensor yang selalu  timbul pada sensor MEMS accelerometer ini.

Categories: imuinsgps

Estimasi Delay Waktu Dua Sinyal

January 16, 2011 Leave a comment

Menghitung delay antara dua buah sinyal sangat beragam. Delay ini sangat berguna untuk menentukan nilai pada sistem pengukur jarak. Delay antara sinyal yang dikirim dengan radio transmitter dan sinyal yang diterima kembali oleh receiver dapat dihitung dengan menggunakan fungsi cross-correlation. Fungsi tersebut dapat ditulis dengan persamaan berikut.

Posisi titik puncak hasil korelasi silang kedua sinyal tersebut merupakan posisi delay waktunya. Berikut ini adalah contoh dua buah sinyal dengan delay waktu tertentu.

Maka nilai korelasi silanganya sbb:

Perbedaan posisi titik puncak merupakan nilai delay waktu. Contoh diatas disimulasikan dengan MATLAB sbb:

fs = 1000                              ; % sampling data
t = 0 : 1/fs : 1                      ; % waktu
y1= sin(2*pi*2*t)             ; % sinyal awal
y2= sin(2*pi*2*t+pi/2) ; % sinyal dengan delay pi/2
figure(1) % Plot sinyal
plot(t,y1,t,y2,’LineWidth’,2.5),grid
xlabel(‘Time (sec)’,’fontsize’,18)
ylabel(‘Amplitude (Volt)’,’fontsize’,18)
legend(‘s1′,’s2’)
figure(2) % Plot cross-correlation
plot((0:2000)/fs-1,[xcorr(y1,y1)’ xcorr(y1,y2)’],’LineWidth’,2.5),grid
xlabel(‘Time (sec)’,’fontsize’,18)
ylabel(‘Correlation’,’fontsize’,18)
axis([-1,1,-500,500])

Dari penjelasan diatas, maka jika sinyal yang dikirim dan diterima kembali oleh radio transceiver dapat kita akuisisi, maka delay waktu dan jarak dapat kita estimasi.

Categories: Radio Tracking