Merge pull request utat-ss#1 from max01110/EKG

EKF

Author: khangaerospace

EKF/Dynamics.slx

@@ -0,0 +1,16 @@
+function q = Eu2Quat(phi_theta_psi)
+%%%Input is a Nx3 vector and output is a Nx4 vector
+
+phi = phi_theta_psi(1);
+theta = phi_theta_psi(2);
+psi = phi_theta_psi(3);
+
+q0 = cos(phi/2).*cos(theta/2).*cos(psi/2) + sin(phi/2).*sin(theta/2).*sin(psi/2);
+q1 = sin(phi/2).*cos(theta/2).*cos(psi/2) - cos(phi/2).*sin(theta/2).*sin(psi/2);
+q2= cos(phi/2).*sin(theta/2).*cos(psi/2) + sin(phi/2).*cos(theta/2).*sin(psi/2);
+q3 = cos(phi/2).*cos(theta/2).*sin(psi/2) - sin(phi/2).*sin(theta/2).*cos(psi/2);
+
+q = [q0,q1,q2,q3]';
+% Copyright - Carlos Montalvo 2015
+% You may freely distribute this file but please keep my name in here
+% as the original owner

EKF/Dynamics.slx.r2021a

@@ -0,0 +1,23 @@
+function ptp = Quat2Eu(q0123)
+%%ptp = quat2euler(q0123)
+%input is a Nx4 vector with quaternions.
+%output is a Nx3 vector of 3-2-1 euler angles
+
+s = size(q0123);
+if s(1) == 4 && s(2) == 1
+    q0123=q0123';
+end
+q0 = q0123(:,1);
+q1 = q0123(:,2);
+q2 = q0123(:,3);
+q3 = q0123(:,4);
+
+ptp(:,1) = (atan2(2.*(q0.*q1 + q2.*q3),1-2.*(q1.^2 + q2.^2))); %phi
+ptp(:,2) = asin(2.*(q0.*q2-q3.*q1)); %theta
+ptp(:,3) = atan2(2.*(q0.*q3 + q1.*q2),1-2.*(q2.^2 + q3.^2)); %psi
+
+ptp = real(ptp);
+
+% Copyright - Carlos Montalvo 2015
+% You may freely distribute this file but please keep my name in here
+% as the original owner

EKF/Dynamics.slxc

@@ -0,0 +1,29 @@
+function dstatedt = Satellite(t,state)
+%%genericState = [q0123;p;q;r];
+global m invI I
+
+%%select states
+q0123 = state(1:4);
+p = state(5);
+q = state(6);
+r = state(7);
+pqr = state(5:7);
+
+%%%Rotational Kinematics
+%Derivative of Quaternions
+PQRMAT = [0 -p -q -r;p 0 r -q;q -r 0 p;r q -p 0];
+q0123dot = 0.5*PQRMAT*q0123;
+
+%%%Rotational Dynamics
+%%%Total External Disturbance Moments
+LMN = [0;0;0];
+H = I*pqr;
+pqrdot = invI*(LMN - cross(pqr,H));
+
+%%%Return Derivatives State
+dstatedt = [q0123dot;pqrdot];
+
+%%%Discretization (check this)
+%dt = 1; 
+%dstatedt_dis = state + dstatedt*dt;
+end

EKF/Eu2Quat.m

@@ -0,0 +1,111 @@
+function [xx,yy,zz] = earth_sphere(varargin)
+%EARTH_SPHERE Generate an earth-sized sphere.
+%   [X,Y,Z] = EARTH_SPHERE(N) generates three (N+1)-by-(N+1)
+%   matrices so that SURFACE(X,Y,Z) produces a sphere equal to 
+%   the radius of the earth in kilometers. The continents will be
+%   displayed.
+%
+%   [X,Y,Z] = EARTH_SPHERE uses N = 50.
+%
+%   EARTH_SPHERE(N) and just EARTH_SPHERE graph the earth as a 
+%   SURFACE and do not return anything.
+%
+%   EARTH_SPHERE(N,'mile') graphs the earth with miles as the unit rather
+%   than kilometers. Other valid inputs are 'ft' 'm' 'nm' 'miles' and 'AU'
+%   for feet, meters, nautical miles, miles, and astronomical units
+%   respectively.
+%
+%   EARTH_SPHERE(AX,...) plots into AX instead of GCA.
+% 
+%  Examples: 
+%    earth_sphere('nm') produces an earth-sized sphere in nautical miles
+%
+%    earth_sphere(10,'AU') produces 10 point mesh of the Earth in
+%    astronomical units
+%
+%    h1 = gca;
+%    earth_sphere(h1,'mile')
+%    hold on
+%    plot3(x,y,z)
+%      produces the Earth in miles on axis h1 and plots a trajectory from
+%      variables x, y, and z
+%   Clay M. Thompson 4-24-1991, CBM 8-21-92.
+%   Will Campbell, 3-30-2010
+%   Copyright 1984-2010 The MathWorks, Inc. 
+%% Input Handling
+[cax,args,nargs] = axescheck(varargin{:}); % Parse possible Axes input
+error(nargchk(0,2,nargs)); % Ensure there are a valid number of inputs
+% Handle remaining inputs.
+% Should have 0 or 1 string input, 0 or 1 numeric input
+j = 0;
+k = 0;
+n = 50; % default value
+units = 'km'; % default value
+for i = 1:nargs
+    if ischar(args{i})
+        units = args{i};
+        j = j+1;
+    elseif isnumeric(args{i})
+        n = args{i};
+        k = k+1;
+    end
+end
+if j > 1 || k > 1
+    error('Invalid input types')
+end
+%% Calculations
+% Scale factors
+Scale = {'km' 'm'  'mile'            'miles'           'nm'              'au'                 'ft';
+         1    1000 0.621371192237334 0.621371192237334 0.539956803455724 6.6845871226706e-009 3280.839895};
+% Identify which scale to use
+try
+    myscale = 6378.1363*Scale{2,strcmpi(Scale(1,:),units)};
+catch %#ok<*CTCH>
+    error('Invalid units requested. Please use m, km, ft, mile, miles, nm, or AU')
+end
+     
+% -pi <= theta <= pi is a row vector.
+% -pi/2 <= phi <= pi/2 is a column vector.
+theta = (-n:2:n)/n*pi;
+phi = (-n:2:n)'/n*pi/2;
+cosphi = cos(phi); cosphi(1) = 0; cosphi(n+1) = 0;
+sintheta = sin(theta); sintheta(1) = 0; sintheta(n+1) = 0;
+x = myscale*cosphi*cos(theta);
+y = myscale*cosphi*sintheta;
+z = myscale*sin(phi)*ones(1,n+1);
+%% Plotting
+if nargout == 0
+    cax = newplot(cax);
+    % Load and define topographic data
+    load('topo.mat','topo','topomap1');
+    % Rotate data to be consistent with the Earth-Centered-Earth-Fixed
+    % coordinate conventions. X axis goes through the prime meridian.
+    % http://en.wikipedia.org/wiki/Geodetic_system#Earth_Centred_Earth_Fixed_.28ECEF_or_ECF.29_coordinates
+    %
+    % Note that if you plot orbit trajectories in the Earth-Centered-
+    % Inertial, the orientation of the contintents will be misleading.
+    topo2 = [topo(:,181:360) topo(:,1:180)]; %#ok<NODEF>
+    
+    % Define surface settings
+    props.FaceColor= 'texture';
+    props.EdgeColor = 'none';
+    props.FaceLighting = 'phong';
+    props.Cdata = topo2;
+    % Create the sphere with Earth topography and adjust colormap
+    surface(x,y,z,props,'parent',cax)
+    colormap(topomap1)
+% Replace the calls to surface and colormap with these lines if you do 
+% not want the Earth's topography displayed.
+%     surf(x,y,z,'parent',cax)
+%     shading flat
+%     colormap gray
+    
+    % Refine figure
+    axis equal
+    xlabel(['X [' units ']'])
+    ylabel(['Y [' units ']'])
+    zlabel(['Z [' units ']'])
+    view(127.5,30)
+else
+    xx = x; yy = y; zz = z;
+end

EKF/Quat2Eu.m

@@ -0,0 +1,6 @@
+%%%Moments of Inertia Cubesat
+Ix = 0.9;
+Iy = 0.9;
+Iz = 0.3;
+I = [Ix,0,0;0,Iy,0;0,0,Iz]; %%kg-m^2
+invI = inv(I);

