# Demo entry 6657799

MATLAB

Submitted by anonymous on Nov 04, 2017 at 18:35
Language: Matlab. Code size: 7.1 kB.

```%% Inertial Navigation - Assignment 3

%  Instructions
%  ------------
%
%  This file is designed to solve the problem in the Assignment 3,
%  and with the data from JetB.mat.
%
%  Two files are used in this file:
%        JetB.mat
%        q2eul.m
%

%% Initialization
clear ; close all; clc

%  We start the programming by first loading and distribute the dataset.
%  The Data consists of the outputs of the gyros and accelerometers.

Data_GY = Data.GMM;
Rows_Data_GY = size(Data_GY,1);
Data_AC = Data.AMM;
Rows_Data_AC = size(Data_AC,1);

%% ======== Part 2: Setup and initilize the parameters =========
%  In this part, we prepare the parameters of Earth and fighter jet
%  and prepare storage blocks. Also, the times of the loop body.

%  Setup the execution time of the loop body
Time_GY = 0.01;                   % Gyro output peroid
Time_AC = 0.2;                    % Accelerometer output period
NavTime = Rows_Data_GY * Time_GY; % Flight time of the jet
%  or NavTime = Rows_Data_AC * Time_AC
KK = NavTime/Time_AC;        % Velocity and Position Iteration Times
kk = Time_AC/Time_GY;        % Attitude Iteration Times of one KK
T = 0:0.2:NavTime;              % Time scale for plot relative figures

%  Setup the parameters of jet
g = 9.780327;                % Acceleration of gravity   (average)
We = 7.292e-5;               % Spinning rate of Earth

%  Setup the initial conditions of fighter jet
Latitude = zeros(KK+1,1);    % Latitude of spaceship
Longitude = zeros(KK+1,1);   % Longitude of spaceship
Height = zeros(KK+1,1);      % Height of spaceship
V_ENT = zeros(KK+1,3);       % Velocity Coordinated in Geographical Frame
Angle_Pitch = zeros(KK+1,1); % pitch of spaceship
Angle_Roll = zeros(KK+1,1);  % roll of spaceship

Latitude(1) = 39.97;
Longitude(1) = 119.75;
Height(1) = 150;
V_ENT(1,:) = [0;0;0];
Angle_Pitch(1) = 0;
Angle_Roll(1) = 0;
Q0 = [cos((-80/2)/180*pi) 0 0 sin((-80/2)/180*pi)];
Q1 = Q0;
Q = zeros(KK+1,4);
Q(1,:)=Q1;
Last_ArG=[0 0 0];

fprintf('Program paused. Check Data or Press enter to continue.\n');
pause;

%% ============ Part 3: Execute the two iteration =============
%  The velocity/position computation loop starts with the embedded jet
%  attitude computation loop. During each sampling period of the gyro,
%  utilizes their current angular increment outputs to update attitude.
%  Each velocity/position period, the attitude will be updated kk times.

%  Velocity and Position Iteration
fprintf('Executing Iteratoins ...\n')
for N = 1:KK

%  Attitude Iteration
for n = 1:kk
% DeltaTheta_GY: Angular Increment of Gyros Rotating x-y-z Axis
DeltaTheta_GY = Data_GY((N-1)*kk+n,:) * 0.01/3600/180*pi;
% DeltaTheta_GYnorm: Mod of Angular Increment
DeltaTheta_GY_norm = norm(DeltaTheta_GY);
% Qbb: 4 order implementation
q_bb = [(1-DeltaTheta_GY_norm^2/8+DeltaTheta_GY_norm^4/384)...
(0.5-DeltaTheta_GY_norm^2/48)*DeltaTheta_GY];
% Quaternion from Geographical Frame n to Body Frame n
Q1 = quatmultiply(Q1,q_bb);
end

%  Wig: Angular rate of the rotation of the geographical frame
Wie = [0 We*cos(Latitude(N)/180*pi) We*sin(Latitude(N)/180*pi)];
unit_vector = Wig/norm(Wig);
%  DeltaTheta_Gf: Angular increment of geographical frame
DeltaTheta_Gf = Wig * Time_AC;
DeltaTheta_Gf_norm = norm(DeltaTheta_Gf);

%  Current attitude of the jet
q_g = [cos(DeltaTheta_Gf_norm/2) sin(DeltaTheta_Gf_norm/2)*unit_vector];
Q1 =quatmultiply(quatinv(q_g),Q1);
Q(N+1,:) = Q1;

% Data From Accelerator
fb = Data_AC(N,:)*1e-7*g;
fg = quatmultiply(quatmultiply(Q1,[0 fb]),quatinv(Q1));

%  Gravity acceleration in the influence of Height and Latitude
Gh = g*(1+0.0053024*(sin(Latitude(N)/180*pi))^2-0.0000058*...
(sin(2*Latitude(N)/180*pi))^2)-(3.086e-06*Height(N));
%  Relative accleration
ArG = (fg(2:4)) - cross((Wie+Wig),V_ENT(N,:))-[0 0 Gh];
%  Integration for velocity
V_ENT(N+1,:) = ((ArG+Last_ArG)/2*Time_AC) + V_ENT(N,:);
Last_ArG = ArG;

% Position of spaceship
Time_AC/pi*180+Latitude(N);
cos(Latitude(N)/180*pi))*Time_AC/pi*180+Longitude(N);
Height(N+1) = (V_ENT(N+1,3)+V_ENT(N,3))/2*Time_AC+Height(N);

% the attitude angles of the spaceship
flag=q2eul(Q1);
end

%% ================ Part 4: Outut the results =================
%  Output the final attitude quaternion, longitude, latitude, height,
%  and east, north, vertical velocities of the jet.
%

fprintf('>> The final attitude Quaternion of the jet:\n')
fprintf('>> Q = [%d; %d; %d; %d]\n',Q1(1),Q1(2),Q1(3),Q1(4));
fprintf('>> Longitude, Latitude, Height of the spaceship:\n');
fprintf('>> Longitude = %f, Latitude = %f, Height = %f\n',Longitude(N+1),Latitude(N+1),Height(N+1));
fprintf('>> Velocities of the spaceship:\n');
fprintf('>> [VE VN VA] = [%f %f %f]\n',V_ENT(N+1,:));

fprintf('Program paused. Press enter to plot figures.\n');
pause;

%% ================ Part 5: Plot the figures ==================
%  1. Draw the latitude-versus-longitude trajectory of the jet, with
%     horizontal longitude axis.
%  2. Draw the curve of the height of the jet, with horizontal time axis.
%  3. Draw the curves of the attitude angles of the jet, with horizontal
%     time axis.

% Draw 1
figure;
plot(Longitude,Latitude,'b');
grid on
xlabel('longitude/degree');
ylabel('latitude/degree');
title('Latitude-versus-longitude Trajectory');

% Draw 2
figure;
plot(T,Height,'b');
grid on
xlabel('Time/second');
ylabel('Height/meter');
title('the Height of the Spaceship');

% Draw 3
figure;
hold on
plot(T,Angle_Pitch,'-b'); % pitch-time curve
hold on
plot(T,Angle_Roll,'--m'); % roll-time curve
grid on
xlabel('Time/second');
ylabel('attitude angle/dec');
title('the Spaceship');

figure;
plot3(Longitude,Latitude,Height);

fprintf('Program paused. Press enter to execute FlightDemo.\n');
pause;

%% ================ Part 6: Execute FlightDemo =================