Trying to recreates the center of mass from equation

Trying to recreates the center of mass from equation, I couldn't debug the code I created! Please help!

varInput = load('quadData.mat');
qDat = varInput.quadrotor; 
[n,-] = size(qDat);
Xi_0 = [qDat(1,2:4)'; qDat(1,8:10)'];
v = qDat(:,11:13);
omega = qDat(:,11:13);
t=qDat(:,1);
dt=qDat(2,1) - qDat(1,1);
T_initial = qDat(1,1);
T_final = qDat(end,1);
pose_dot(1,Xi_0,{dt,v,omega});
[t_out,Xi] = ode45(@pose_dot,[T_initial T_final],Xi_0);

figure
plot(t_out,Xi(:,4:6)*(180/pi),':');
hold on
plot(t,qDat(:,8:10)*(180/pi),'-');
legend('\phi_{calc}', '\theta_{calc}','\psi_{calc}','\phi_{true}','\theta_{true}','\phi_{true}');
grid on
title('Euler Angle vs. Time');
ylabel('Euler Angle o');
xlabel('Time s');

figure
plot(t_out,Xi(:,1:3),':');
hold on
plot(t,qDat(:,2:4),'-');
legend('x_{calc}','y_{calc}','z_{calc}','x_{true}','y_{true}','z_{true}');
grid on
title('Position vs. Time');
xlabel('Time m');
ylabel('Position m');

function Xi_dot = pose_dot(t,Xi,prstVars)
if nargin > 2
    dt = prstVars{1};
    v = prstVars{2};
    omega = prstVars{3};
end
ind = floor(t/dt) + 1; 
nu = [v(ind,:)'; omega(ind,:)'];
cPsi = cos(Xi(6));
sPsi = sin(Xi(6));
cTh = cos(Xi(5));
sTh = sin(Xi(5));
cPhi = cos(Xi(4));
sPhi = sin(Xi(4));
tTh = tan(Xi(5));
Xi_dot=[cPsi*cTh cPsi*sTh*sPhi-sPsi*cPhi sPsi*sPhi+cPsi*sTh*cPhi 0 0 0;sPsi*cTh cPsi*cPhi+sPsi*sTh*sPhi sPsi*sTh*cPhi-cPsi*sPhi 0 0 0;-sTh cTh*sPhi cTh*sPhi cTh*cPhi 0 0 0;0 0 0 1 sPhi*tTh cPhi*tTh;0 0 0 0 cPhi -sPhi; 0 0 0 0 sPhi/cTh cPhi/cTh]*nu;
end

[Image Analyst]

Try Debugging in MATLAB for a debugging tutorial.

Answer 1

[Fabio Freschi]

Macroscopically, I see two main issues

1) third line

[n,-] = size(qDat);

should be (tilde sign)

[n,~] = size(qDat);

2) last line of the function: the number of columns for each row is different

Xi_dot=[cPsi*cTh cPsi*sTh*sPhi-sPsi*cPhi sPsi*sPhi+cPsi*sTh*cPhi 0 0 0;  % 6 entries
    sPsi*cTh cPsi*cPhi+sPsi*sTh*sPhi sPsi*sTh*cPhi-cPsi*sPhi 0 0 0;      % 6 entries
    -sTh cTh*sPhi cTh*sPhi cTh*cPhi 0 0 0;                               % 7 entries (too zeros?)
    0 0 0 1 sPhi*tTh cPhi*tTh;                                           % 6 entries
    0 0 0 0 cPhi -sPhi;                                                  % 6 entries
    0 0 0 0 sPhi/cTh cPhi/cTh]*nu;                                       % 6 entries