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

%% =========== Part 1: Loading and Distribute Data =============
%  We start the programming by first loading and distribute the dataset. 
%  The Data consists of the outputs of the gyros and accelerometers.

%  Load Data fromJetB.mat
fprintf('Loading Data ...\n')
Data = load('JetB.mat');
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
Radius = 6371000;            % Radius of Earth  (average)
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_Head = zeros(KK+1,1);  % head of spaceship 
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_Head(1) = -80;
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)];
    Wig = [-V_ENT(N,2)/(Radius+Height(N)) V_ENT(N,1)/(Radius+Height(N)) ...
           V_ENT(N,1)/(Radius+Height(N))*tan(Latitude(N)/180*pi)] + Wie;
    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
    Latitude(N+1) = (V_ENT(N+1,2)+V_ENT(N,2))/(2*(Radius+Height(N)))*...
                     Time_AC/pi*180+Latitude(N);
    Longitude(N+1) = (V_ENT(N+1,1)+V_ENT(N,1))/(2*(Radius+Height(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);
    Angle_Head(N+1) = rad2deg(flag(1));
    Angle_Pitch(N+1) = rad2deg(flag(2));
    Angle_Roll(N+1) = rad2deg(flag(3));
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;
plot(T,Angle_Head,'-.r');  % head-time curve
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 =================
HPRatt = [Angle_Head Angle_Pitch Angle_Roll];
LonM = Longitude;
LatM = Latitude;
HtM = Height;
save FlightData HPRatt LonM LatM HtM
main;

This snippet took 0.01 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).