Supplied objective function must return a scalar value. ERROR in fmincon

hi I write this code and i'm using fmincon to minimize the K function, this is the code:

clc;
clear;
close all;
Ts = 0.01;
Tf = 10;
t = 0:Ts:Tf;
Np = 5;
N = numel(t);
Yref = ones(3 , N);
y = zeros(3,N);
u = zeros(3,N);
U = zeros(3,N);
theta1=zeros(N,1);
theta2=zeros(N,1);
theta3=zeros(N,1);
theta_dot1=zeros(N,1);
theta_dot2=zeros(N,1);
theta_dot3=zeros(N,1);
theta1(1) = 0;
theta2(1) = 0;
theta3(1) = 0;
theta_dot1(1) = 0.01;
theta_dot2(1) = 0.01;
theta_dot3(1) = 0.01;
theta_vec(:,1)=[theta1(1);theta2(1);theta3(1);theta_dot1(1);theta_dot2(1);theta_dot3(1)];
for i = 1:N
    C=[(1.0*cos(theta2(i))*(0.36*cos(conj(theta2(i))) + cos(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + sin(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.6e-17*cos(theta3(i))*(2.2e-18*cos(conj(theta2(i))) + 4.4e-18*cos(conj(theta2(i)))*cos(conj(theta3(i)))) + 3.8e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.8e-3*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 1.2e-34*cos(conj(theta2(i)))*sin(conj(theta3(i)))*sin(theta3(i))) + sin(theta2(i))*(0.04*sin(conj(theta2(i))) + 1.0*cos(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - sin(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i))))) + 0.066)  (1.0*cos(theta2(i))*(9.3e-19*sin(conj(theta3(i)))*sin(theta3(i)) + 2.6e-17*cos(theta3(i))*(0.036*cos(conj(theta3(i))) + 0.018) - cos(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19) - 5.7e-19) - sin(theta2(i))*(6.2e-21*sin(conj(theta3(i)))*cos(theta3(i)) - sin(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19)))  (-1.0*cos(theta2(i))*(4.6e-19*cos(theta3(i)) + 3.7e-19));(0.21*cos(theta3(i))*(2.2e-18*cos(conj(theta2(i))) + 4.4e-18*cos(conj(theta2(i)))*cos(conj(theta3(i)))) - 1.2e-16*cos(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.2e-16*sin(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 5.7e-19*cos(conj(theta2(i))) + 4.6e-19*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 9.4e-19*cos(conj(theta2(i)))*sin(conj(theta3(i)))*sin(theta3(i)))  (3.8e-3*cos(conj(theta3(i))) + 7.7e-3*sin(conj(theta3(i)))*sin(theta3(i)) + 0.21*cos(theta3(i))*(0.036*cos(conj(theta3(i))) + 0.018) + 1.2e-16*cos(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19) + 0.36)  (- 3.8e-3*cos(theta3(i)) - 3.0e-3);(- 3.7e-19*cos(conj(theta2(i))) - 4.6e-19*cos(conj(theta2(i)))*cos(conj(theta3(i))))  (- 3.8e-3*cos(conj(theta3(i))) - 3.0e-3) 3.0e-3];
E1=- 1.0*cos(theta2(i))*(4.3e-17*cos(conj(theta2(i))) - 0.036*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.2e-18*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.6e-17*cos(theta3(i))*(0.17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 2.0e-17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.6*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.6*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 2.2e-18*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.6e-34*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 1.2e-16*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + cos(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.6e-17*sin(theta3(i))*(0.17*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i)))) + 2.0e-17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + 1.6*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 1.6*cos(conj(theta3(i)))*sin(conj(theta2(i))) - 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 2.1e-17*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 2.2e-18*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 1.2e-16*(1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.1e-17*sin(conj(theta2(i)))*sin(conj(theta3(i))) + sin(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 0.67*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 9.4e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 7.9e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 4.6e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - sin(theta2(i))*(1.0*cos(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - sin(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.045*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i))));
E2=0.018*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 4.4e-18*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.28*cos(conj(theta2(i))) - 0.21*cos(theta3(i))*(0.17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 2.0e-17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.6*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.6*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 2.2e-18*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.2e-18*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.0*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 1.2e-16*cos(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.21*sin(theta3(i))*(0.17*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i)))) + 2.0e-17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + 1.6*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 1.6*cos(conj(theta3(i)))*sin(conj(theta2(i))) - 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.018*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.17*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 1.2e-16*sin(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 8.0e-19*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.31*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 7.1e-19*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 5.7e-35*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 3.8e-3*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)));
E3= (5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 2.2e-18*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 0.018*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.17*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.018*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 1.0*(1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.17*sin(conj(theta2(i)))*sin(conj(theta3(i))) - 1.4e-19*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) + 2.3e-19*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)));
E=[E1;E2;E3];
digits(2);
c_inv=inv(C);
V=c_inv*(-E);


    A=[0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1;0 0 0 V(1,1)/theta_dot1(i) 0 0;0 0 0 0 V(2,1)/theta_dot2(i) 0;0 0 0 0 0 V(3,1)/theta_dot3(i)];
    B=[0 0 0;0 0 0;0 0 0;c_inv(1,1) 0 0;0 c_inv(2,2) c_inv(2,3);0 c_inv(3,2) c_inv(3,3)];
  fc =  A*theta_vec(:,i)+B*u(:,i) ; 
  f = theta_vec(:,i) + Ts*fc ;
