├── README.md ├── Reading_Project.mat ├── Reading_Project_Report.pdf ├── Reading_Project_model.slx ├── Reading_Project_script.m └── funs ├── A2g.m ├── A2q.m ├── A2q1.m ├── B_calc.m ├── MA2TA.m ├── car2spher.m ├── g2A.m ├── hat3.m ├── hat4.m ├── hat4q.m ├── kep2car_v1.m ├── kep_eq_E.m ├── magfd.m ├── q2A.m ├── rot.m ├── saturation1.m ├── sh2020.mat └── spher2car.m /README.md: -------------------------------------------------------------------------------- 1 | # Attitude-Control-Using-Chebyshev-Neural-Network 2 | Re-Implementation of paper "Active attitude fault-tolerant tracking control of flexible spacecraft via the Chebyshev neural Network" 3 | 4 | Re-implemented paper: Lu, K., Li, T., & Zhang, L. (2019). Active attitude fault-tolerant tracking control of flexible spacecraft via the Chebyshev neural network. Transactions of the Institute of Measurement and Control, 41(4), 925–933. https://doi.org/10.1177/0142331218803410 5 | 6 | To get results; 7 | 1) run "Reading_Project_script.m" to create .mat file. 8 | 2) initialize "Reading_Project_model.slx" with created .mat file. 9 | 3) simulate "Reading_Project_model.slx" for 500 seconds. 10 | -------------------------------------------------------------------------------- /Reading_Project.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abkoc/Attitude-Control-Using-Chebyshev-Neural-Network/a47e745dd5a8da0b9bfa201aae90fcb5857ffd39/Reading_Project.mat -------------------------------------------------------------------------------- /Reading_Project_Report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abkoc/Attitude-Control-Using-Chebyshev-Neural-Network/a47e745dd5a8da0b9bfa201aae90fcb5857ffd39/Reading_Project_Report.pdf -------------------------------------------------------------------------------- /Reading_Project_model.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abkoc/Attitude-Control-Using-Chebyshev-Neural-Network/a47e745dd5a8da0b9bfa201aae90fcb5857ffd39/Reading_Project_model.slx -------------------------------------------------------------------------------- /Reading_Project_script.m: -------------------------------------------------------------------------------- 1 | %% Ahmet Burak KOC - kocab@hotmail.com 2 | 3 | %% Initialization 4 | clear; clc; close all 5 | addpath('funs') 6 | % 3x3 Nominal moment of inertia [kg.m^2] eq(49) 7 | J0 = [ 800.027 0 0 ; 8 | 0 839.930 0 ; 9 | 0 0 289.930 ]; 10 | % 3x3 Uncertain inertia matrix [kg.m^2] 11 | %Delta_J = diag([50 30 20]'); 12 | Delta_J = diag([0 0 0]'); 13 | % 3x3 Actual inertia matrix 14 | J = J0 + Delta_J; 15 | 16 | 17 | % Flexible parameters 18 | % Vibration-mode frequency matrix i=1,2,3,4 19 | omega_n = [1.0973, 1.2761, 1.6538, 2.2893]'; 20 | % Ratio of vibration-mode damping i=1,2,3,4 21 | zeta = [0.05, 0.06, 0.08, 0.025]'; 22 | % Coupling matrix (delta) 4x3 matrix 23 | coupling_matrix = [ 6.45637 1.27814 2.15629 ; 24 | -1.25619 0.91756 -1.67264 ; 25 | 1.11687 2.48901 -0.83674 ; 26 | 1.23637 -2.65810 -1.12503 ]; 27 | 28 | % K, L -> stiffness and damping matrices 29 | K = diag(omega_n.^2); 30 | L = diag(2*omega_n.*zeta); 31 | 32 | % 4x1 Initial flexible vibration 33 | eta_init = [0.01242, 0.01584, -0.01749, 0.01125]'; 34 | 35 | % 3x1 Initial Angular velocity (omega) 36 | omega_init = [0, 0, 0]'; 37 | 38 | % 4x1 Initial attitude in quaternion 39 | q_init = [0.3, -0.2, -0.3, 0.8832]' ; 40 | 41 | %qd_init = zeros(4,1) ; 42 | qd_init = [0, 0, 0, 1]' ; 43 | 44 | % disturbances 45 | d = zeros(3,1); 46 | 47 | % sliding_mode 48 | epsilon = 0.1 ; 49 | iota1 = 3 ; 50 | iota2 = 5 ; 51 | q_p = 3/5 ; 52 | k11 = 0.5 ; 53 | k12 = 0.5 ; 54 | k13 = 0.5 ; 55 | k21 = 0.5 ; 56 | k22 = 0.5 ; 57 | k23 = 0.5 ; 58 | k1 = [k11, k12, k13]'; 59 | k2 = [k21, k22, k23]'; 60 | K1 = diag(k1); 61 | K2 = diag(k2); 62 | 63 | % Chebyshev Polynomial 64 | % order_num = 2 65 | sigma1 = 100 ; 66 | sigma2 = 0.01 ; 67 | weight_init = rand(3,25) ; 68 | 69 | % Neural Network Controller 70 | % J0 ; 71 | tau = 10 ; 72 | chi = 25 ; 73 | gamma = 0.35 ; 74 | lambda = 0.6 ; 75 | 76 | % Actuator parameters 77 | %%%%%%%%%%% to be changed later %%%%%%%%%% 78 | um = 68; % maximum torque Nm 79 | 80 | 81 | save('Reading_Project.mat') -------------------------------------------------------------------------------- /funs/A2g.m: -------------------------------------------------------------------------------- 1 | function g = A2g(A) 2 | %A2G DCN to gibbs vector 3 | tmp = 1+A(1,1)+A(2,2)+A(3,3); 4 | g = [(A(2,3)-A(3,2))/tmp;... 5 | (A(3,1)-A(1,3))/tmp; 6 | (A(1,2)-A(2,1))/tmp]; 7 | end 8 | 9 | -------------------------------------------------------------------------------- /funs/A2q.m: -------------------------------------------------------------------------------- 1 | function q = A2q(A) 2 | % DCN to quaternion 3 | tol = 1e-3; 4 | q4=sqrt(1+A(1,1)+A(2,2)+A(3,3))/2; 5 | if abs(q4) > tol 6 | q=[(A(2,3)-A(3,2))/4/q4;... 7 | (A(3,1)-A(1,3))/4/q4;... 8 | (A(1,2)-A(2,1))/4/q4;... 9 | q4]; 10 | else 11 | q1 = sqrt(1+A(1,1)-A(2,2)-A(3,3))/2; 12 | q2 = sqrt(1-A(1,1)+A(2,2)-A(3,3))/2; 13 | q3 = sqrt(1-A(1,1)-A(2,2)+A(3,3))/2; 14 | if abs(q1) > tol 15 | q=[ q1;... 16 | (A(1,2)-A(2,1))/4/q1;... 17 | (A(1,3)-A(3,1))/4/q1;... 18 | (A(2,3)-A(3,2))/4/q1]; 19 | elseif abs(q2) > tol 20 | q=[ (A(1,2)-A(2,1))/4/q2;... 21 | q2;... 22 | (A(2,3)-A(3,2))/4/q2;... 23 | (A(3,1)-A(1,3))/4/q2]; 24 | else 25 | q=[ (A(1,3)-A(3,1))/4/q3;... 26 | (A(2,3)-A(3,2))/4/q3;... 27 | q3;... 28 | (A(1,2)-A(2,1))/4/q3]; 29 | end 30 | end 31 | q = q/norm(q); 32 | end 33 | 34 | -------------------------------------------------------------------------------- /funs/A2q1.m: -------------------------------------------------------------------------------- 1 | function q = A2q1(Q) 2 | % DCN to quaternion from H.Curtis book 3 | %{ 4 | This function calculates the quaternion from the direction 5 | cosine matrix. 6 | 7 | Q - direction cosine matrix 8 | q - quaternion (where q(4) is the scalar part) 9 | %} 10 | % ---------------------------------------------- 11 | K3 = ... 12 | [Q(1,1)-Q(2,2)-Q(3,3), Q(2,1)+Q(1,2), Q(3,1)+Q(1,3), Q(2,3)-Q(3,2); 13 | Q(2,1)+Q(1,2), Q(2,2)-Q(1,1)-Q(3,3), Q(3,2)+Q(2,3), Q(3,1)-Q(1,3); 14 | Q(3,1)+Q(1,3), Q(3,2)+Q(2,3), Q(3,3)-Q(1,1)-Q(2,2), Q(1,2)-Q(2,1); 15 | Q(2,3)-Q(3,2), Q(3,1)-Q(1,3), Q(1,2)-Q(2,1), Q(1,1)+Q(2,2)+Q(3,3)]/3; 16 | [eigvec, eigval] = eig(K3); 17 | [~,i] = max(diag(eigval)); 18 | q = eigvec(:,i); 19 | 20 | end -------------------------------------------------------------------------------- /funs/B_calc.m: -------------------------------------------------------------------------------- 1 | function B = B_calc(vb,vn,a) 2 | %B_CALC calcs B matrix for SVD, q and QUEST methods 3 | a = a/norm(a); 4 | B = zeros(3); 5 | for i=1:length(a) 6 | B = B + a(i)*vb(:,i)*vn(:,i)'; 7 | end 8 | end 9 | 10 | -------------------------------------------------------------------------------- /funs/MA2TA.m: -------------------------------------------------------------------------------- 1 | function TA = MA2TA(M,e) 2 | %MA2TA calcs true anomaly from mean anomaly 3 | % INPUT M - mean anomaly in radians 4 | % e - eccentricity 5 | % OUTPUT TA - eccentric anomaly in radians 6 | E = kep_eq_E(M,e); 7 | TA = 2*atan(tan(E/2)*sqrt((1-e)/(1+e))^-1); 8 | if TA < 0 9 | TA = TA+2*pi; 10 | end 11 | end 12 | 13 | -------------------------------------------------------------------------------- /funs/car2spher.m: -------------------------------------------------------------------------------- 1 | function sph_coords = car2spher(X) 2 | %CAR2SPHER converts cartesian coordinates to spherical 3 | % INPUT [X Y Z] 4 | % OUTPUT [Right accencion [rad], declination [rad], radius [as XYZ]] 5 | RA = atan2(X(2),X(1)); 6 | if RA < 0 7 | RA = RA+2*pi; 8 | end 9 | r=sqrt(sum(X.^2)); 10 | decl = asin(X(3)/r); 11 | sph_coords = [RA,decl,r]; 12 | end 13 | 14 | -------------------------------------------------------------------------------- /funs/g2A.m: -------------------------------------------------------------------------------- 1 | function A = g2A(g) 2 | % g2A GIBBS vector to cosine matrix 3 | % gibbs vector in input 4 | % its a redneck way to do algorithm from week 4 lectures page 10 5 | % 04/01/2020 6 | if length(g)~=3 7 | error('Wrong Gibbs vector') 8 | end 9 | A = [1+g(1)^2-g(2)^2-g(3)^2 2*(g(1)*g(2)+g(3)) 2*(g(1)*g(3)-g(2)); 10 | 2*(g(1)*g(2)-g(3)) 1-g(1)^2+g(2)^2-g(3)^2 2*(g(2)*g(3)+g(1)); 11 | 2*(g(1)*g(3)+g(2)) 2*(g(2)*g(3)-g(1)) 1-g(1)^2-g(2)^2+g(3)^2]/... 12 | (1+g(1)^2+g(2)^2+g(3)^2); 13 | 14 | 15 | end 16 | 17 | -------------------------------------------------------------------------------- /funs/hat3.m: -------------------------------------------------------------------------------- 1 | function B = hat3(a) 2 | %HAT3 forms a 3hat matrix from vector for DCN kinematics integration 3 | % 3x1 vector as input 4 | % 3x3 matrix as output 5 | B = [0 -a(3) a(2);a(3) 0 -a(1);-a(2) a(1) 0]; 6 | end 7 | 8 | -------------------------------------------------------------------------------- /funs/hat4.m: -------------------------------------------------------------------------------- 1 | function B = hat4(w) 2 | %HAT4 forms a 4hat matrix from vector used for kinemitics quaternion 3 | %integration 4 | % 3x1 vector as input 5 | % 4x4 matrix as output 6 | B = [0 w(3) -w(2) w(1);... 7 | -w(3) 0 w(1) w(2);... 8 | w(2) -w(1) 0 w(3);... 9 | -w(1) -w(2) -w(3) 0]; 10 | end -------------------------------------------------------------------------------- /funs/hat4q.m: -------------------------------------------------------------------------------- 1 | function B = hat4q(q) 2 | % calc hat4 quaternion used for attitude error calculation 3 | if length(q)~=4 4 | error('Wrong dimensions at hat4q') 5 | end 6 | B = [q(4) q(3) -q(2) -q(1);... 7 | -q(3) q(4) q(1) -q(2);... 8 | q(2) -q(1) q(4) -q(3);... 9 | q(1) q(2) q(3) q(4)]; 10 | end 11 | 12 | -------------------------------------------------------------------------------- /funs/kep2car_v1.m: -------------------------------------------------------------------------------- 1 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 2 | function [r, v] = kep2car_v1(coe,mu) 3 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 4 | %{ 5 | This function computes the state vector (r,v) from the 6 | classical orbital elements (coe). 7 | mu - gravitational parameter (km^3;s^2) 8 | coe - orbital elements [a e RA incl w TA] 9 | where 10 | a = semimajor axis (km) 11 | e = eccentricity 12 | RA = right ascension of the ascending node (rad) 13 | incl = inclination of the orbit (rad) 14 | w = argument of perigee (rad) 15 | TA = true anomaly (rad) 16 | R3_w - Rotation matrix about the z-axis through the angle w 17 | R1_i - Rotation matrix about the x-axis through the angle i 18 | R3_W - Rotation matrix about the z-axis through the angle RA 19 | Q_pX - Matrix of the transformation from perifocal to geocentric 20 | equatorial frame 21 | rp - position vector in the perifocal frame (km) 22 | vp - velocity vector in the perifocal frame (km/s) 23 | r - position vector in the geocentric equatorial frame (km) 24 | v - velocity vector in the geocentric equatorial frame (km/s) 25 | User M-functions required: none 26 | %} 27 | % ---------------------------------------------- 28 | a = coe(1); 29 | e = coe(2); 30 | RA = coe(3); 31 | incl = coe(4); 32 | w = coe(5); 33 | TA = coe(6); 34 | h = sqrt(a*mu*(1-e^2)); 35 | %...Equations 4.45 and 4.46 (rp and vp are column vectors): 36 | rp = (h^2/mu) * (1/(1 + e*cos(TA))) * (cos(TA)*[1;0;0] + sin(TA)*[0;1;0]); 37 | vp = (mu/h) * (-sin(TA)*[1;0;0] + (e + cos(TA))*[0;1;0]); 38 | %...Equation 4.34: 39 | R3_W = [ cos(RA) sin(RA) 0 40 | -sin(RA) cos(RA) 0 41 | 0 0 1]; 42 | %...Equation 4.32: 43 | R1_i = [1 0 0 44 | 0 cos(incl) sin(incl) 45 | 0 -sin(incl) cos(incl)]; 46 | %...Equation 4.34: 47 | R3_w = [ cos(w) sin(w) 0 48 | -sin(w) cos(w) 0 49 | 0 0 1]; 50 | %...Equation 4.49: 51 | 52 | Q_pX = (R3_w*R1_i*R3_W)'; 53 | %...Equations 4.51 (r and v are column vectors): 54 | r = Q_pX*rp; 55 | v = Q_pX*vp; 56 | %...Convert r and v into row vectors: 57 | r = r'; 58 | v = v'; 59 | end 60 | % ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -------------------------------------------------------------------------------- /funs/kep_eq_E.m: -------------------------------------------------------------------------------- 1 | function E = kep_eq_E(M,e) 2 | %KEP_EQ_E solves Kepler's equation by Newton E-e*sin(E)-M=0 3 | % INPUT M - mean anomaly in radians 4 | % e - eccentricity 5 | % OUTPUT E - eccentric anomaly in radians 6 | tolerance = 1e-8; 7 | E = M; 8 | dE=10; 9 | while abs(dE) > tolerance 10 | dE = (E-e*sin(E)-M)/(1-e*cos(E)); 11 | E = E-dE; 12 | end 13 | end -------------------------------------------------------------------------------- /funs/magfd.m: -------------------------------------------------------------------------------- 1 | function J=magfd(DATE,ITYPE,ALT,COLAT,ELONG,dgh,agh) 2 | % MAGFD 3 | % Function to compute Earths magnetic field 4 | % and components: X,Y,Z,T for a given latitude 5 | % and longitude, date and altitude using IGRF13. 6 | % 7 | % DATE = date of survey (decimal years) 8 | % ITYPE=1 for geodetic coordinates (usual case) 9 | % ITYPE=2 for geocentric coordinates 10 | % ALT = (for ITYPE=1) altitude of survey relative to sealevel (km +ve up) 11 | % ALT = (for ITYPE=2) radial distance from center of earth in km 12 | % COLAT=90-latitude (decimal degrees) 13 | % ELONG=longitude of survey (decimal degrees) 14 | % 15 | % Output array out contains components X,Y,Z,T in nanoteslas 16 | % X north component 17 | % Y east component 18 | % Z vertical component +ve down 19 | % T total field magnitude 20 | % 21 | % Usage: out=magfd(DATE,ITYPE,ALT,COLAT,ELONG); 22 | % 23 | % ref: IAGA, Division V, Working Group VMOD, 24 | % The 10th generation International Geomagnetic 25 | % Reference Field, Geophys. J. Int, 161, 561-565, 2005. 26 | % 27 | % Maurice A. Tivey March 1997 28 | % Mod Dec 1999 (add igrf2000 and y2k compliance 29 | % Mod Nov 2000 (use up to degree 10 sh coefficients) 30 | % Mod Apr 2005 added 2005 coeffs 31 | % Mod Sep 2006 some clean up and info added 32 | % Mod 2010 coeffs Ref: IGRF11 Finlay et al., 2010 33 | % Mod Jan 2015 coeffs Ref: IGRF12 IAGA V-MOD Working Group 34 | % Mod June 2017 uses coefficients thru degree 13 35 | % Mod Jan 2020 coeffs Ref: IGRF13 36 | % 37 | % http://deeptow.whoi.edu/matlab.html 38 | % Copyright: Maurice A. Tivey, 2017 39 | % Woods Hole Oceanographic Institution 40 | 41 | 42 | % Initialize IGRFYEAR as 2020 43 | COLAT = pi/2-COLAT; 44 | igrfyear=2020; 45 | %DGRF=[1000:5:igrfyear]; 46 | % simple switch if printout needed 47 | % negative DATE means don't print out 48 | %pl=0; 49 | 50 | % Determine year for base DGRF to use. 51 | % if DATE < igrfyear, 52 | % BASE=fix(DATE-DGRF(1)); 53 | % i=fix(BASE/5)+1; 54 | % BASE=DGRF(i); 55 | % if pl==0, 56 | % %fprintf('Using DGRF base year %f \n',BASE); 57 | % end 58 | % eval(['load sh',num2str(BASE)]) 59 | % % loads agh and agh41 but now need to get 60 | % iagh=agh;iagh41=agh41; 61 | % % load next epoch 62 | % if BASE < 1900, % a check to get pre-1900 estimates of gauss coeffs 63 | % eval(['load sh',num2str(BASE+25)]) 64 | % else 65 | % eval(['load sh',num2str(DGRF(i+1))]) 66 | % end 67 | % eagh=agh;eagh41=agh41; 68 | % dgh=(eagh-iagh)./5;dgh41=(eagh41-iagh41)./5; 69 | % agh=iagh;agh41=iagh41; 70 | % clear iagh iagh41 eagh eagh41 71 | % T = DATE - BASE; 72 | % else 73 | % if pl==0, 74 | % %fprintf('Using IGRF base year %f \n',igrfyear); 75 | % end 76 | %tmp=load('sh2020.mat'); 77 | % agh=coefs.agh; 78 | % agh41=coefs.agh41; 79 | % dgh=coefs.dgh; 80 | % dgh41=coefs.dgh41; 81 | CL=zeros(1,13);%----------------------------------------added by Nicola 82 | SL=zeros(1,13);%----------------------------------------added by Nicola 83 | P=zeros(1,104);%----------------------------------------added by Nicola 84 | Q=zeros(1,104);%----------------------------------------added by Nicola 85 | FN=0;FM=0;RR=0; 86 | 87 | %eval(['load ',igrffile]) % load in igrf data file 88 | T = DATE - igrfyear; 89 | % end 90 | % combine spherical harmonic coefficients from first 8 degrees 91 | % with degrees 9 thru 13 if they exist in the data file 92 | % agh=[agh,agh41]; 93 | % dgh=[dgh,dgh41]; 94 | % 95 | %D2R = 0.017453292; %pi/180; 96 | R = ALT; 97 | SLAT = cos(COLAT); 98 | CLAT = sin(COLAT); 99 | CL(1) = cos(ELONG); 100 | SL(1) = sin(ELONG); 101 | X = 0.0; 102 | Y = 0.0; 103 | Z = 0.0; 104 | CD = 1.0; 105 | SD = 0.0; 106 | L = 1; 107 | M = 1; 108 | N = 0; 109 | RE = 6371.2; % Earth's mean radius 110 | if ITYPE == 1 % CONVERSION FROM GEODETIC TO GEOCENTRIC COORDINATES 111 | %A2 = 40680925.; % squared semi major axis 112 | %B2 = 40408588.; % squared semi minor axis 113 | % WGS84 114 | A2 = 40680631.6; %6378.137^2; % squared semi major axis 115 | B2 = 40408296.0; %6356.7523142^2; % squared semi minor axis 116 | ONE = A2*CLAT*CLAT; 117 | TWO = B2*SLAT*SLAT; 118 | THREE = ONE + TWO; 119 | FOUR = sqrt(THREE); 120 | R = sqrt(ALT*(ALT + 2.0*FOUR) + (A2*ONE + B2*TWO)/THREE); 121 | CD = (ALT + FOUR)/R; 122 | SD = (A2 - B2)/FOUR*SLAT*CLAT/R; 123 | ONE = SLAT; 124 | SLAT = SLAT*CD - CLAT*SD; 125 | CLAT = CLAT*CD + ONE*SD; 126 | end 127 | % if geocentric coordinates desired then only need to define the following 128 | RATIO = RE/R; 129 | % 130 | % COMPUTATION OF SCHMIDT QUASI-NORMAL COEFFICIENTS P AND X(=Q) 131 | % 132 | P(1) = 2.0*SLAT; 133 | P(2) = 2.0*CLAT; 134 | P(3) = 4.5*SLAT*SLAT - 1.5; 135 | P(4) = sqrt(27)*CLAT*SLAT; 136 | Q(1) = -CLAT; 137 | Q(2) = SLAT; 138 | Q(3) = -3.0*CLAT*SLAT; 139 | Q(4) = sqrt(3)*(SLAT*SLAT - CLAT*CLAT); 140 | 141 | NMAX=13; % Max number of harmonic degrees , 13 142 | NPQ=(NMAX*(NMAX+3))/2; 143 | for K=1:NPQ 144 | if N < M 145 | M = 0; 146 | N = N + 1; 147 | RR = RATIO^(N + 2); 148 | FN = N; 149 | end 150 | FM = M; 151 | if K >= 5 %8,5,5 152 | if (M-N) == 0 %,7,6,7 153 | ONE = sqrt(1.0 - 0.5/FM); 154 | J = K - N - 1; 155 | P(K) = (1.0 + 1.0/FM)*ONE*CLAT*P(J); 156 | Q(K) = ONE*(CLAT*Q(J) + SLAT/FM*P(J)); 157 | SL(M) = SL(M-1)*CL(1) + CL(M-1)*SL(1); 158 | CL(M) = CL(M-1)*CL(1) - SL(M-1)*SL(1); 159 | else 160 | ONE = sqrt(FN*FN - FM*FM); 161 | TWO = sqrt((FN - 1.0)^2 - FM*FM)/ONE; 162 | THREE = (2.0*FN - 1.0)/ONE; 163 | I = K - N; 164 | J = K - 2*N + 1; 165 | P(K) = (FN + 1.0)*(THREE*SLAT/FN*P(I) - TWO/(FN - 1.0)*P(J)); 166 | Q(K) = THREE*(SLAT*Q(I) - CLAT/FN*P(I)) - TWO*Q(J); 167 | end 168 | % 169 | % SYNTHESIS OF X, Y AND Z IN GEOCENTRIC COORDINATES 170 | % 171 | end 172 | ONE = (agh(L) + dgh(L)*T)*RR; 173 | 174 | if M == 0 %10,9,10 175 | X = X + ONE*Q(K); 176 | Z = Z - ONE*P(K); 177 | L = L + 1; 178 | else 179 | TWO = (agh(L+1) + dgh(L+1)*T)*RR; 180 | THREE = ONE*CL(M) + TWO*SL(M); 181 | X = X + THREE*Q(K); 182 | Z = Z - THREE*P(K); 183 | if CLAT > 0 %12,12,11 184 | Y = Y+(ONE*SL(M)-TWO*CL(M))*FM*P(K)/((FN + 1.0)*CLAT); 185 | else 186 | Y = Y + (ONE*SL(M) - TWO*CL(M))*Q(K)*SLAT; 187 | end 188 | L = L + 2; 189 | end 190 | M = M + 1; 191 | end 192 | % CONVERSION TO COORDINATE SYSTEM SPECIFIED BY ITYPE 193 | ONE = X; 194 | X = X*CD + Z*SD; 195 | Z = Z*CD - ONE*SD; 196 | %T = sqrt(X*X + Y*Y + Z*Z); 197 | J=[X,Y,Z]'*1e-9; 198 | % END 199 | -------------------------------------------------------------------------------- /funs/q2A.m: -------------------------------------------------------------------------------- 1 | function A = q2A(q) 2 | %q2A Quaternion to DCN 3 | % quaternion in input 4 | % it's a redneck way to write: A=(q(4)^2-q'*q)*I+2*q*q'-2*q(4)*q^; 5 | % 04/01/2020 6 | if (length(q)~=4)||(abs(norm(q)-1)>0.1) 7 | error('Wrong quaternion input') 8 | end 9 | % A = [q(1)^2-q(2)^2-q(3)^2+q(4)^2 2*(q(1)*q(2)+q(3)*q(4)) 2*(q(1)*q(3)-q(2)*q(4));... 10 | % 2*(q(1)*q(2)-q(3)*q(4)) -q(1)^2+q(2)^2-q(3)^2+q(4)^2 2*(q(2)*q(3)+q(1)*q(4));... 11 | % 2*(q(1)*q(3)+q(2)*q(4)) 2*(q(2)*q(3)-q(1)*q(4)) -q(1)^2-q(2)^2+q(3)^2+q(4)^2]; 12 | A=(q(4)^2-q(1:3)'*q(1:3))*eye(3)+2*q(1:3)*(q(1:3)')-2*q(4)*hat3(q(1:3)); 13 | 14 | for i=1:5 15 | A = A*3/2-A*(A')*A/2; 16 | end 17 | end 18 | 19 | -------------------------------------------------------------------------------- /funs/rot.m: -------------------------------------------------------------------------------- 1 | function B = rot(axis,ang) 2 | %Returns a rotation matrix around a specific axis 3 | % INPUT axis - x,y or z 4 | % angle in radians 5 | switch axis 6 | case 'x' 7 | B = [1 0 0; 0 cos(ang) -sin(ang);0 sin(ang) cos(ang)]; 8 | case 'y' 9 | B = [cos(ang) 0 sin(ang);0 1 0;-sin(ang) 0 cos(ang)]; 10 | case 'z' 11 | B = [cos(ang) -sin(ang) 0; sin(ang) cos(ang) 0; 0 0 1]; 12 | otherwise 13 | error('Bad axis in rotation matrix') 14 | end 15 | 16 | -------------------------------------------------------------------------------- /funs/saturation1.m: -------------------------------------------------------------------------------- 1 | function y = saturation1(u,lim) 2 | %Describes a saturation discontinuity 3 | % u - value 4 | % lim - saturation limit 5 | 6 | y = (abs(u)=lim).*sign(u)*lim; 7 | 8 | end 9 | 10 | -------------------------------------------------------------------------------- /funs/sh2020.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/abkoc/Attitude-Control-Using-Chebyshev-Neural-Network/a47e745dd5a8da0b9bfa201aae90fcb5857ffd39/funs/sh2020.mat -------------------------------------------------------------------------------- /funs/spher2car.m: -------------------------------------------------------------------------------- 1 | function X = spher2car(sph_coord) 2 | %SPHER2CAR converts spherical coordinates to cartesian 3 | % INPUT [Right accencion [rad], declination [rad], radius [whatever]] 4 | % OUTPUT [X Y Z] 5 | RA=sph_coord(:,1);decl=sph_coord(:,2);r=sph_coord(:,3); 6 | X = (r.*[cos(decl).*cos(RA) cos(decl).*sin(RA) sin(decl)])'; 7 | end 8 | 9 | --------------------------------------------------------------------------------