Commit 0e7aef8e authored by mmroma's avatar mmroma
Browse files

Progress commit for the night.

parent e5aa384c
......@@ -13,7 +13,7 @@ function [params] = getLQRGainMatrixIntAct(params)
params.ueq = ueq;
[A,B] = symLin(quadModel, xeq, ueq);
C = eye(n);
% C = eye(n);
%% 3) Design a Control Gain With LQR & Int Act
A_ = [A, zeros(n,3);
......
function [params] = parameters_pointMass()
function [params] = parameters_Payload()
%parameters_pointMass
% Parameters file for a point mass payload
% Parameters file for payload
%%
params.g = 9.81; % Acc. Due to Gravity (m/s^2)
......
function [params] = parameters_APM_Quad()
function [params] = parameters_Quad()
%parameters_APM_Quad
% Parameters file for a Quadrotor using APM's cascaded PID controller
......@@ -27,106 +27,47 @@ function [params] = parameters_APM_Quad()
params.distSTD = 0.1; % 0-mean White Gaussian Dist (Ext Force)
%% Controller Parameters
% Select Which Function will be the controller
% params.controller = @(X,params) attitudePID(X,params);
% params.controller = @(X,params) positionPID(X,params);
% params.controller = @(X,params) testNegativeYawTorque(X,params);
% Full State lqr with integral action
params = getLQRGainMatrixIntAct(params);
params.controller = @(X,params) fullstate_lqr_intAct(X,params);
% % Full State lqr
% params = getLQRGainMatrix(params);
% params.controller = @(X,params) fullstate_lqr(X,params);
% Controller Rate (s)
params.ctlDt = 0.0025;
% PID Blocks (adjust the parameters as described below)
% PID(kp,ki,kd,imax,filt_hz,dt,ff)
params.xPID = PID(1,0,0,0,20,params.ctlDt,0);
params.yPID = PID(1,0,0,0,20,params.ctlDt,0);
params.zPID = PID(4,2,0,2,20,params.ctlDt,0);
params.uPID = PID(0.4,0,0.01,0,20,params.ctlDt,0);
params.vPID = PID(0.4,0,0.01,0,20,params.ctlDt,0);
params.wPID = PID(3,0.1,0,1,20,params.ctlDt,0);
params.rollPID = PID(3,0,0,0,20,params.ctlDt,0);
params.pitchPID = PID(3,0,0,0,20,params.ctlDt,0);
params.yawPID = PID(3,0,0,0,20,params.ctlDt,0);
params.rollRatePID = PID(0.5,0.075,0.01,1,20,params.ctlDt,0);
params.pitchRatePID = PID(0.5,0.075,0.02,1,20,params.ctlDt,0);
params.yawRatePID = PID(0.9,0.02,0.008,1,5,params.ctlDt,0);
%% Animation Parameters
%% Initial Conditions
x0_.x = 0; % x position
x0_.y = 0; % y position
x0_.z = 0; % z position
x0_.u = 0; % Body Frame Velocity in x-direction
x0_.v = 0; % Body Frame Velocity in y-direction
x0_.w =0; % Body Frame Velocity in z-direction
x0_.phi = 0; % Roll
x0_.theta = 0; % Pitch
x0_.psi = 0; % Yaw
x0_.p = 0; % Roll Rate
x0_.q = 0; % Pitch Rate
x0_.r = 0; % Yaw Rate
x0 = [x0_.x;
x0_.y;
x0_.z;
x0_.u;
x0_.v;
x0_.w;
x0_.phi;
x0_.theta;
x0_.psi;
x0_.p;
x0_.q;
x0_.r];
params.x0 = x0;
%% Desired Final State
xd_.x = 0; % x position
xd_.y = 0; % y position
xd_.z = 0; % z position
xd_.u = 0; % Body Frame Velocity in x-direction
xd_.v = 0; % Body Frame Velocity in y-direction
xd_.w =0; % Body Frame Velocity in z-direction
xd_.phi = 0; % Roll
xd_.theta = 0; % Pitch
xd_.psi = 0; % Yaw
xd_.p = 0; % Roll Rate
xd_.q = 0; % Pitch Rate
xd_.r = 0; % Yaw Rate
xd = [xd_.x;
xd_.y;
xd_.z;
xd_.u;
xd_.v;
xd_.w;
xd_.phi;
xd_.theta;
xd_.psi;
xd_.p;
xd_.q;
xd_.r];
params.xd = xd;
%% Initial Conditions
params.x0 = [0, % x position [m]
0, % y position [m]
0, % z position [m]
0, % u (m/s)
0, % v (m/s)
0, % w (m/s)
0, % phi (roll) (rad)
0, % theta (pitch) (rad)
0, % psi (yaw) (rad)
0, % p (rad/s)
0, % q (rad/s)
0]; % r (rad/s)
%% Desired Final State
params.xd = [0,... % x position [m]
0,... % y position [m]
0,... % z position [m]
0,... % u (m/s)
0,... % v (m/s)
0,... % w (m/s)
0,... % phi (roll) (rad)
0,... % theta (pitch) (rad)
0,... % psi (yaw) (rad)
0,... % p (rad/s)
0,... % q (rad/s)
0]; % r (rad/s)
%% Tether Parameters
params.L = 0.5; % Unstretched Length (m)
params.k = 20; % Spring Constant
params.k = 20; % Spring Constant
params.b = 5; % Damping Coefficient
params.NegTension = 0; % Allow "Negative Tensions"? 0 = No, 1 = Yes
params.r_Pi_P = [0,0,0]; % Tether Mounting Point in Payload Frame
end
function [params] = parameters_Quad()
%parameters_APM_Quad
% Parameters file for a Quadrotor using APM's cascaded PID controller
%% Vehicle Type
params.numRotors = 4; % Quadrotor = 4, Hex = 6, ...
params.cross = true; % Cross Configuration (as opposed to plus)
%% Multirotor Physics Parameters
params.g = 9.81; % Acc. Due to Gravity (m/s^2)
params.m = 1.05; % Mass of Quadrotor (kg)
params.c1 = 5; % Motor Force Constant
params.c2 = 0.04; % Motor Torque Constant
params.Jx = 0.008; % Moment of Inertia about x (kg m^2)
params.Jy = 0.008; % Moment of Inertia about y (kg m^2)
params.Jz = 0.014; % Moment of Inertia about z (kg m^2)
params.a = 0.165; % Length of Rotor Arm (Radius) (m)
params.M = makeMotorMixerMatrix(params);
% Motor Command to Maintain Hover assuming 0 attitude (normalized)
params.hoverMotorCmd = (params.m * params.g) / sum(params.M(1,:));
% Thrust Value needed to maintain hover and fight gravity (N)
params.hoverThrust = (params.m * params.g);
%% Noise and Disturbances
params.distSTD = 0.1; % 0-mean White Gaussian Dist (Ext Force)
%% Controller Parameters
% Full State lqr with integral action
params = getLQRGainMatrixIntAct(params);
params.controller = @(X,params) fullstate_lqr_intAct(X,params);
% Controller Rate (s)
params.ctlDt = 0.0025;
%% Initial Conditions
params.x0 = [0;... % x position [m]
0;... % y position [m]
0;... % z position [m]
0;... % u (m/s)
0;... % v (m/s)
0;... % w (m/s)
0;... % phi (roll) (rad)
0;... % theta (pitch) (rad)
0;... % psi (yaw) (rad)
0;... % p (rad/s)
0;... % q (rad/s)
0]; % r (rad/s)
%% Desired Final State
params.xd = [0;... % x position [m]
0;... % y position [m]
0;... % z position [m]
0;... % u (m/s)
0;... % v (m/s)
0;... % w (m/s)
0;... % phi (roll) (rad)
0;... % theta (pitch) (rad)
0;... % psi (yaw) (rad)
0;... % p (rad/s)
0;... % q (rad/s)
0]; % r (rad/s)
%% Tether Parameters
params.L = 0.5; % Unstretched Length (m)
params.k = 20; % Spring Constant
params.b = 5; % Damping Coefficient
params.NegTension = 0; % Allow "Negative Tensions"? 0 = No, 1 = Yes
params.r_Pi_P = [0,0,0]; % Tether Mounting Point in Payload Frame
end
......@@ -11,31 +11,31 @@ function [params] = setUpParameters()
L_all = 1.5;
% Q1
params.multirotors{1} = parameters_APM_Quad();
params.multirotors{1} = parameters_Quad();
params.multirotors{1}.x0(1:3) = [1.5;0.5;0];
params.multirotors{1}.xd(1:3) = [0.5;-0.5;0];
params.multirotors{1}.L = L_all;
% Q2
params.multirotors{2} = parameters_APM_Quad();
params.multirotors{2} = parameters_Quad();
params.multirotors{2}.x0(1:3) = [1.5;1.5;0];
params.multirotors{2}.xd(1:3) = [0.5;0.5;0];
params.multirotors{2}.L = L_all;
% Q3
params.multirotors{3} = parameters_APM_Quad();
params.multirotors{3} = parameters_Quad();
params.multirotors{3}.x0(1:3) = [0.5;1.5;0];
params.multirotors{3}.xd(1:3) = [-0.5;0.5;0];
params.multirotors{3}.L = L_all;
% Q4
params.multirotors{4} = parameters_APM_Quad();
params.multirotors{4} = parameters_Quad();
params.multirotors{4}.x0(1:3) = [0.5;0.5;0];
params.multirotors{4}.xd(1:3) = [-0.5;-0.5;0];
params.multirotors{4}.L = L_all;
%% Load Payload
params.payload = parameters_pointMass();
params.payload = parameters_Payload();
% params.payload.x0(1:3) = [0;0;sqrt(3)/2];
params.payload.x0(1:3) = [1;1;0];
......
function [zAcc] = accFromState(X,params)
% Convenience Function to obtain the current vertical acceleration
zAcc = 0;
end
......@@ -6,9 +6,9 @@ function [M] = makeMotorMixerMatrix(params)
%% Quadrotor in Cross Configuration
if( (params.numRotors == 4) && (params.cross == true) )
C1 = params.k1;
C2 = sqrt(2) / 2 * params.l * params.k1;
C3 = params.k2;
C1 = params.c1;
C2 = sqrt(2) / 2 * params.a * params.c1;
C3 = params.c2;
M = [C1 C1 C1 C1;
-C2 C2 C2 -C2;
......
......@@ -7,13 +7,6 @@ function [xdot] = systemModel(X,U,params)
% Outputs:
% xdot: state derivative (num_vehicles*12 + 6 x1)
%% Using the quadrotor model as given in
% Quadrotor Dynamics and Control Rev 0.1
% Randal Beard
% 2008-05-05
% https://scholarsarchive.byu.edu/cgi/viewcontent.cgi?article=2324&context=facpub
% With a modification to the mixer matrix
%% 0) Constrain Input
U = min(U,1);
U = max(U,0);
......@@ -69,11 +62,11 @@ function [xdot] = systemModel(X,U,params)
Tao_psi = F_and_Taos(4);
%% 2.3) (1-3) Position Derivative ([x_d; y_d; z_d])
R = [cos(theta)*cos(psi) sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);
R_V_G = [cos(theta)*cos(psi) sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi) cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);
cos(theta)*sin(psi) sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi) cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
-sin(theta) sin(phi)*cos(theta) cos(phi)*cos(theta)];
pos_d = R * [u; v; w];
pos_d = R_V_G * [u; v; w];
%% 2.4) Compute Force From Slung Load (this might be more complicated)
l = sqrt( (load_x-x)^2 + (load_y-y)^2 + (load_z-z)^2 );
......@@ -96,7 +89,7 @@ function [xdot] = systemModel(X,U,params)
g*cos(theta)*sin(phi);
g*cos(theta)*cos(phi)];
A3 = (1/m) .* [0; 0; -F];
A4 = -1 * (1/m) .* F_T;
A4 = -1 * (1/m) .* R_V_G' * F_T;
mr_dist_vec = params.multirotors{1}.distSTD .* randn(size(A3));
......
......@@ -19,7 +19,7 @@ simData = runSimulation(params);
plotData(simData,params);
%% Animate
% animate(simData,params);
animate(simData,params);
%% Save the Data
% saveData(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