K = @(u) tee(theta_vec(:,i),Yref(:,i),Np,u(:,i),Ts);
U = fmincon(K,U,[],[]);

u(:,i+1) = U(:,i);
theta1(i+1) = f(1,1);
theta2(i+1) = f(2,1);
theta3(i+1) = f(3,1);
theta_dot1(i+1) = f(4,1);
theta_dot2(i+1) = f(5,1);
theta_dot3(i+1) = f(6,1);
y(:,i+1) = [theta1(i+1);theta2(i+1);theta3(i+1)];
theta_vec(:,i+1)=[theta1(i+1);theta2(i+1);theta3(i+1);theta_dot1(i+1);theta_dot2(i+1);theta_dot3(i+1)];
end

and tee is:

function J = tee(theta_veci,Yref,Np,ui,Ts)

u = zeros(3,Np);
u = [ui u];
theta_vec = zeros(6,Np);
theta_vec = [theta_veci theta_vec];
theta1(1) = theta_vec(1,1);
theta2(1) = theta_vec(2,1);
theta3(1) = theta_vec(3,1);
theta_dot1(1) = theta_vec(4,1);
theta_dot2(1) = theta_vec(5,1);
theta_dot3(1) = theta_vec(6,1);

for i =1:Np
    C=[(1.0*cos(theta2(i))*(0.36*cos(conj(theta2(i))) + cos(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + sin(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.6e-17*cos(theta3(i))*(2.2e-18*cos(conj(theta2(i))) + 4.4e-18*cos(conj(theta2(i)))*cos(conj(theta3(i)))) + 3.8e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.8e-3*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 1.2e-34*cos(conj(theta2(i)))*sin(conj(theta3(i)))*sin(theta3(i))) + sin(theta2(i))*(0.04*sin(conj(theta2(i))) + 1.0*cos(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - sin(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i))))) + 0.066)  (1.0*cos(theta2(i))*(9.3e-19*sin(conj(theta3(i)))*sin(theta3(i)) + 2.6e-17*cos(theta3(i))*(0.036*cos(conj(theta3(i))) + 0.018) - cos(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19) - 5.7e-19) - sin(theta2(i))*(6.2e-21*sin(conj(theta3(i)))*cos(theta3(i)) - sin(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19)))  (-1.0*cos(theta2(i))*(4.6e-19*cos(theta3(i)) + 3.7e-19));(0.21*cos(theta3(i))*(2.2e-18*cos(conj(theta2(i))) + 4.4e-18*cos(conj(theta2(i)))*cos(conj(theta3(i)))) - 1.2e-16*cos(theta3(i))*(3.8e-3*cos(conj(theta2(i))) + 3.1e-3*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 3.1e-3*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.2e-16*sin(theta3(i))*(5.1e-5*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 5.1e-5*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 5.7e-19*cos(conj(theta2(i))) + 4.6e-19*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 9.4e-19*cos(conj(theta2(i)))*sin(conj(theta3(i)))*sin(theta3(i)))  (3.8e-3*cos(conj(theta3(i))) + 7.7e-3*sin(conj(theta3(i)))*sin(theta3(i)) + 0.21*cos(theta3(i))*(0.036*cos(conj(theta3(i))) + 0.018) + 1.2e-16*cos(theta3(i))*(3.7e-19*cos(conj(theta3(i))) + 4.6e-19) + 0.36)  (- 3.8e-3*cos(theta3(i)) - 3.0e-3);(- 3.7e-19*cos(conj(theta2(i))) - 4.6e-19*cos(conj(theta2(i)))*cos(conj(theta3(i))))  (- 3.8e-3*cos(conj(theta3(i))) - 3.0e-3) 3.0e-3];
E1=- 1.0*cos(theta2(i))*(4.3e-17*cos(conj(theta2(i))) - 0.036*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.2e-18*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.6e-17*cos(theta3(i))*(0.17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 2.0e-17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.6*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.6*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 2.2e-18*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.6e-34*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 1.2e-16*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + cos(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 2.6e-17*sin(theta3(i))*(0.17*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i)))) + 2.0e-17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + 1.6*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 1.6*cos(conj(theta3(i)))*sin(conj(theta2(i))) - 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 2.1e-17*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 2.2e-18*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 1.2e-16*(1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.1e-17*sin(conj(theta2(i)))*sin(conj(theta3(i))) + sin(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 0.67*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 9.4e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 7.9e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 4.6e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 3.8e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - sin(theta2(i))*(1.0*cos(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - sin(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.045*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i))));
E2=0.018*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 4.4e-18*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.28*cos(conj(theta2(i))) - 0.21*cos(theta3(i))*(0.17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 2.0e-17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.6*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.6*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 2.2e-18*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 2.2e-18*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 1.0*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 1.2e-16*cos(theta3(i))*(2.1e-17*cos(conj(theta2(i))) + (theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.018*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 1.2e-3*theta_dot3(i)*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 1.0*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) - 3.8e-3*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 4.6e-19*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 3.8e-3*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 2.3e-19*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 1.9e-3*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*theta_dot2(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.21*sin(theta3(i))*(0.17*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i))))*(0.11*theta_dot2(i) - 0.11*theta_dot3(i) + 1.3e-17*theta_dot1(i)*cos(conj(theta2(i)))) + 2.0e-17*sin(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) + 0.17*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + 1.6*cos(conj(theta2(i)))*sin(conj(theta3(i))) + 1.6*cos(conj(theta3(i)))*sin(conj(theta2(i))) - 0.036*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i)))*sin(conj(theta3(i)))) + 0.17*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 0.018*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.17*sin(conj(theta2(i)))*sin(conj(theta3(i))) + 1.2e-16*sin(theta3(i))*(5.1e-5*theta_dot3(i)*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 1.0*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(1.1e-3*theta_dot2(i) - 1.1e-3*theta_dot3(i) + 1.4e-19*theta_dot1(i)*cos(conj(theta2(i)))) + (1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot2(i) - 1.0*theta_dot3(i) + 1.2e-16*theta_dot1(i)*cos(conj(theta2(i)))) + 5.1e-5*theta_dot1(i)*theta_dot2(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) - 5.1e-5*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 8.0e-19*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.31*theta_dot1(i)^2*cos(conj(theta2(i)))*sin(conj(theta2(i))) + 7.1e-19*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) - 5.7e-35*theta_dot2(i)*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot2(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i)))*conj(theta_dot3(i)) + 4.6e-19*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))*conj(theta_dot3(i)) - 3.8e-3*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)));
E3= (5.1e-5*theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 6.2e-21*theta_dot2(i)*sin(conj(theta3(i))) + 5.1e-5*theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i))))*(1.2e-16*theta_dot2(i)*cos(conj(theta3(i))) - 1.0*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i)))) - 2.2e-18*cos(conj(theta3(i)))*(0.21*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) - 0.21*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i))) - 0.018*(1.3e-17*theta_dot2(i)*cos(conj(theta3(i))) - 0.11*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.11*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) - 0.17*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 0.018*sin(conj(theta3(i)))*(0.21*theta_dot1(i)^2*cos(conj(theta2(i)))^2 + 0.21*theta_dot2(i)^2) - 1.0*(1.5e-19*theta_dot2(i)*cos(conj(theta3(i))) - 1.2e-3*theta_dot1(i)*cos(conj(theta2(i)))*cos(conj(theta3(i))) + 1.2e-3*theta_dot1(i)*sin(conj(theta2(i)))*sin(conj(theta3(i))))*(theta_dot1(i)*cos(conj(theta2(i)))*sin(conj(theta3(i))) - 1.2e-16*theta_dot2(i)*sin(conj(theta3(i))) + theta_dot1(i)*cos(conj(theta3(i)))*sin(conj(theta2(i)))) + 0.17*sin(conj(theta2(i)))*sin(conj(theta3(i))) - 1.4e-19*theta_dot1(i)*theta_dot2(i)*sin(conj(theta2(i))) + 2.3e-19*theta_dot1(i)*sin(conj(theta2(i)))*conj(theta_dot2(i)) + 3.8e-3*theta_dot1(i)^2*cos(conj(theta2(i)))*cos(conj(theta3(i)))*sin(conj(theta2(i)));
E=[E1;E2;E3];
digits(2);
c_inv=inv(C);
V=c_inv*(-E);


    A=[0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1;0 0 0 V(1,1)/theta_dot1(i) 0 0;0 0 0 0 V(2,1)/theta_dot2(i) 0;0 0 0 0 0 V(3,1)/theta_dot3(i)];
    B=[0 0 0;0 0 0;0 0 0;c_inv(1,1) 0 0;0 c_inv(2,2) c_inv(2,3);0 c_inv(3,2) c_inv(3,3)];
  fc =  A*theta_vec(:,i)+B*u(:,i) ; 
  f = theta_vec(:,i) + Ts*fc ;
  theta1(i+1) = f(1,1);
