Trying to recreates the center of mass from equation
1 view (last 30 days)
Show older comments
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
Answers (1)
Fabio Freschi
on 8 Oct 2019
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
0 Comments
See Also
Categories
Find more on Directed Graphs in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!