% File- pos_est1.m

% The resultant matrix has 11 columns. These are:
% Yaw (deg), X-rate (V), Pitch (deg), Y-rate (V), Roll (deg), Z-rate (V),
% Compass_msec (msecs), X-Accel (V), Compass_week (weeks), Y-Accel (V), 
% Z-Accel (V), IMU_msec (msecs), IMU_week (weeks)

load test0.dat ;

Sample_rate = 64 ; % Sample rate in Hz
POINTS = 602 ; % Total number of sample points 

data = test0' ;


plot(cos(pi*data(:,1)/180.0), sin(pi*data(:,1)/180.0)) ;

% Matrix, "time" gives the time ;plot(time(:,1), compass(:,1), time(:,1), imu(:,1)) ; 

time = zeros(POINTS,1) ;

for j = 2:POINTS
	time(j,1) = time(j-1,1) + 1/Sample_rate;

end ;

% Matrix, compass stores compass output- roll , pitch, yaw ;
compass = zeros(POINTS,3) ;
compass(:,1) = data(:,4) ;  % Roll
compass(:,2) = data(:,2) ;  % Pitch
compass(:,3) = data(:,1) ;  % Yaw

imu_rate = zeros(POINTS,3) ;
imu_rate1 = zeros(POINTS,3) ;
% Lets average out the IMU noise here

for j = 1: 3
	imu_rate1(j,1) = data(j,5) ;
	imu_rate1(j,2) = data(j,3)  ;
	imu_rate1(j,3) = data(j,7) ;
end;

for j = 4: POINTS
	imu_rate1(j,1) = (data(j-3,5) + data(j-2,5) + data(j-1,5) + data(j,5))/4.0 ;
	imu_rate1(j,2) = (data(j-3,3) + data(j-2,3) + data(j-1,3) + data(j,3))/4.0 ;
	imu_rate1(j,3) = (data(j-3,7) + data(j-2,7) + data(j-1,7) + data(j,7))/4.0 ;
end;

% Matrix, imu_rate stores imu rates- roll, pitch, yaw ;

% imu_rate = zeros(POINTS,3) ;

%imu_rate(:,1) = - data(:,5)*20.0509 - 0.1925; ;  % Roll deg/sec
%imu_rate(:,2) = data(:,3)*19.864 - 0.1490; ;  % Pitch deg/sec
%imu_rate(:,3) = data(:,7)*19.980 - 0.0340 ;  % Yaw  deg/sec

imu_rate(:,1) = imu_rate1(:,1)*20.0509 - 0.074; ;  % Roll deg/sec
imu_rate(:,2) = imu_rate1(:,2)*19.864 + 0.0466; ;  % Pitch deg/sec
imu_rate(:,3) = imu_rate1(:,3)*19.980 + 0.0522;  % Yaw  deg/sec

% matrix, imu stores the angle calculated using IMU angles ;

imu = zeros(POINTS,3) ;

% Here we integrate to caluclate imu ;
imu(1,:) = compass(1,:) ;

temp_rate = zeros(3,1) ;

for j = 2: POINTS	
	AX = pi*(imu(j-1,1))/180.0 ;
	AY = pi*(imu(j-1,2))/180.0 ;
	AZ = pi*(imu(j-1,3))/180.0 ;

	C(1,1) = cos(AY) ;
	C(1,2) = 0.0 ;
	C(1,3) = sin(AY) ;
	C(2,1) = tan(AX)*sin(AY) ;
	C(2,2) = 1.0 ;
	C(2,3) = - tan(AX)*cos(AY) ;
	C(3,1) = - sin(AY)/cos(AX) ;
	C(3,2) = 0.0 ;
	C(3,3) = cos(AY)/cos(AX) ;

	temp_rate = C * (imu_rate(j-1,:))' ;

        imu(j,:) = imu(j-1,:) + temp_rate'*(time(j,1) - time(j-1,1)) ;
        if (imu(j,1) >= 360)
		imu(j,1) = imu(j,1) - 360.0 ;
	end;
	if (imu(j,1) <= - 360)
		imu(j,1) = imu(j,1) + 360.0 ;
	end;
       if (imu(j,2) >= 360)
		imu(j,2) = imu(j,2) - 360.0 ;
	end;
	if (imu(j,2) <= - 360)
		imu(j,2) = imu(j,2) + 360.0 ;
	end;
	if (imu(j,3) >= 360)
		imu(j,3) = imu(j,3) - 360.0 ;
	end;
	if (imu(j,3) <= - 360)
		imu(j,3) = imu(j,3) + 360.0 ;
	end;
end;

plot(time(:,1), compass(:,3), time(:,1), imu(:,3)) ;
