Untitled

clc;
clear;
close all;
% Constants
mu = 3.986e14;           % Gravitational parameter in m^3/s^2

% Satellite properties
altitude = 400000;        % Altitude of the orbit in meters
r = altitude + 6371000;   % Total distance from the center of the Earth

% Desired altitude
desired_altitude = altitude;

% MPC parameters
N = 10;  % Prediction horizon
Q = diag([1, 1, 1, 1]);  % State cost
R = 1;    % Control cost

% Initial conditions
initial_position = [r, 0];   % Initial position
initial_velocity = [0, sqrt(mu/r)];   % Initial velocity (circular orbit velocity)

% Simulation time
time = 0:1:10000; % Time vector in seconds
% Define state-space matrices for linearized system
A = [0 0 1 0; 
    0 0 0 1; 
    3*mu/r^5*r^2 3*mu/r^5*r*0 0 2*mu/r^3*r; 
    3*mu/r^5*r*0 3*mu/r^5*0^2 -2*mu/r^3*0 0];
B = [0; 
    0; 
    -mu/r^3; 
    0];

% Solving the equations of motion
[t, state] = ode45(@(t, y) satelliteOrbit(t, y, mu), time, ...
    [initial_position, initial_velocity]);

% Extracting altitude
altitude = vecnorm(state(:, 1:2), 2, 2) - 6371000; % Subtracting Earth's radius


% MPC simulation
num_steps = length(t);  % Use t instead of time
state_trajectory = zeros(num_steps, 4);

for k = 1:num_steps
    % Current state
    current_state = [state(k, 1:2), state(k, 3:4)];

    % MPC optimization problem
    H = 2 * (B'*Q*B + R);
    f = 2 * B'*Q*(current_state' - [r; 0; 0; sqrt(mu/r)]);

    % Solve quadratic programming problem
    delta_v = quadprog(H, f);

    % Update the state using the nonlinear dynamics
    [~, updated_state] = ode45(@(t, y) satelliteOrbit(t, y, mu), [0 1], current_state' + B*delta_v);

    % Use the final state after 1 second as the updated state
    current_state = updated_state(end, :)';

    % Store the updated state
    state_trajectory(k, :) = current_state';
    dd(k) = delta_v;
end


% Extracting altitude for MPC
altitude_mpc = vecnorm(state_trajectory(:, 1:2), 2, 2) - 6371000; % Subtracting Earth's radius

% Plotting altitude as a function of time
figure;
plot(t, altitude, 'LineWidth', 2, 'DisplayName', 'Without MPC');
hold on;
plot(t, altitude_mpc, 'LineWidth', 2, 'DisplayName', 'With MPC');
title('Satellite Altitude vs Time with MPC');
xlabel('Time (seconds)');
ylabel('Altitude (meters)');
legend('Location', 'best');




% Function to define the system of differential equations
function dydt = satelliteOrbit(~, y, mu)
    r = norm(y(1:2)); % Distance from the center of the Earth
    acceleration = -mu / r^3; % Gravitational acceleration

    % Equations of motion
    dydt = [y(3); y(4); acceleration * y(1); acceleration * y(2)];
end