Failure in initial objective function evaluation in FMINCON function

hi

I have a code with some functions but when I run the code error is accured!

the code is:

clc
clear all
close all
Ts = 0.01;
Tf = 10;
t = 0:Ts:Tf;
N = numel(t);
x = zeros(6,N); 
u = zeros(3,N);
y = zeros(3,N);
                               
A = @(x) [0 0 0 1 0 0
          0 0 0 0 1 0
          0 0 0 0 0 1
          0 0 0 (1.0e5*(cos(x(2))*(cos(x(3))*(5.6e-4*x(6)*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + (x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(5) + x(6)) + (0.13*x(5) + 0.13*x(6))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.025*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.025*x(4)*x(5)*sin(conj(x(2))) - 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6)) + 5.6e-4*x(4)*x(5)*cos(conj(x(2)))*sin(conj(x(3))) - 5.6e-4*x(4)*x(5)*cos(conj(x(3)))*sin(conj(x(2)))) - 1.0*sin(x(3))*(6.8e-3*x(6)*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(x(5) + x(6))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.8e-3*x(4)*x(5)*cos(conj(x(2)))*cos(conj(x(3))) + 6.8e-3*x(4)*x(5)*sin(conj(x(2)))*sin(conj(x(3)))) + (0.13*x(5) + 0.13*x(6))*(0.19*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.19*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.075*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.075*x(4)*x(5)*sin(conj(x(2))) - 0.025*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.025*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.025*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.025*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6))) + sin(x(2))*(cos(x(3))*(6.8e-3*x(6)*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(x(5) + x(6))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.8e-3*x(4)*x(5)*cos(conj(x(2)))*cos(conj(x(3))) + 6.8e-3*x(4)*x(5)*sin(conj(x(2)))*sin(conj(x(3)))) + sin(x(3))*(5.6e-4*x(6)*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + (x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(5) + x(6)) + (0.13*x(5) + 0.13*x(6))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.025*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.025*x(4)*x(5)*sin(conj(x(2))) - 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6)) + 5.6e-4*x(4)*x(5)*cos(conj(x(2)))*sin(conj(x(3))) - 5.6e-4*x(4)*x(5)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.017*x(4)*x(5)*cos(conj(x(2))))))/(x(4)*(7.5e3*cos(conj(x(2)))*cos(x(2)) + 922.0*sin(conj(x(2)))*sin(x(2)) + 2.5e3*cos(conj(x(2)))*cos(x(2))*cos(x(3)) + 2.5e3*cos(conj(x(2)))*sin(x(2))*sin(x(3)) + 2.5e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2)) + 2.5e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*sin(x(2))*sin(x(3)) + 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*sin(x(3)) - 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(3))*sin(x(2)) - 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(2))*sin(x(3)) + 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(3))*sin(x(2)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*sin(x(2))*sin(x(3)) + 33.0)) 0 0
          0 0 0 0 ((8.5e4*(1.1*cos(conj(x(2))) + 0.3*sin(x(3))*(0.65*cos(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + (0.13*x(5) + 0.13*x(6))*(0.65*x(5) + 0.65*x(6)) + (0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.65*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.4*cos(conj(x(2)))*sin(conj(x(3))) - 6.4*cos(conj(x(3)))*sin(conj(x(2))) + 0.19*x(4)^2*cos(conj(x(2)))*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.3*cos(x(3))*(6.4*cos(conj(x(2)))*cos(conj(x(3))) - 0.65*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.65*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 6.4*sin(conj(x(2)))*sin(conj(x(3))) + 0.19*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 7.6e-3*x(4)^2*cos(conj(x(2)))*sin(conj(x(2))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) - (5.0e3*(25.0*cos(x(3)) + 17.0)*((6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3))/x(5) 0
          0 0 0 0 0 -((5.0e3*(25.0*cos(conj(x(3))) + 17.0)*(1.1*cos(conj(x(2))) + 0.3*sin(x(3))*(0.65*cos(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + (0.13*x(5) + 0.13*x(6))*(0.65*x(5) + 0.65*x(6)) + (0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.65*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.4*cos(conj(x(2)))*sin(conj(x(3))) - 6.4*cos(conj(x(3)))*sin(conj(x(2))) + 0.19*x(4)^2*cos(conj(x(2)))*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.3*cos(x(3))*(6.4*cos(conj(x(2)))*cos(conj(x(3))) - 0.65*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.65*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 6.4*sin(conj(x(2)))*sin(conj(x(3))) + 0.19*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 7.6e-3*x(4)^2*cos(conj(x(2)))*sin(conj(x(2))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) - (1.0e3*(122.0*cos(conj(x(3))) + 133.0*cos(x(3)) + 288.0*cos(conj(x(3)))*cos(x(3)) + 299.0*sin(conj(x(3)))*sin(x(3)) + 211.0)*((6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3))/x(6)];
      
      B = @(x) [0 0 0
          0 0 0
          0 0 0
          1.0e5/(7.5e3*cos(conj(x(2)))*cos(x(2)) + 922.0*sin(conj(x(2)))*sin(x(2)) + 2.5e3*cos(conj(x(2)))*cos(x(2))*cos(x(3)) + 2.5e3*cos(conj(x(2)))*sin(x(2))*sin(x(3)) + 2.5e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2)) + 2.5e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*sin(x(2))*sin(x(3)) + 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*sin(x(3)) - 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(3))*sin(x(2)) - 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(2))*sin(x(3)) + 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(3))*sin(x(2)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*sin(x(2))*sin(x(3)) + 33.0) 0 0
          0 8.5e4/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) -(5.0e3*(25.0*cos(x(3)) + 17.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3)
          0 -(5.0e3*(25.0*cos(conj(x(3))) + 17.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) (1.0e3*(122.0*cos(conj(x(3))) + 133.0*cos(x(3)) + 288.0*cos(conj(x(3)))*cos(x(3)) + 299.0*sin(conj(x(3)))*sin(x(3)) + 211.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3)];
fc = @(x,u) A(x)*x+B(x)*u;
f = @(x,u) x + Ts*fc(x,u);
Np = 5;
Uopt = zeros(3,Np);
Yref = ones(3,N+Np+1);
for i =1:N-1
    
    FCN = @(U) Cost (x(:,i) , U(:,i) , Yref(:,i+1:i+Np) , Np , Ts)
    Uopt = fmincon(FCN ,Uopt , [],[]);
    u(:,i) = Uopt(:,1);
   x(:,i+1) = f(x(:,i) , u(:,i));
   y(i+1) = h(x(:,i+1));
   
end
 plot(t,y)

cost function is :

  function J = Cost (xi , U , Yref , Np , Ts)
 %%% U = [u(i) u(i+1)   u(i+Nc-1)]
A = @(x) [0 0 0 1 0 0
          0 0 0 0 1 0
          0 0 0 0 0 1
          0 0 0 (1.0e5*(cos(x(2))*(cos(x(3))*(5.6e-4*x(6)*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + (x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(5) + x(6)) + (0.13*x(5) + 0.13*x(6))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.025*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.025*x(4)*x(5)*sin(conj(x(2))) - 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6)) + 5.6e-4*x(4)*x(5)*cos(conj(x(2)))*sin(conj(x(3))) - 5.6e-4*x(4)*x(5)*cos(conj(x(3)))*sin(conj(x(2)))) - 1.0*sin(x(3))*(6.8e-3*x(6)*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(x(5) + x(6))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.8e-3*x(4)*x(5)*cos(conj(x(2)))*cos(conj(x(3))) + 6.8e-3*x(4)*x(5)*sin(conj(x(2)))*sin(conj(x(3)))) + (0.13*x(5) + 0.13*x(6))*(0.19*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.19*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.075*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.075*x(4)*x(5)*sin(conj(x(2))) - 0.025*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.025*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.025*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.025*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6))) + sin(x(2))*(cos(x(3))*(6.8e-3*x(6)*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(x(5) + x(6))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.8e-3*x(4)*x(5)*cos(conj(x(2)))*cos(conj(x(3))) + 6.8e-3*x(4)*x(5)*sin(conj(x(2)))*sin(conj(x(3)))) + sin(x(3))*(5.6e-4*x(6)*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + (x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(6.3e-3*x(5) + 6.3e-3*x(6)) - 1.0*(6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(5) + x(6)) + (0.13*x(5) + 0.13*x(6))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.025*x(4)*sin(conj(x(2)))*conj(x(5)) - 0.025*x(4)*x(5)*sin(conj(x(2))) - 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(5)) + 0.011*x(4)*cos(conj(x(2)))*sin(conj(x(3)))*conj(x(6)) - 0.011*x(4)*cos(conj(x(3)))*sin(conj(x(2)))*conj(x(6)) + 5.6e-4*x(4)*x(5)*cos(conj(x(2)))*sin(conj(x(3))) - 5.6e-4*x(4)*x(5)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.017*x(4)*x(5)*cos(conj(x(2))))))/(x(4)*(7.5e3*cos(conj(x(2)))*cos(x(2)) + 922.0*sin(conj(x(2)))*sin(x(2)) + 2.5e3*cos(conj(x(2)))*cos(x(2))*cos(x(3)) + 2.5e3*cos(conj(x(2)))*sin(x(2))*sin(x(3)) + 2.5e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2)) + 2.5e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*sin(x(2))*sin(x(3)) + 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*sin(x(3)) - 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(3))*sin(x(2)) - 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(2))*sin(x(3)) + 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(3))*sin(x(2)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*sin(x(2))*sin(x(3)) + 33.0)) 0 0
          0 0 0 0 ((8.5e4*(1.1*cos(conj(x(2))) + 0.3*sin(x(3))*(0.65*cos(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + (0.13*x(5) + 0.13*x(6))*(0.65*x(5) + 0.65*x(6)) + (0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.65*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.4*cos(conj(x(2)))*sin(conj(x(3))) - 6.4*cos(conj(x(3)))*sin(conj(x(2))) + 0.19*x(4)^2*cos(conj(x(2)))*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.3*cos(x(3))*(6.4*cos(conj(x(2)))*cos(conj(x(3))) - 0.65*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.65*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 6.4*sin(conj(x(2)))*sin(conj(x(3))) + 0.19*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 7.6e-3*x(4)^2*cos(conj(x(2)))*sin(conj(x(2))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) - (5.0e3*(25.0*cos(x(3)) + 17.0)*((6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3))/x(5) 0
          0 0 0 0 0 -((5.0e3*(25.0*cos(conj(x(3))) + 17.0)*(1.1*cos(conj(x(2))) + 0.3*sin(x(3))*(0.65*cos(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + (0.13*x(5) + 0.13*x(6))*(0.65*x(5) + 0.65*x(6)) + (0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.65*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + 6.4*cos(conj(x(2)))*sin(conj(x(3))) - 6.4*cos(conj(x(3)))*sin(conj(x(2))) + 0.19*x(4)^2*cos(conj(x(2)))*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) + (6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.3*cos(x(3))*(6.4*cos(conj(x(2)))*cos(conj(x(3))) - 0.65*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.65*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.65*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 6.4*sin(conj(x(2)))*sin(conj(x(3))) + 0.19*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 7.6e-3*x(4)^2*cos(conj(x(2)))*sin(conj(x(2))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) - (1.0e3*(122.0*cos(conj(x(3))) + 133.0*cos(x(3)) + 288.0*cos(conj(x(3)))*cos(x(3)) + 299.0*sin(conj(x(3)))*sin(x(3)) + 211.0)*((6.8e-3*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 6.8e-3*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(x(4)*cos(conj(x(2)))*cos(conj(x(3))) + x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 1.0*(x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 1.0*x(4)*cos(conj(x(3)))*sin(conj(x(2))))*(5.6e-4*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 5.6e-4*x(4)*sin(conj(x(2)))*sin(conj(x(3)))) - 0.084*sin(conj(x(3)))*(0.3*x(4)^2*cos(conj(x(2)))^2 + 0.3*x(5)^2) + 0.83*cos(conj(x(2)))*cos(conj(x(3))) - 1.0*(0.13*x(4)*cos(conj(x(2)))*cos(conj(x(3))) + 0.13*x(4)*sin(conj(x(2)))*sin(conj(x(3))))*(0.084*x(4)*cos(conj(x(2)))*sin(conj(x(3))) - 0.084*x(4)*cos(conj(x(3)))*sin(conj(x(2)))) + 0.83*sin(conj(x(2)))*sin(conj(x(3))) + 0.025*x(4)^2*cos(conj(x(2)))*cos(conj(x(3)))*sin(conj(x(2)))))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3))/x(6)];
      
      B = @(x) [0 0 0
          0 0 0
          0 0 0
          1.0e5/(7.5e3*cos(conj(x(2)))*cos(x(2)) + 922.0*sin(conj(x(2)))*sin(x(2)) + 2.5e3*cos(conj(x(2)))*cos(x(2))*cos(x(3)) + 2.5e3*cos(conj(x(2)))*sin(x(2))*sin(x(3)) + 2.5e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2)) + 2.5e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*cos(conj(x(2)))*cos(conj(x(3)))*sin(x(2))*sin(x(3)) + 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*sin(x(3)) - 688.0*cos(conj(x(2)))*sin(conj(x(3)))*cos(x(3))*sin(x(2)) - 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(2))*sin(x(3)) + 688.0*cos(conj(x(3)))*sin(conj(x(2)))*cos(x(3))*sin(x(2)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*cos(x(2))*cos(x(3)) + 1.2e3*sin(conj(x(2)))*sin(conj(x(3)))*sin(x(2))*sin(x(3)) + 33.0) 0 0
          0 8.5e4/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) -(5.0e3*(25.0*cos(x(3)) + 17.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3)
          0 -(5.0e3*(25.0*cos(conj(x(3))) + 17.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3) (1.0e3*(122.0*cos(conj(x(3))) + 133.0*cos(x(3)) + 288.0*cos(conj(x(3)))*cos(x(3)) + 299.0*sin(conj(x(3)))*sin(x(3)) + 211.0))/(17.0*cos(x(3)) + 1.7e3*cos(conj(x(3)))*cos(x(3)) + 4.9e3*sin(conj(x(3)))*sin(x(3)) + 2.1e3)];
fc = @(x,u) A(x)*x+B(x)*u;
f = @(x,u) x + Ts*fc(x,u);
x = zeros(6 ,Np);
x = [xi x];
y = zeros(3 , Np);
for i=1:Np
    x(: , i+1) = f(x(: , i) , U(:,i));
    y(:,i) = h(x(: , i+1));
      
    
end
R = 1;
J = sum ((Yref-y).^2) + R*sum (U.^2);
end

and h is:

function y=h(x)
y=[x(1);x(2);x(3)];
end

but I got this error:

FCN =

function_handle with value:

@(U)Cost(x(:,i),U(:,i),Yref(:,i+1:i+Np),Np,Ts)

Index exceeds matrix dimensions.

Error in Cost (line 23)

x(: , i+1) = f(x(: , i) , U(:,i));

Error in MPC>@(U)Cost(x(:,i),U(:,i),Yref(:,i+1:i+Np),Np,Ts)

Error in fmincon (line 536)

initVals.f = feval(funfcn{3},X,varargin{:});

Error in MPC (line 34)

Uopt = fmincon(FCN ,Uopt , [],[]);

Caused by:

Failure in initial objective function evaluation. FMINCON cannot continue.

Answer 1

[Matt J]

This loop does not make sense, because in the workspace of Cost(), the variable U only has 1 column.

for i=1:Np
    
    x(: , i+1) = f(x(: , i) , U(:,i));
    y(:,i) = h(x(: , i+1));

end

[john white]

but I want to U(:,i) elements get the values of u(:,i) after the Uopt update the u(:,i)

[Matt J]

But as you've written it, FCN depends only on the i-th column of U. Perhaps you meant to write,

FCN = @(Z) Cost (x(:,i) , Z , Yref(:,i+1:i+Np) , Np , Ts)

[john white]

and then how to write the Z for my purpose?

[Matt J]

I don't know what your purpose is - only you know the form your objective function is supposed to take. However, if your intention was to have all columns of the unknown matrix available for use inside Cost(), then rewriting FCN as,

FCN = @(Z) Cost (x(:,i) , Z , Yref(:,i+1:i+Np) , Np , Ts)

will achieve that.

[john white]

you know at the first time I have wriiten my code as you mentioned, I mean FCN = @(U) Cost (x(:,i),U,Yref(:,i+1:i+Np),Np,Ts)

But I got the error about Index of matrix in for loop

x(:,i+1) = f(x(:,i) ,U);

that I changed the code to

x(: , i+1) = f(x(: , i) , U(:,i));

and that error is solved and then I rewrite the FCN as new form

FCN = @(U) Cost (x(:,i) , U(:,i) , Yref(:,i+1:i+Np) , Np , Ts)

but I got this error about Failure in initial objective

actually my purpose is

the code get x nad u and Uopt be the fmincon (minimizing) the cost function , the cost function get u , x ,yref,Np and Ts and y is x dependent and in the loop all of cost function, y and Uopt and x etc will be update