EKF/Satellite.m

@@ -0,0 +1,118 @@
+%%% Clear workspace
+clear
+clc
+close all
+
+%%%Simulated noise parameters
+% Process noise covariance
+Q = 1e-7;
+p_noise = [Q;Q;Q;Q;Q;Q;Q];
+% Measurement noise covariance
+Re = 5e-6;
+m_noise = [Re;Re;Re;Re;0.0001*Re;0.0001*Re;0.0001*Re];
+% Sampling time
+Ts = 0.1; % [s]
+
+%%%Get Earth Parameters for orbit
+planet
+
+%%%Cubesat parameters
+global m invI I
+m = 2.6; %%mass in kilograms
+inertia
+
+%%%Initial Conditions Position and Velocity
+altitude = 408*1000; %%meters
+x0 = R + altitude;
+y0 = 0;
+z0 = 0; 
+xdot0 = 0;
+inclination = deg2rad(56);
+semi_major = norm([x0;y0;z0]); %%Orbit Radius for circular orbit
+vcircular = sqrt(mu/semi_major); %%Orbital Speed from Vis-Viva Equation with a=r
+ydot0 = vcircular*cos(inclination);
+zdot0 = vcircular*sin(inclination);
+
+%%%Initial Conditions Attitude and Angular velocity
+%Initialize Euler Angles
+phi0 = 0;
+theta0 = 0;
+psi0 = 0;
+ptp0 = [phi0;theta0;psi0];
+q0123_0 = Eu2Quat(ptp0); %Transform to quat initial conditions
+
+%Initial Angular Velocity (Body Frame)
+p0=0.01;
+q0=0.01;
+r0=0;
+
+%Initial Conditions state vector
+stateinitial = [q0123_0;p0;q0;r0];
+
+%%%Orbit time parameters Circular Orbit
+period = 2*pi/sqrt(mu)*semi_major^(3/2); %%Tcircular
+number_of_orbits = 1;
+tfinal = period*number_of_orbits;
+tspan = [0,tfinal];
+
+%%%This is where we integrate the equations of motion
+[tout,stateout] = ode45(@Satellite,tspan,stateinitial);
+%{
+%%%Convert state to kilometers
+stateout(:,1:6) = stateout(:,1:6)/1000;
+
+%%%Extract the state vector
+xout = stateout(:,1);
+yout = stateout(:,2);
+zout = stateout(:,3);
+%}
+q0123out = stateout(:,1:4);
+ptpout = Quat2Eu(q0123out);
+pqrout = stateout(:,5:7);
+%{
+%%%Make an Earth
+[X,Y,Z] = sphere(100);
+X = X*R/1000;
+Y = Y*R/1000;
+Z = Z*R/1000;
+
+%%%Plot 3D orbit
+fig = figure();
+set(fig,'color','white')
+plot3(xout,yout,zout,'b-','LineWidth',4)
+xlabel('X')
+ylabel('Y')
+zlabel('Z')
+grid on
+hold on
+earth_sphere()
+axis equal
+
+%%%Plot Quaternions
+fig2 = figure();
+set(fig2,'color','white')
+plot(tout,q0123out,'-','LineWidth',2);
+grid on
+xlabel('Time (sec)')
+ylabel('Quaternions')
+legend('q0','q1','q2','q3')
+
+%%%Plot Euler Angles
+fig3 = figure();
+set(fig3,'color','white')
+plot(tout,ptpout*180/pi,'-','LineWidth',2);
+grid on
+xlabel('Time (sec)')
+ylabel('Euler Angles (deg)')
+legend('Phi','Theta','Psi')
+
+%%%Plot Angular Velocity
+fig4 = figure();
+set(fig4,'color','white')
+plot(tout,pqrout,'-','LineWidth',2);
+grid on
+xlabel('Time (sec)')
+ylabel('Angular Velocity (rad/s)')
+legend('p','q','r')
+%}
+

EKF/earth_sphere.m

@@ -0,0 +1,6 @@
+function y = myMeasurementFcn(x)
+% x = [q0123;p;q;r]
+q0123 =x(1:4);
+pqr = x(5:7);
+y = [q0123;pqr]; 
+end