Commit 2bae739a authored by mmroma's avatar mmroma
Browse files

Added disturbance forces and sensor noise back in.

parent 70fb5a41
......@@ -16,8 +16,15 @@ function [params] = parameters_Payload(global_params)
0, params.Jy, 0;
0, 0, params.Jz];
%% Noise and Disturbances
params.distSTD = 0; % 0-mean White Gaussian Dist (Ext Force)
%% Disturbances
params.dist_F_power = 0.1 .* [1; 1; 1]; % 0-mean Band-limited, White Gaussian Dist (Ext Force)
params.dist_F_seed = [1 2 3];
params.dist_M_power = 0.1 .* [1; 1; 1]; % 0-mean Band-limited, White Gaussian Dist (Ext Torque)
params.dist_M_seed = [4 5 6];
%% Noise
params.noise_power = 0.1 .* ones(12,1);
params.noise_seed = 1001:1012;
%% Initial Conditions
params.x0 = [0;... % x position [m]
......
......@@ -26,8 +26,15 @@ function [params] = parameters_Quad(global_params)
% Thrust Value needed to maintain hover and fight gravity (N)
params.hoverThrust = (params.m * params.g);
%% Noise and Disturbances
params.distSTD = 0; % 0-mean White Gaussian Dist (Ext Force)
%% Disturbances
params.dist_F_power = 0.1 .* [1; 1; 1]; % 0-mean Band-limited, White Gaussian Dist (Ext Force)
params.dist_F_seed = [1 2 3];
params.dist_M_power = 0.1 .* [1; 1; 1]; % 0-mean Band-limited, White Gaussian Dist (Ext Force)
params.dist_M_seed = [4 5 6];
%% Noise
params.noise_power = 0.1 .* ones(12,1);
params.noise_seed = 101:112;
%% Controller Parameters
% Full State lqr with integral action
......
......@@ -21,21 +21,30 @@ function [params] = setUpParameters(traj_num)
params.ctlDt = 0.01; % Controller Step Size (s)
params.tStep = params.ctlDt / 10; % Simulation Step Size (s)
%% Noise and Disturbance Forces
quad_dist_STD = 0; %0.1; % 0-mean White Gaussian Quad Disturbance
load_dist_STD = 0.1; %0.1; % 0-mean White Gaussian Load Disturbance
%% Sensor Noise on Vehicle States (0-mean White Gaussian Noise)
std = 0;
params.noiseSTD = [std;std;std;
0;0;0;
0;0;0;
0;0;0];
%% Disturbance Forces & Torques
quads_dist_F_power = 1e-3;
quads_dist_M_power = 0;
load_dist_F_power = 1e-3;
load_dist_M_power = 0;
%% Sensor Noise
quad_noise_power = [ones(3,1).* 1e-8;
ones(3,1).* 1e-6;
ones(3,1).* 1e-6;
ones(3,1).* 1e-4];
load_noise_power = [ones(3,1).* 1e-8;
ones(3,1).* 1e-6;
ones(3,1).* 1e-6;
ones(3,1).* 1e-4];
%% Load Payload Parameters
params.payload = parameters_Payload(params);
params.payload.distSTD = load_dist_STD;
% params.payload.distSTD = load_dist_STD;
%params.payload.m = 0.5;
params.payload.dist_F_power = load_dist_F_power;
params.payload.dist_M_power = load_dist_M_power;
params.payload.noise_power = load_noise_power;
params.payload.m = 0.5;
r = params.payload.diameter / 2;
......@@ -62,9 +71,18 @@ function [params] = setUpParameters(traj_num)
makehgtform('yrotate',ele(i)) *...
[r;0;0;1];
params.multirotors{i}.r_Pi_P = r_Pi_P(1:3);
params.multirotors{i}.distSTD = quad_dist_STD;
% params.multirotors{i}.distSTD = quad_dist_STD;
params.l_i_last{i} = L_all;
params.multirotors{i}.ctlDt = params.ctlDt;
params.multirotors{i}.dist_F_seed = i*10 + [1 2 3];
params.multirotors{i}.dist_F_power = quads_dist_F_power .* [1 1 1];
params.multirotors{i}.dist_M_seed = i*10 + [4 5 6];
params.multirotors{i}.dist_M_power = quads_dist_M_power .* [1 1 1];
params.multirotors{i}.noise_power = quad_noise_power;
params.multirotors{i}.noise_seed = i*100 + [1:12];
end
%% Set Wind Parameters
......
% Random Noise Messing Around
%% 1) Call Simulink
simData = sim('random_noise');
%% 2) Plot the noise
figure(1);
plot(simData.tout,simData.noise(:,1));
hold on;
plot(simData.tout,simData.noise(:,2));
hold off;
legend('x','y')
%% Calc STD
std(simData.noise(:,1))
\ No newline at end of file
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