Untitled
unknown
matlab
2 years ago
2.7 kB
6
Indexable
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
Editor is loading...
Leave a Comment