Finding the parameters of a system which is defined by a set of differential equations

Hello. I have a physical robot whose parameters (such as moment of inertia, electric motor constants, etc ) I will like to find using the technic of optimisation. In order to achieve my goal I've supposed some reasonable values for these constants which have enabled me to numerically solve my system of 14 differential equations for all my 14 variables (robot's position, velocity, etc) using ode45. I've done the same test on the physical robot which has enabled me to measure and register results for all of my 14 variables. I now have two files (one from tests made on the physical robot and another from numerically solving my system of differential equations) containing results for all of my 14 variables. I will now like to use the optimisation toolbox to compare my two sets of variables and hence find the correct values for the robot's parameters (constants). Below is the MATLAB code I've written:

OBJECTIF FUNCTION:

function [J_theta, J_thetaPoint, J_x, J_y, J_w1, J_w2, J_w3, J_w4, J_I1, J_I2, J_I3, J_I4, J_Vt, J_Vn] = critere(p,t,y,ym)
dydt = bsxfun(@rdivide,diff(y),diff(t));
ym(end,:) = []
friction = p(1) ;
At = p(2) ;
Ct = p(3) ;
An = p(4) ;
Cn = p(5) ;
A = p(6) ;
C = p(7) ;
L1 = p(8) ;
L2 = p(9) ;
L3 = p(10) ;
L4 = p(11) ;
Kb1 = p(12) ;
Kb2 = p(13) ;
Kb3 = p(14) ;
Kb4 = p(15) ;
Km1 = p(16) ;
Km2 = p(17) ;
Km3 = p(18) ;
Km4 = p(19) ;
R1 = p(20) ;
R2 = p(21) ;
R3 = p(22) ;
R4 = p(23) ;
Kr = p(24);
m = p(25);
R = p(26);
d = p(27) ;
J = p(28);
Jm1 = p(29);
Jm2 = p(30);
Jm3 = p(31);
Jm4 = p(32);
n = p(33);
dy_theta_dt = dydt(:,1);
dy_thetaPoint_dt = dydt(:,2);
dy_x_dt = dydt(:,3);
dy_y_dt = dydt(:,4);
dy_w1_dt = dydt(:,5);
dy_w2_dt = dydt(:,6);
dy_w3_dt = dydt(:,7);
dy_w4_dt = dydt(:,8);
dy_I1_dt = dydt(:,9);
dy_I2_dt = dydt(:,10);
dy_I3_dt = dydt(:,11);
dy_I4_dt = dydt(:,12);
dy_Vt_dt = dydt(:,13);
dy_Vn_dt = dydt(:,14);
U1 = ym(:,15);
U2 = ym(:,16);
U3 = ym(:,17);
U4 = ym(:,18);
dym_theta_dt = ym(:,2);
dym_thetaPoint_dt = ( (n*d/R)*(Km1*ym(:,9) + Km2*ym(:,10) + Km3*ym(:,11) + Km4*ym(:,12)) - friction*A*ym(:,2) - friction*C*sign(ym(:,2)) )./J ;
dym_x_dt = ym(:,13).*cos(ym(:,1)) + ym(:,14).*sin(ym(:,1)) ;
dym_y_dt = -ym(:,13).*sin(ym(:,1)) + ym(:,14).*cos(ym(:,1)) ;
dym_w1_dt = ( (Kb1-Km1/n)*ym(:,9) - n*Kr*ym(:,5) )./(n*Jm1) ;
dym_w2_dt = ( (Kb2-Km2/n)*ym(:,10) - n*Kr*ym(:,6) )./(n*Jm2) ;
dym_w3_dt = ( (Kb3-Km3/n)*ym(:,11) - n*Kr*ym(:,7) )./(n*Jm3) ;
dym_w4_dt = ( (Kb4-Km4/n)*ym(:,12) - n*Kr*ym(:,8) )./(n*Jm4) ;
dym_I1_dt = ( U1-n*Kb1*ym(:,5) - R1*ym(:,9) )./L1 ;
dym_I2_dt = ( U2-n*Kb2*ym(:,6) - R2*ym(:,10) )./L2 ;
dym_I3_dt = ( U3-n*Kb3*ym(:,7) - R3*ym(:,11) )./L3 ;
dym_I4_dt = ( U4-n*Kb4*ym(:,8) - R4*ym(:,12) )./L4 ;
dym_Vt_dt = ( (n/R)*( -Km1*ym(:,9) + Km3*ym(:,11)  ) - friction*At*ym(:,13) - friction*Ct*sign(ym(:,13)) )./m ;
dym_Vn_dt = ( (n/R)*( -Km2*ym(:,10) + Km4*ym(:,12) ) - friction*An*ym(:,14) - friction*Cn*sign(ym(:,14)) )./m ;
J_theta = sum((dy_theta_dt - dym_theta_dt).^2);
J_thetaPoint = sum((dy_thetaPoint_dt - dym_thetaPoint_dt).^2);
J_x = sum((dy_x_dt - dym_x_dt).^2);
J_y = sum((dy_y_dt - dym_y_dt).^2);
J_w1 = sum((dy_w1_dt - dym_w1_dt).^2);
J_w2 = sum((dy_w2_dt - dym_w2_dt).^2);
J_w3 = sum((dy_w3_dt - dym_w3_dt).^2);
J_w4 = sum((dy_w4_dt - dym_w4_dt).^2);
J_I1 = sum((dy_I1_dt - dym_I1_dt).^2);
J_I2 = sum((dy_I2_dt - dym_I2_dt).^2);
J_I3 = sum((dy_I3_dt - dym_I3_dt).^2);
J_I4 = sum((dy_I4_dt - dym_I4_dt).^2);
J_Vt = sum((dy_Vt_dt - dym_Vt_dt).^2);
J_Vn = sum((dy_Vn_dt - dym_Vn_dt).^2);

