simple_harmonic_oscillator.m

clear all; close all; clc;
format longG;

k = 1
M = 1
w = sqrt(k/M)

r0 = 0
v0 = 1

N = 100
T = pi / 12

% state-space representation and ZOH discretization
A = [ 0 1; -w^2 0 ]
A_d = expm(A*T)

% number of states
n_x = size(A_d, 1);

[Aeq, beq] = build_dynamics_constraints(A_d, N, r0, v0)

options = optimoptions('fmincon', ...
    'Algorithm', 'interior-point', ...
    'Display', 'iter', ...
    'SpecifyConstraintGradient', false, ...
    'HessianApproximation', 'bfgs');

obj = @(x) 0;

x0_guess = repmat([r0; v0], N + 1, 1);

[x_opt, f_opt] = fmincon(obj, x0_guess, [], [], Aeq, beq, [], [], [], options);

r_traj = x_opt(1:n_x:end);
v_traj = x_opt(2:n_x:end);

figure('Position', [100, 100, 1200, 400]);
t = (0:N) * T;

subplot(1,3,1);
plot(t, r_traj, 'b-', 'LineWidth', 1.5);
xlabel('Time (s)'); ylabel('Position r'); grid on;

subplot(1,3,2);
plot(t, v_traj, 'r-', 'LineWidth', 1.5);
xlabel('Time (s)'); ylabel('Velocity v'); grid on;

subplot(1,3,3);
plot(r_traj, v_traj, 'k-', 'LineWidth', 1.5);
xlabel('Position r'); ylabel('Velocity v');
title('Phase Portrait'); grid on; axis equal;

function [Aeq, beq] = build_dynamics_constraints(A_d, N, r0, v0)
    n_x = size(A_d, 1);
    n_eq = (N + 1) * n_x;  % N dynamics + 1 IC
    n_vars = (N + 1) * n_x;

    % Initialize sparse constraint matrix
    Aeq = sparse(n_eq, n_vars);
    beq = zeros(n_eq, 1);

    % Initial condition: x_0 = [r0; v0]
    Aeq(1:n_x, 1:n_x) = eye(n_x);
    beq(1:n_x) = [r0; v0];

    % Dynamics: x_{k+1} = A_d * x_k
    for k = 0:N-1
        rows = n_x + k*n_x + (1:n_x);
        cols_xk = k*n_x + (1:n_x);
        cols_xkp1 = (k+1)*n_x + (1:n_x);

        Aeq(rows, cols_xk) = -A_d;
        Aeq(rows, cols_xkp1) = eye(n_x);
    end
end