lsim() trims time vector in an odd fashion

clc
close all
clear all

%% System parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t_f = 5*2;
N = 50*2;
h = t_f/N;
t_vec = zeros(N + 1, 1);
for i = 2:N + 1
    t_vec(i) = t_vec(i - 1) + h;
end
alpha = 0.2;
x_0 = [-0.8; 1];
continousSystem = ss([0, 1; 2, -3], [0;  1], [], []);

%% Discretization of the continouus problem %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Using ZOH
discreteSystemZOH = c2d(continousSystem, h, 'zoh');
A_D = discreteSystemZOH.A;
B_D = discreteSystemZOH.B;

% Using Forward Rectangular Method
A_D_prime = diag(ones(2, 1)) + h*continousSystem.A;
B_D_prime = h*continousSystem.B;

%% Construct and solve the optimal control problem
A_D_list = {A_D, A_D_prime};
B_D_list = {B_D, B_D_prime};

% H matrix
dim_H = [3*(N + 1), 3*(N + 1)];
H = zeros(dim_H);
H_hat = [1, 0,     0;
         0, 1,     0;
         0, 0, alpha];
for j = 1:3:dim_H(1)
    H(j:j + 3 - 1, j:j + 3 - 1) = H_hat;
end

% y_0 vector (initial guess)
dim_y_0 = [3*(N + 1), 1];
y_0 = zeros(dim_y_0);
y_0(1:2) = x_0;

y_vals = zeros(dim_y_0, 2);
f_vals = zeros(2, 1);
f_vals = zeros(2, 1);
for i = 1:2
    clear A_eq B_eq

    % A_eq and B_eq matrices
    dim_A_eq = [2*(N + 1), 3*(N + 1)];
    dim_y = [3*(N + 1), 1];
    dim_B_eq = [2*(N + 1), 1];
    A_eq = zeros(dim_A_eq);
    B_eq = zeros(dim_B_eq);
    A_eq(1:2, 1:2) = diag(ones(2, 1));
    indent_counter = 0;
    for j = 3:2:dim_A_eq(1)
        A_eq(j:j + 2 - 1, indent_counter*3 + 1:indent_counter*3 + 1 + 2 - 1) = -A_D_list{i};
        A_eq(j:j + 2 - 1, indent_counter*3 + 1 + 2) = -B_D_list{i};
        A_eq(j:j + 2 - 1, indent_counter*3 + 1 + 2 + 1:indent_counter*3 + 2 + 1 + 1 + 2 -1) = diag(ones(2, 1));
        ++indent_counter;
    end
    B_eq(1:2) = x_0;

    % Solve the optimization problem
    [y, fval, info, lambda] = qp(y_0, H, [], A_eq, B_eq);
    y_val(:, i) = y;
    f_vals(i) = fval;
end

%% Create plots for the optimal inputs %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
f1 = figure;
stairs(t_vec, y_val(3:3:end, 1), 'b-')
hold on
stairs(t_vec, y_val(3:3:end, 2), 'r-')
legend('$u(t)$', '$u(t)^\prime$');
xlabel('t [s]');
grid on

%cleanfigure();
%matlab2tikz('plot_optinputs.tex',
%            'figurehandle', f1,
%            'width', '8cm',
%            'height', '4.75cm',
%            'parseStrings', false,
%            'encoding', 'UTF-8',
%            'showInfo', false);

%'\pgfplotsset{grid style={dotted, dash pattern=on 0pt off 3\pgflinewidth,line width=0.3pt,lightgray}}'

%% Simulate the exact discrete time system with the optimal input %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[y_out, t_out, x_out] = lsim(discreteSystemZOH, y_val(3:3:end, 1), t_vec(:,1), x_0);
f2 = figure;
subplot(1, 2, 1);
stairs(t_out, y_out(:, 1), 'b-');
hold on
stairs(t_vec, y_val(3:3:end, 1), 'r-');
grid on
legend('$x_1(t)$', '$u(t)$');
xlabel('t [s]');
subplot(1, 2, 2);
stairs(t_out, y_out(:, 2), 'b-');
hold on
stairs(t_vec, y_val(3:3:end, 1), 'r-');
grid on
legend('$x_2(t)$', '$u(t)$');
xlabel('t [s]');

%cleanfigure();
%matlab2tikz('plot_optstates.tex',
%            'figurehandle', f2,
%            'width', '14cm',
%            'height', '4.5cm',
%            'parseStrings', false,
%            'encoding', 'UTF-8',
%            'showInfo', false);