Trying to recreates the center of mass from equation
4 Ansichten (letzte 30 Tage)
Ältere Kommentare anzeigen
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
Antworten (1)
Fabio Freschi
am 8 Okt. 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 Kommentare
Siehe auch
Kategorien
Mehr zu Directed Graphs finden Sie in Help Center und File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!