theta2(i+1) = f(2,1);
theta3(i+1) = f(3,1);
theta_dot1(i+1) = f(4,1);
theta_dot2(i+1) = f(5,1);
theta_dot3(i+1) = f(6,1);
y(:,i+1) = [theta1(i+1);theta2(i+1);theta3(i+1)];
theta_vec(:,i+1)=[theta1(i+1);theta2(i+1);theta3(i+1);theta_dot1(i+1);theta_dot2(i+1);theta_dot3(i+1)];
end 
  
J = sum((Yref-y).^2) + sum(u.^2);
end

and the error is:

Error using fmincon (line 609)

Supplied objective function must return a scalar value.

Error in MPC1 (line 42)

U = fmincon(K,U,[],[]);

Answer 1

[Stephan]

J is a 1x6 vector, but should be a scalar. You have to find out why and fix this.

[john white]

thanks for your answer

I changed the code as:

J = sum((Yref(:,i)-y(:,i)).^2) + sum(u(:,i).^2);

and the J be the scalar but I got another error:

Index exceeds matrix dimensions.

Error in tee (line 39)

J = sum((Yref(:,i)-y(:,i)).^2) + sum(u(:,i).^2);

Error in MPC1>@(u)tee(theta_vec(:,i),Yref(:,i),Np,u(:,i),Ts)

Error in fmincon (line 536)

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

Error in MPC1 (line 42)

U = fmincon(K,U,[],[]);

Caused by:

Failure in initial objective function evaluation. FMINCON cannot continue.