How to add a terminal cost to the objective function

[BolunDai0216]

Hi, I am trying to add a terminal cost to the objective function that is only state dependent, I tried doing this:

function objectiveFunctions()
    R = [0, 0, 0, 0, 0];
    Q = [10, 10, 10, 10, 10, 10, 10, 10, 10, 10]
    tar = [...] % Some Value
    F = u1*R(1)*u1 + u2*R(2)*u2 + u3*R(3)*u3 + u4*R(4)*u4 + u5*R(5)*u5;
    F = F + (q1-tar(1))*Q(1)*(q1-tar(1)) + (q2-tar(2))*Q(2)*(q2-tar(2)) + (q3-tar(3))*Q(3)*(q3-tar(3)) + (q4-tar(4))*Q(4)*(q4-tar(4)) + (q5-tar(5))*Q(5)*(q5-tar(5));
    F = F + (dq1-tar(6))*Q(6)*(dq1-tar(6)) + (dq2-tar(7))*Q(7)*(dq2-tar(7)) + (dq3-tar(8))*Q(8)*(dq3-tar(8)) + (dq4-tar(9))*Q(9)*(dq4-tar(9)) + (dq5-tar(10))*Q(10)*(dq5-tar(10));
    [f, ~, fz, fzi, ~]  = computeGradients(F,z,empty);

    matlabFunction(f,fz,fzi,...
        'file','autoGen_obj_quadraticCost2.m',...
        'vars',{'q1','q2','q3','q4','q5','dq1','dq2','dq3','dq4','dq5','u1','u2','u3','u4','u5'});        
end

and in the objective function I gave to the problem I did this

dObj_end = autoGen_obj_quadraticCost2(x(1,end),x(2,end),x(3,end),x(4,end),x(5,end),x(6,end),x(7,end),x(8,end),x(9,end),x(10,end),u(1,end),u(2,end),u(3,end),u(4,end),u(5,end));
dObj_pre = autoGen_obj_quadraticCost(x(1,1:end-1),x(2,1:end-1),x(3,1:end-1),x(4,1:end-1),x(5,1:end-1),x(6,1:end-1),x(7,1:end-1),x(8,1:end-1),x(9,1:end-1),x(10,1:end-1),u(1,1:end-1),u(2,1:end-1),u(3,1:end-1),u(4,1:end-1),u(5,1:end-1));
dObj = [dObj_pre, dObj_end];

Where autoGen_obj_quadraticCost is the similar to autoGen_obj_quadraticCost2 with the only difference being the R matrix is not zeros. I tried this but at the end the control just blows up since I suppose the cost function defined in autoGen_obj_quadraticCost2 also is used to calculate the cost on states other than the last state.

Can anyone help me on this matter?

Thanks in advance!