NON-LINEAR CONSTRAINTS FUNCTION:

function [Cieq, Ceq] = contraintes(p,t,y,ym)
dydt = bsxfun(@rdivide,diff(y),diff(t));
ym(end,:) = [];
friction = p(1) ;
At = p(2) ;
Ct = p(3) ;
An = p(4) ;
Cn = p(5) ;
A = p(6) ;
C = p(7) ;
L1 = p(8) ;
L2 = p(9) ;
L3 = p(10) ;
L4 = p(11) ;
Kb1 = p(12) ;
Kb2 = p(13) ;
Kb3 = p(14) ;
Kb4 = p(15) ;
Km1 = p(16) ;
Km2 = p(17) ;
Km3 = p(18) ;
Km4 = p(19) ;
R1 = p(20) ;
R2 = p(21) ;
R3 = p(22) ;
R4 = p(23) ;
Kr = p(24);
m = p(25);
R = p(26);
d = p(27) ;
J = p(28);
Jm1 = p(29);
Jm2 = p(30);
Jm3 = p(31);
Jm4 = p(32);
n = p(33);
dy_theta_dt = dydt(:,1);
dy_thetaPoint_dt = dydt(:,2);
dy_x_dt = dydt(:,3);
dy_y_dt = dydt(:,4);
dy_w1_dt = dydt(:,5);
dy_w2_dt = dydt(:,6);
dy_w3_dt = dydt(:,7);
dy_w4_dt = dydt(:,8);
dy_I1_dt = dydt(:,9);
dy_I2_dt = dydt(:,10);
dy_I3_dt = dydt(:,11);
dy_I4_dt = dydt(:,12);
dy_Vt_dt = dydt(:,13);
dy_Vn_dt = dydt(:,14);
U1 = ym(:,15);
U2 = ym(:,16);
U3 = ym(:,17);
U4 = ym(:,18);
Cieq = [];
Ceq = [
dy_theta_dt - ym(:,2);
dy_thetaPoint_dt - ( (n*d/R)*(Km1*ym(:,9) + Km2*ym(:,10) + Km3*ym(:,11) + Km4*ym(:,12)) - friction*A*ym(:,2) - friction*C*sign(ym(:,2)) )./J ;
dy_x_dt - ym(:,13).*cos(ym(:,1)) + ym(:,14).*sin(ym(:,1)) ;
dy_y_dt - -ym(:,13).*sin(ym(:,1)) + ym(:,14).*cos(ym(:,1)) ;
dy_w1_dt - ( (Kb1-Km1/n)*ym(:,9) - n*Kr*ym(:,5) )./(n*Jm1) ;
dy_w2_dt - ( (Kb2-Km2/n)*ym(:,10) - n*Kr*ym(:,6) )./(n*Jm2) ;
dy_w3_dt - ( (Kb3-Km3/n)*ym(:,11) - n*Kr*ym(:,7) )./(n*Jm3) ;
dy_w4_dt - ( (Kb4-Km4/n)*ym(:,12) - n*Kr*ym(:,8) )./(n*Jm4) ;
dy_I1_dt - ( U1-n*Kb1*ym(:,5) - R1*ym(:,9) )./L1 ;
dy_I2_dt - ( U2-n*Kb2*ym(:,6) - R2*ym(:,10) )./L2 ;
dy_I3_dt - ( U3-n*Kb3*ym(:,7) - R3*ym(:,11) )./L3 ;
dy_I4_dt - ( U4-n*Kb4*ym(:,8) - R4*ym(:,12) )./L4 ;
dy_Vt_dt - ( (n/R)*( -Km1*ym(:,9) + Km3*ym(:,11)  ) - friction*At*ym(:,13) - friction*Ct*sign(ym(:,13)) )./m ;
dy_Vn_dt - ( (n/R)*( -Km2*ym(:,10) + Km4*ym(:,12) ) - friction*An*ym(:,14) - friction*Cn*sign(ym(:,14)) )./m 
      ];

MAIN PROGRAMME:

clear;
close all;
clc;
% y représente les données obtenues avec le robot physique
% ym représente les données obtenues avec le simulateur
y = load('NumericalResolutionResults.txt');
ym = load('PhysicalTestResults.txt');
temps_echantillonage = 0.01 ;
Tf = 5 ; % Temps de simulation [s]
t = (0:temps_echantillonage:Tf)' ;
LB = [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0];
UB = [0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.1 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 4 4 4 4 0.5 1.5 1 1 3.5 2 2 2 2 101];
% UB(:) = 105;
p0 = [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 3.5 3.5 3.5 3.5 0 1 0 0 2.7 0 0 0 0 99];
opt = optimoptions('fmincon', 'TolFun', 1e-6);
[p,fval,exitflag]=fmincon(@(p) critere(p,t,y,ym),p0,[],[],[],[],LB,UB,@(p) contraintes(p,t,y,ym),opt);
if exitflag~=1
    error(['EXITFLAG = ' num2str(exitflag) '. Voir le texte ci-dessus et la section sur l''EXITFLAG en tapant ''help fmincon''.']);
end

The programme suddenly ends with an error message and without giving me any reasonable value for the constants. I do not know what's wrong. Any help on how to resolve this issue will be greatly appreciated. Thanks in advance