Subscripted assignment dimension mismatch.

clc
clear
% define model parametres
Kp=3.3; Ao1=0.1781; Ao2=0.1781; At1=15.5179;At2=15.5179; g=981;Le1=15; Le2=15;
Ac=[(-Ao1*sqrt(g/(2*Le1))/At1) 0;(Ao1*sqrt(g/(2*Le2))/At2)  (-Ao2*sqrt(g/(2*Le2))/At2)];
Bc=[Kp/At1;0];
Cc=[0 1];
%discrete time model
dt=0.01; %sampling time
sys=ss(Ac,Bc,Cc,0);
ds=c2d(sys,dt);
Ad=ds.a; Bd=ds.b; Cd= ds.c;
N = 9;
u_max = 18;
u_min = 2;
Atid=[Ad zeros(2,1);eye(1) zeros(1,2)];
Btid=[Bd;zeros(1)];
Ctid=[Cd zeros(1)];
% Number of states and control signals
nx = size(Atid,2);
nu = size(Btid,2);
% LQR values
Q_mat =[Ctid'*Ctid];
S = eye(nu);
R = 0.00001;
[~,P,~] = dlqr(Atid,Btid,Q_mat,R);
%% Reference Parameters
% Initilize matrices
% xf = zeros(1:nx)';
xf = [15 15 0]';
% Initilize QP matrices
Psi = zeros(nx*N, nx);
Omega = zeros(nx*N, nu*N);
Phi = zeros(nu*N, nx*N);
H11 = zeros(nu*N,nu*N);
H22 = zeros(nx*N,nx*N);
H33 = zeros(nu*N,nu*N);
H44 = [zeros(nx,nx*(N-1)) eye(nx)];
% Construct QP matrices
for i = 1:N
    Psi( (i-1)*nx+1:i*nx, :) = Atid^i;
    Phi( (i-1)*nu+1:i*nu, (i-1)*nx+1:i*nx) = Ctid
    H11( (i-1)*nu+1:i*nu, (i-1)*nu+1:i*nu) = R;
    
    if i<N
        H22( (i-1)*nx+1:i*nx, (i-1)*nx+1:i*nx ) = Q_mat;
        H33( (i-1)*nu+1:i*nu, (i-1)*nu+1:i*nu ) = S; 
    else
        H22( (i-1)*nx+1:i*nx, (i-1)*nx+1:i*nx ) = P;
    end
    
    for j=1:i
       Omega( (i-1)*nx+1:i*nx, (j-1)*nu+1:j*nu ) =[Atid^(i-j)*Btid] ;   
    end
end