%%WOD Measurements
%Read coarse sun sensor data from WOD file along with unix time
css = readmatrix('C:\Users\kevin\Desktop\Uni Work - Kevin\Masters\Disso\Matlab\WOD.xlsx', 'Range', 'H113926:I122919');
csslength = 1499; %Number of measurements from each sensor
csst = css(1:csslength,2); %Time Stamps
css = [css(1:csslength,1), css((csslength + 1):(2*csslength),1), css((2*csslength + 1):(3*csslength),1), ...
css((3*csslength + 1):(4*csslength),1), css((4*csslength + 1):(5*csslength),1), css((5*csslength + 1):(6*csslength),1)]; %CSS data sorted in 6 columns
%Remove the sensor data captured during eclipse
css(65, :) = []; %Corrupted data from excel removed
css(65, :) = []; %Corrupted data from excel removed
css(111:210, :) = [];
css(306:405, :) = [];
css(501:601, :) = [];
css(695:795, :) = [];
css(890:989, :) = [];
csst(65) = [];
csst(65) = [];
csst(111:210) = [];
csst(306:405) = [];
csst(501:601) = [];
csst(695:795) = [];
csst(890:989) = [];
%Convert sun sensor data into engineering values (i.e. angles)
css = css./255; %Normalise the values to get them in the range [0, 1]
cssangle = acos(css); %Get sun angle in radians
cssd = css * (180/pi); %Or in degrees
%Separate the 6 sun sensors
css1 = css(:, 1); %Xb
css2 = css(:, 2); %-Xb
css3 = css(:, 3); %Yb
css4 = css(:, 4); %-Yb
css5 = css(:, 5); %Zb
css6 = css(:, 6); %-Zb
%Similarly for magnetometers
mgm = readmatrix('C:\Users\kevin\Desktop\Uni Work - Kevin\Masters\Disso\Matlab\WOD.xlsx', 'Range', 'H127417:I131913');
mgmlength = 1499;
mgm = [mgm(1:mgmlength,1), mgm((mgmlength + 1):(2*mgmlength),1), mgm((2*mgmlength + 1):(3*mgmlength),1)]; %Magnetometer measurements sorted in 3 columns
mgmt = csst;
mgm(65, :) = []; %Corrupted data from excel removed
mgm(65, :) = []; %Corrupted data from excel removed
mgm(111:210, :) = [];
mgm(306:405, :) = [];
mgm(501:601, :) = [];
mgm(695:795, :) = [];
mgm(890:989, :) = [];
%Read Y axis angular rate measurements
w_obs = readmatrix('C:\Users\kevin\Desktop\Uni Work - Kevin\Masters\Disso\Matlab\WOD.xlsx', 'Range', 'H83946:H86943');
w_obs(2:2:end) = [];
w_obs = w_obs';
w_obs(65) = [];
w_obs(65) = [];
w_obs(111:210) = [];
w_obs(306:405) = [];
w_obs(501:601) = [];
w_obs(695:795) = [];
w_obs(890:989) = [];
for i = 1:length(w_obs)
w_obs(i) = deg2rad(w_obs(i));
end
%% Orbit Simulation
%TLE for ALSAT 1N at the time closest to 1st sensor measurement
%1 41789U 16059G 18144.44923147 .00000100 00000+0 28090-4 0 9995
%2 41789 98.1194 207.5640 0028815 159.8203 200.4146 14.64153751 88548
%Calculate orbit period and semi-major axis from TLE
R_ = 6378.137; %Earth radius in km
mu = 398600; %km^3/s^2
T = 86400/(14.64153751);
a = (((T/(2*pi))^2)*mu)^(1/3);
inclination = deg2rad(98.1194);
RAAN = deg2rad(207.5640);
e = 0.0028815;
w = deg2rad(159.8203);
n = sqrt(mu/(a^3));
M0 = deg2rad(200.4146) + n*(36*60+38); %Propagate mean anomaly to the time of first measurement
%Timespan from 1st to last sensor measurement
t = 0:20:(8*60*60 + 19*60 + 20);
M = M0 + n.*t;
theta = M; %Orbit is near circular so assume M = True Anomaly
coe = zeros(6, length(csst));
X = zeros(6, length(csst));
for tt = 1:length(t)
coe(:, tt) = [a, e, inclination, RAAN, w, theta(tt)]';
X(:, tt) = COE2RV(coe(:, tt), mu);
end
Xi = X(:, 1);
Xf = X(:, end);
[Xe,Ye,Ze] = sphere(50);
% Create figure
figure('units','normalized','outerposition',[0 0 1 1])
% Plot Earth
surf(R_*Xe, R_*Ye, R_*Ze, 'EdgeColor', 'none', 'FaceColor', 'c'); hold on; axis equal;
xlabel('X, ECI (km)');
ylabel('Y, ECI (km)');
zlabel('Z, ECI (km)');
svectoreci = [6.877390932857540E+07 1.238448303642737E+08 5.368708797204571E+07];
% Plot ECI axes
quiver3(0, 0, 0, 1, 0, 0, 1e4, 'k', 'LineWidth', 2); % I-axis
quiver3(0, 0, 0, 0, 1, 0, 1e4, 'k', 'LineWidth', 2); % J-axis
quiver3(0, 0, 0, 0, 0, 1, 1e4, 'k', 'LineWidth', 2); % K-axis
quiver3(0, 0, 0, svectoreci(1), svectoreci(2), svectoreci(3), 6.6e-5, 'k', 'LineWidth', 2);
text(1e4, 0, 0, 'I', 'FontSize', 20, 'Interpreter', 'tex', 'Color', 'k')
text(0, 1e4, 0, 'J', 'FontSize', 20, 'Interpreter', 'tex', 'Color', 'k')
text(0, 0, 1e4, 'K', 'FontSize', 20, 'Interpreter', 'tex', 'Color', 'k')
text(svectoreci(1)*6.6e-5, svectoreci(2)*6.6e-5, svectoreci(3)*6.6e-5, 'svectoreci', 'FontSize', 20, 'Interpreter', 'tex', 'Color', 'k')
% 113 212
% Plot Trajectory
plot3(X(1,:), X(2,:), X(3,:), 'k', 'LineWidth', 2);
plot3(X(1,1), X(2,1), X(3,1), 'o', 'MarkerFaceColor', 'b');
plot3(X(1,end), X(2,end), X(3,end), 'o', 'MarkerFaceColor', 'r');
plot3(X(1, 113), X(2, 113), X(3, 113), 'o', 'MarkerFaceColor', 'm');
plot3(X(1, 212), X(2, 212), X(3, 212), 'o', 'MarkerFaceColor', 'm');
legend('Earth', 'I-axis', 'J-axis', 'K-axis', 'Sun Vector from Geocentre', 'Orbital Path', 'Start of Simulation', 'End of Simulation', 'Start of Eclipse', 'End of Eclipse');
%% Get latitude and longitude in order to call IGRF function later
rECEF = zeros(3, length(csst));
long = zeros(1, length(csst));
lat = zeros(1, length(csst));
R3 = zeros(3, 3, length(csst));
utc = datetime(csst, 'ConvertFrom', 'posixtime', 'TimeZone', 'UTC');
year = year(utc);
month = month(utc);
day = day(utc);
hour = hour(utc);
minute = minute(utc);
second = second(utc);
UTC = [year, month, day, hour, minute, second];
deltaAT = 38; %TAI - UTC in seconds... https://celestrak.com/SpaceData/
deltaUT1 = 0.0827070; %UT1 - UTC in seconds
x = 0.102644;
y = 0.447763;
polarmotion = [x, y];
for i = 1:size(UTC, 1)
R3(:, :, i) = dcmeci2ecef('IAU-2000/2006',UTC(i, :),deltaAT,deltaUT1,polarmotion);
rECEF(:, i) = R3(:, :, i)*X(1:3, i);
long(:, i) = rad2deg(atan2(rECEF(2, i), rECEF(1, i)));
if rECEF(2, i) >= 0 && rECEF(1, i) >= 0
long(:, i) = rad2deg(atan2(rECEF(2, i), rECEF(1, i)));
elseif rECEF(2, i) < 0 && rECEF(1, i) >= 0
long(:, i) = 360 + rad2deg(atan2(rECEF(2, i), rECEF(1, i)));
elseif rECEF(2, i) >= 0 && rECEF(1, i) < 0
long(:, i) = rad2deg(atan2(rECEF(2, i), rECEF(1, i)));
elseif rECEF(2, i) < 0 && rECEF(1, i) < 0
long(:, i) = 360 + rad2deg(atan2(rECEF(2, i), rECEF(1, i)));
end
lat(:, i) = rad2deg(asin(rECEF(3, i)/norm(X(1:3, i))));
end
%% Attitude Estimation
%Solar Ephemeris at the time of first measurement in ICRF/ECI coordinates. Obtained from JPL (Horizon).
%2458262.972222222 = A.D. 2018-May-24 11:20:00.0000 TDB
% X = 6.877390932857540E+07 Y = 1.238448303642737E+08 Z = 5.368708797204571E+07
X(:, 65) = [];
X(:, 65) = [];
X(:, 111:210) = [];
X(:, 306:405) = [];
X(:, 501:601) = [];
X(:, 695:795) = [];
X(:, 890:989) = [];
sunvectorsim = ones(3, length(csst)) + [6.877390932857540E+07; 1.238448303642737E+08; 5.368708797204571E+07] - X(1:3, :); %Simulated sun vector in ECI frame / "Computed"
theta = linspace(0, 2*pi, 100); % parameter for circle
css_body = zeros(3, size(css, 1));
for i = 1:size(css, 1)
if cssangle(i, 1) < cssangle(i, 2)
r1 = 0.7*cos(cssangle(i, 1))*tan(cssangle(i, 1)); % radius of the circle
z1 = r1 * cos(theta); % z coordinates (2D)
y1 = r1 * sin(theta); % y coordinates (2D)
phi1 = asin(sqrt(z1.^2 + y1.^2)); % polar angle
lambda1 = atan2(y1, z1); % azimuthal angle
X1 = cos(phi1); % X coordinates (3D)
Y1 = sin(lambda1) .* sin(phi1); % Y coordinates (3D)
Z1 = cos(lambda1) .* sin(phi1); % Z coordinates (3D)
elseif cssangle(i, 1) > cssangle(i, 2)
r1 = 0.7*cos(cssangle(i, 2))*tan(cssangle(i, 2));
z1 = r1 * cos(theta);
y1 = r1 * sin(theta);
phi1 = asin(sqrt(z1.^2 + y1.^2)); % polar angle
lambda1 = atan2(y1, z1); % azimuthal angle
X1 = -cos(phi1);
Y1 = sin(lambda1) .* sin(phi1);
Z1 = cos(lambda1) .* sin(phi1);
end
if cssangle(i, 3) < cssangle(i, 4)
r2 = 0.9*cos(cssangle(i, 3))*tan(cssangle(i, 3));
x2 = r2 * cos(theta);
z2 = r2 * sin(theta);
phi2 = asin(sqrt(x2.^2 + z2.^2));
lambda2 = atan2(z2, x2);
X2 = sin(lambda2) .* sin(phi2);
Y2 = cos(phi2);
Z2 = cos(lambda2) .* sin(phi2);
elseif cssangle(i, 3) > cssangle(i, 4)
r2 = 0.9*cos(cssangle(i, 4))*tan(cssangle(i, 4));
x2 = r2 * cos(theta);
z2 = r2 * sin(theta);
phi2 = asin(sqrt(x2.^2 + z2.^2));
lambda2 = atan2(z2, x2);
X2 = sin(lambda2) .* sin(phi2);
Y2 = -cos(phi2);
Z2 = cos(lambda2) .* sin(phi2);
end
if cssangle(i, 5) < cssangle(i, 6)
r3 = 0.9*cos(cssangle(i, 5))*tan(cssangle(i, 5));
x3 = r3 * cos(theta);
y3 = r3 * sin(theta);
phi3 = asin(sqrt(x3.^2 + y3.^2));
lambda3 = atan2(y3, x3);
X3 = sin(lambda3) .* sin(phi3);
Y3 = cos(lambda3) .* sin(phi3);
Z3 = cos(phi3);
elseif cssangle(i, 5) > cssangle(i, 6)
r3 = 0.9*cos(cssangle(i, 6))*tan(cssangle(i, 6));
x3 = r3 * cos(theta);
y3 = r3 * sin(theta);
phi3 = asin(sqrt(x3.^2 + y3.^2));
lambda3 = atan2(y3, x3);
X3 = sin(lambda3) .* sin(phi3);
Y3 = cos(lambda3) .* sin(phi3);
Z3 = -cos(phi3);
end
initial_guess = [0.5, 0.5, 0.5];
options = optimoptions('fminunc', 'Display', 'iter', 'Algorithm', 'quasi-newton');
[solution, fval] = fminunc(@(vars) objectiveFunc(vars, X1, Y1, Z1, X2, Y2, Z2, X3, Y3, Z3), initial_guess, options);
X_sol = solution(1);
Y_sol = solution(2);
Z_sol = solution(3);
css_body(:, i) = [X_sol; Y_sol; Z_sol]/norm([X_sol; Y_sol; Z_sol]);
end
wgs84 = wgs84Ellipsoid('kilometer');
for i = 1:length(mgmt)
u2ms = unix_to_matlab_serial(mgmt(i));
B(:, i) = igrf(u2ms, lat(i), long(i), norm(X(1:3, i)), 'geocentric');
[x, y, z] = ned2ecef(B(1, i), B(2, i), B(3, i), lat(i), long(i), norm(X(1:3, i)) - 6328, wgs84); %Verify transformation
B(:, i) = [x, y, z]';
B(:, i) = R3(:, :, i)'*B(:, i);
end
C = zeros(3, 3, length(mgmt));
q = zeros(4, length(mgmt));
mgm = mgm';
for i = 1:length(mgmt)
C(:, :, i) = triad([sunvectorsim(:, i)/norm(sunvectorsim(:, i)), B(:, i)/norm(B(:, i))], [css_body(:, i), mgm(:, i)/norm(mgm(:, i))]);
q(:, i) = dcm2quat(C(:, :, i)); %Quaternion representing the satellite attitude wrt ECI frame
end
%% Angular Rate Estimation
w_est = zeros(3, size(q, 2));
% Bdot = zeros(3, size(q, 2));
%
% for i = 3:size(q, 2)
% Bdot(:, i) = (mgm(:, i) - mgm(:, i-2))/40;
% Bcross = [0, -mgm(3, i-1), mgm(2, i-1);
% mgm(3, i-1), 0, -mgm(1, i-1);
% -mgm(2, i-1), mgm(1, i-1), 0];
% w_est(:, i) = pinv(Bcross)*Bdot(:, i);
% end
for i = 2:size(q, 2)
qdot = (q(:, i) - q(:, i-1))/20;
q1 = q(1, i-1);
q2 = q(2, i-1);
q3 = q(3, i-1);
q4 = q(4, i-1);
Q = [-q2, -q3, -q4;
q1, -q4, q3;
q4, q1, -q2;
-q3, q2, q1];
Qinv = pinv(Q);
w_est(:, i) = 2*Qinv*qdot;
end
%% Plot attitude quaternion time evolution
% figure(2)
% plot(tt, q(1, :), 'b', tt, q(2, :), 'r', tt, q(3, :), 'y', tt, q(4, :), 'g');
% legend('q0', 'q1', 'q2', 'q3');
%% Plot estimated and measured angular rates against time for comparison
tt = 0:20:20*(size(q, 2)-1);
figure(2)
plot(tt, rad2deg(w_obs), 'b', tt, rad2deg(w_est(2, :)), 'r')
title('Estimated and measured Y-axis body angular rates in degrees per second')
legend('Measured', 'Estimated')
figure(3)
plot(tt, rad2deg(abs(w_est(2, :) -w_obs)))
title('Error in degrees per second between estimated and measured angular rates')
%% Celestial Sphere Sun Vector Visualisation
num_vectors = size(css_body, 2);
% Create a unit sphere
[phi, theta] = meshgrid(linspace(0, 2*pi, 100), linspace(0, pi, 50));
x = sin(theta) .* cos(phi);
y = sin(theta) .* sin(phi);
z = cos(theta);
% Set up the figure and plot the celestial sphere
figure(4)
b_sphere = surf(x, y, z, 'FaceAlpha', 0.1, 'EdgeColor', 'none');
hold on;
axis equal;
xlabel('X');
ylabel('Y');
zlabel('Z');
title('Celestial Sphere with Animated Sun Vectors');
grid on;
%Plot body axes
quiver3(0, 0, 0, 1, 0, 0, 'k', 'LineWidth', 2);
quiver3(0, 0, 0, 0, 1, 0, 'k', 'LineWidth', 2);
quiver3(0, 0, 0, 0, 0, 1, 'k', 'LineWidth', 2);
text(1, 0, 0, 'X', 'FontSize', 16, 'Interpreter', 'tex', 'Color', 'k');
text(0, 1, 0, 'Y', 'FontSize', 16, 'Interpreter', 'tex', 'Color', 'k');
text(0, 0, 1, 'Z', 'FontSize', 16, 'Interpreter', 'tex', 'Color', 'k');
% Initialize plot for the sun vector
b_vector = plot3([0, css_body(1, 1)], [0, css_body(2, 1)], [0, css_body(3, 1)], '-o', 'LineWidth', 2);
% Animation loop
for i = 1:num_vectors
% Update the sun vector plot
set(b_vector, 'XData', [0, css_body(1, i)], ...
'YData', [0, css_body(2, i)], ...
'ZData', [0, css_body(3, i)]);
% Pause to create the animation effect
pause(1/50); % Adjust the pause duration as needed
% Update the title to indicate the time step or vector index
figure(4)
title(['Celestial Sphere with Sun Vectors - Frame ' num2str(i)]);
end
%% Celestial Sphere Magnetic Field Vector Visualisation
num_vectors = size(mgm, 2);
% Create a unit sphere
[phi, theta] = meshgrid(linspace(0, 2*pi, 100), linspace(0, pi, 50));
x = sin(theta) .* cos(phi);
y = sin(theta) .* sin(phi);
z = cos(theta);
% Set up the figure and plot the celestial sphere
figure(5)
b_sphere = surf(x, y, z, 'FaceAlpha', 0.1, 'EdgeColor', 'none');
hold on;
axis equal;
xlabel('X');
ylabel('Y');
zlabel('Z');
title('Celestial Sphere with Animated Magnetic Field Vectors');
grid on;
%Plot body axes
quiver3(0, 0, 0, 1, 0, 0, 'k', 'LineWidth', 2);
quiver3(0, 0, 0, 0, 1, 0, 'k', 'LineWidth', 2);
quiver3(0, 0, 0, 0, 0, 1, 'k', 'LineWidth', 2);
text(1, 0, 0, 'X', 'FontSize', 16, 'Interpreter', 'tex', 'Color', 'k');
text(0, 1, 0, 'Y', 'FontSize', 16, 'Interpreter', 'tex', 'Color', 'k');
text(0, 0, 1, 'Z', 'FontSize', 16, 'Interpreter', 'tex', 'Color', 'k');
% Initialize plot for the magnetic field vector
b_vector = plot3([0, mgm(1, 1)/norm(mgm(:, 1))], [0, mgm(2, 1)/norm(mgm(:, 1))], [0, mgm(3, 1)/norm(mgm(:, 1))], '-o', 'LineWidth', 2);
% Animation loop
for i = 1:num_vectors
% Update the magnetic field vector plot
set(b_vector, 'XData', [0, mgm(1, i)/norm(mgm(:, i))], ...
'YData', [0, mgm(2, i)/norm(mgm(:, i))], ...
'ZData', [0, mgm(3, i)/norm(mgm(:, i))]);
% Pause to create the animation effect
pause(1/50); % Adjust the pause duration as needed
% Update the title to indicate the time step or vector index
figure(5)
title(['Celestial Sphere with Magnetic Field Vectors - Frame ' num2str(i)]);
end