%Her lages H, E og F funksjonen som trengs for å løse problemet som et QP-problem
function [H,E,F]=lagHEF(A,B,C,Q,R,S,N)

dimu=length(B(1,:));    %systemets dimensjoner
dimx=length(A(1,:));

Qbar=dlyap(A',C'*Q*C);  %lyaponov ligningen

H=0; H1=0; H2=0;
for rad=1:N             %lager H
    for kolonne=1:N
        if kolonne >=rad
            H1((rad-1)*dimu+1:rad*dimu,(kolonne-1)*dimu+1:kolonne*dimu)=B'*(A')^(kolonne-rad)*Qbar*B;
        else
            H1((rad-1)*dimu+1:rad*dimu,(kolonne-1)*dimu+1:kolonne*dimu)=B'*Qbar*A^(rad-kolonne)*B;
        end
        if rad==kolonne
            H2((rad-1)*dimu+1:rad*dimu,(kolonne-1)*dimu+1:kolonne*dimu)=R+2*S;
        elseif rad==kolonne-1
            H2((rad-1)*dimu+1:rad*dimu,(kolonne-1)*dimu+1:kolonne*dimu)=-S;
        elseif rad==kolonne+1
            H2((rad-1)*dimu+1:rad*dimu,(kolonne-1)*dimu+1:kolonne*dimu)=R+2*S;
        end
    end
end
H=H1+H2;
H=(H+H')/2;
E=0; F=0;
F(1:dimu,1:dimu)=S;
for rad=1:N
    E((rad-1)*dimu+1:rad*dimu,1:dimx)=B'*Qbar*A^rad;            %lager G og F
    if rad>1
        F((rad-1)*dimu+1:rad*dimu,1:dimu)=zeros(dimu);
    end
end
