Commit ec14441b authored by mmroma's avatar mmroma
Browse files

Switched integration from Euler's method to ode45 and I'm pretty sure that solved my problems

parent cc367e8b
......@@ -2,8 +2,8 @@ function [Xd, T] = gen_payload_traj(params)
%gen_payload_traj
%% 1) Design Coarse Waypoints [x, y, z, t] in meters and sec
coarse_WPs = WPs_set1_hover();
% coarse_WPs = WPs_set2_square();
% coarse_WPs = WPs_set1_hover();
coarse_WPs = WPs_set2_square();
% coarse_WPs = WPs_set3_rise_and_fall();
%% 2) Extract Relevant Params
......
......@@ -78,7 +78,7 @@ function [params] = setUpParameters()
%% Choose Additional Parameters
params.tFinal = 10; % Simulation Length (s)
params.tStep = 0.0001; % Simulation Step Size (s)
params.tStep = 0.001; % Simulation Step Size (s)
params.anim_FPS = 30; % Animation FPS
%% Set Guidamce
......
......@@ -89,11 +89,18 @@ function [simData] = runSimulation(params)
F_drags(tstep_count,:) = F_drags_curr';
Tether_states(tstep_count,:) = T_states_curr';
%% 2.9) Simulate with Euler's Method
for t=t_curr:params.tStep:(t_curr+params.multirotors{1}.ctlDt-params.tStep)
X_curr = X_curr + params.tStep * systemModel(X_curr,U_curr,params);
% X_curr = constrain_euler_angles(X_curr);
end
% %% 2.9) Simulate with Euler's Method
% for t=t_curr:params.tStep:(t_curr+params.multirotors{1}.ctlDt-params.tStep)
% X_curr = X_curr + params.tStep * systemModel(X_curr,U_curr,params);
% % X_curr = constrain_euler_angles(X_curr);
% end
%% 2.9) Simulate with ode45
[~,Xi] = ode45( @(t,x) systemModel(x,U_curr,params),...
t_curr+params.tStep:params.tStep:t_curr+params.multirotors{1}.ctlDt,...
X_curr);
X_curr = Xi(end,:)';
%% 2.10) Increment the time step count
tstep_count = tstep_count + 1;
......
......@@ -19,5 +19,5 @@ simData = runSimulation(params);
plotData(simData,params);
%% Animate
% animate(simData,params);
animate(simData,params);
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment