diff options
| -rw-r--r-- | uav_model.m | 178 | ||||
| -rw-r--r-- | uav_params.m | 50 |
2 files changed, 114 insertions, 114 deletions
diff --git a/uav_model.m b/uav_model.m index 7b2bf9d..2ae4d96 100644 --- a/uav_model.m +++ b/uav_model.m @@ -31,25 +31,25 @@ alpha = [alpha_1; alpha_2; alpha_3; alpha_4]; % positive / negative with respect to the roll / pitch axis to which they % are attached to. Reference table: % -% angle attached axis lift force direction -% when angle is positive -% ------- ------------- ---------------------- -% alpha_1 pos. x axis y direction -% alpha_2 pos. y axis -x direction -% alpha_3 neg. x axis y direction -% alpha_4 neg. y axis -x direction +% angle attached axis lift force direction +% when angle is positive +% ------- ------------- ---------------------- +% alpha_1 pos. x axis y direction +% alpha_2 pos. y axis -x direction +% alpha_3 neg. x axis y direction +% alpha_4 neg. y axis -x direction % Rotation matrix to change between frames of reference: % multiplying by R moves from the inertial frame to the body frame % to go from body frame to inertial frame use R transpose (R is SO(3)) R = [ - cos(theta) * cos(psi), cos(theta) * sin(psi), -sin(theta); - (sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi)), ... - (sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi)), ... - sin(phi) * cos(theta); - (cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi)), ... - (cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi)), ... - cos(phi) * cos(theta); + cos(theta) * cos(psi), cos(theta) * sin(psi), -sin(theta); + (sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi)), ... + (sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi)), ... + sin(phi) * cos(theta); + (cos(phi) * sin(theta) * cos(psi) + sin(phi) * sin(psi)), ... + (cos(phi) * sin(theta) * sin(psi) - sin(phi) * cos(psi)), ... + cos(phi) * cos(theta); ]; % Matrix to relate Euler angles to angular velocity in the body frame @@ -79,7 +79,7 @@ nu = omega / pi * sqrt(k_T / (2 * a * rho)); % Aerodynamic force caused by flaps in body frame F_flap = @(alpha, uvec_n) rho * S * nu^2 / 2 * (... - (c_d * alpha^2 + c_0) * uvec_z + c_l * alpha * uvec_n); + (c_d * alpha^2 + c_0) * uvec_z + c_l * alpha * uvec_n); F_1 = F_flap(alpha_1, uvec_y); F_2 = F_flap(alpha_2, uvec_x); @@ -94,12 +94,12 @@ tau_4 = cross((d * uvec_z - a/3 * uvec_y), F_4); % Total force acting on the UAV in the body frame F = R * (m * g * uvec_k) ... % gravity - - k_T * omega^2 * uvec_z ... % thrust - + F_1 + F_2 + F_3 + F_4; % flaps + - k_T * omega^2 * uvec_z ... % thrust + + F_1 + F_2 + F_3 + F_4; % flaps % Total torque acting on the UAV in the body frame tau = J_r * omega * R * cross(uvec_k, Omega) + ... % gyroscopic procession - tau_1 + tau_2 + tau_3 + tau_4; % flaps + tau_1 + tau_2 + tau_3 + tau_4; % flaps % State space form with state variable xi and input u % @@ -115,10 +115,10 @@ u = [alpha; omega]; % Right hand side of dynamics dxi = f(xi, u) f = [ - Pdot; - R' * F / m; % translational dynamics - U * Omega; - inv(J) * (tau - cross(Omega, J * Omega)); % rotational dynamics + Pdot; + R' * F / m; % translational dynamics + U * Omega; + inv(J) * (tau - cross(Omega, J * Omega)); % rotational dynamics ]; % ------------------------------------------------------------------------ @@ -126,10 +126,10 @@ f = [ % Equilibrium point xi_eq = [ - params.linearization.Position; - params.linearization.Velocity; - params.linearization.Angles; - params.linearization.AngularVelocities; + params.linearization.Position; + params.linearization.Velocity; + params.linearization.Angles; + params.linearization.AngularVelocities; ]; u_eq = params.linearization.Inputs; @@ -139,31 +139,31 @@ B = subs(jacobian(f, u), [xi; u], [xi_eq; u_eq]); % Insert values of parameters phy = struct(... - 'g', params.physics.Gravity, ... - 'rho', params.physics.AirDensity ... + 'g', params.physics.Gravity, ... + 'rho', params.physics.AirDensity ... ); A = subs(A, phy); B = subs(B, phy); mech = struct(... - 'm', params.mechanical.Mass, ... - 'a', params.mechanical.DuctRadius, ... - 'b', params.mechanical.DuctHeight, ... - 'd', params.mechanical.FlapZDistance, ... - 'J_1', params.mechanical.InertiaTensor(1, 1), ... - 'J_2', params.mechanical.InertiaTensor(2, 2), ... - 'J_3', params.mechanical.InertiaTensor(3, 3), ... - 'J_r', params.mechanical.GyroscopicInertiaZ ... + 'm', params.mechanical.Mass, ... + 'a', params.mechanical.DuctRadius, ... + 'b', params.mechanical.DuctHeight, ... + 'd', params.mechanical.FlapZDistance, ... + 'J_1', params.mechanical.InertiaTensor(1, 1), ... + 'J_2', params.mechanical.InertiaTensor(2, 2), ... + 'J_3', params.mechanical.InertiaTensor(3, 3), ... + 'J_r', params.mechanical.GyroscopicInertiaZ ... ); A = subs(A, mech); B = subs(B, mech); aero = struct(... - 'k_T', params.aerodynamics.ThrustOmegaProp, ... - 'S', params.aerodynamics.FlapArea, ... - 'c_d', params.aerodynamics.DragCoefficients(1), ... - 'c_0', params.aerodynamics.DragCoefficients(2), ... - 'c_l', params.aerodynamics.LiftCoefficient ... + 'k_T', params.aerodynamics.ThrustOmegaProp, ... + 'S', params.aerodynamics.FlapArea, ... + 'c_d', params.aerodynamics.DragCoefficients(1), ... + 'c_0', params.aerodynamics.DragCoefficients(2), ... + 'c_l', params.aerodynamics.LiftCoefficient ... ); A = subs(A, aero); B = subs(B, aero); @@ -196,51 +196,51 @@ eigvals = eig(A); % Check system controllability / stabilizability Wc = ctrb(sys); if rank(Wc) < nx - fprintf('Linearized system has %d uncontrollable states!\n', ... - (nx - rank(Wc))); - - % Is the system at least stabilizable? - unstabilizable = 0; - for i = 1:nx - if real(eigvals(i)) >= 0 - % PBH test - W = [(A - eigvals(i) * eye(size(A))), B]; - if rank(W) < nx - % fprintf(' State %d is not stabilizable\n', i); - unstabilizable = unstabilizable + 1; - end - end - end - if unstabilizable > 0 - fprintf('Linearized system has %d unstabilizable modes!\n', ... - unstabilizable); - else - fprintf('However, it is stabilizable.\n'); - end + fprintf('Linearized system has %d uncontrollable states!\n', ... + (nx - rank(Wc))); + + % Is the system at least stabilizable? + unstabilizable = 0; + for i = 1:nx + if real(eigvals(i)) >= 0 + % PBH test + W = [(A - eigvals(i) * eye(size(A))), B]; + if rank(W) < nx + % fprintf(' State %d is not stabilizable\n', i); + unstabilizable = unstabilizable + 1; + end + end + end + if unstabilizable > 0 + fprintf('Linearized system has %d unstabilizable modes!\n', ... + unstabilizable); + else + fprintf('However, it is stabilizable.\n'); + end end % Check system observability / detectability Wo = obsv(sys); if rank(Wo) < nx - fprintf('Linearized system has %d unobservable state!\n', ... - (nx - rank(Wo))); - % is the system at least detectable? - undetectable = 0 - for i = 1:nx - if real(eigvals(i)) >= 0 - % PBH test - W = [C; (A - eigvals(i) * eye(size(A)))]; - if rank(W) < nx - undetectable = undetectable + 1; - end - end - end - if undetectable > 0 - fprintf('Linearized system has %d undetectable modes!\n', ... - undetectable); - else - fprintf('However, it is detectable.') - end + fprintf('Linearized system has %d unobservable state!\n', ... + (nx - rank(Wo))); + % is the system at least detectable? + undetectable = 0 + for i = 1:nx + if real(eigvals(i)) >= 0 + % PBH test + W = [C; (A - eigvals(i) * eye(size(A)))]; + if rank(W) < nx + undetectable = undetectable + 1; + end + end + end + if undetectable > 0 + fprintf('Linearized system has %d undetectable modes!\n', ... + undetectable); + else + fprintf('However, it is detectable.') + end end % ------------------------------------------------------------------------ @@ -250,21 +250,21 @@ model = struct(); % Function to compute the rotation matrix model.FrameRot = @(pitch, roll, yaw) ... - subs(R, [phi, theta, psi], [pitch, roll, yaw]); + subs(R, [phi, theta, psi], [pitch, roll, yaw]); % Equations of non-linear model (algebraic) model.nonlinear = struct(... - 'State', xi, ... - 'Inputs', u, ... - 'Dynamics', f ... + 'State', xi, ... + 'Inputs', u, ... + 'Dynamics', f ... ); % Linearized dynamics (numerical) model.linear = struct(... - 'Nx', nx, 'Nu', nu, 'Ny', ny, ... % number of states, inputs, and outputs - 'State', xi, 'Inputs', u, ... % state and input variables - 'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized - 'StateSpace', sys ... % state space object + 'Nx', nx, 'Nu', nu, 'Ny', ny, ... % number of states, inputs, and outputs + 'State', xi, 'Inputs', u, ... % state and input variables + 'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized + 'StateSpace', sys ... % state space object ); end diff --git a/uav_params.m b/uav_params.m index fb9121f..dd0c101 100644 --- a/uav_params.m +++ b/uav_params.m @@ -5,11 +5,11 @@ function [params] = uav_params() % Unit of measurements unless specified otherwise are -% Mass in kg -% Lenghts in m -% Time in s -% Frequencies in Hz -% Angular velocities in rad / s +% Mass in kg +% Lenghts in m +% Time in s +% Frequencies in Hz +% Angular velocities in rad / s params = struct(); @@ -17,29 +17,29 @@ params = struct(); % Physical constants params.physics = struct(... - 'Gravity', 9.81, ... - 'AirDensity', 1.204 ... % kg / m^3 + 'Gravity', 9.81, ... + 'AirDensity', 1.204 ... % kg / m^3 ); % ------------------------------------------------------------------------ % Mechanical measurements params.mechanical = struct(... - 'Mass', 4.0, ... - 'DuctRadius', 46e-3, ... - 'DuctHeight', 171e-3, ... - 'FlapZDistance', 98e-3, ... % flap distance along z from center of mass - 'InertiaTensor', 5e-3 * eye(3), ... % TODO, assume diagonal - 'GyroscopicInertiaZ', 0 ... % assume no gyroscopic effects for now + 'Mass', 4.0, ... + 'DuctRadius', 46e-3, ... + 'DuctHeight', 171e-3, ... + 'FlapZDistance', 98e-3, ... % flap distance along z from center of mass + 'InertiaTensor', 5e-3 * eye(3), ... % TODO, assume diagonal + 'GyroscopicInertiaZ', 0 ... % assume no gyroscopic effects for now ); % ------------------------------------------------------------------------ % Actuator limits params.actuators = struct(... - 'TurbineMaxSpeed', 620.7, ... % In Hz - 'ServoAbsMaxAngleDeg', 25, ... % in Degrees - 'MeasurementDelay', 8e-3 ... % delay caused by sensor fusion algorithm + 'TurbineMaxSpeed', 620.7, ... % In Hz + 'ServoAbsMaxAngleDeg', 25, ... % in Degrees + 'MeasurementDelay', 8e-3 ... % delay caused by sensor fusion algorithm ); % ------------------------------------------------------------------------ @@ -53,10 +53,10 @@ F_max = 38.637; % in N (measured) k_T = sqrt(F_max / omega_max); params.aerodynamics = struct(... - 'ThrustOmegaProp', k_T, ... - 'FlapArea', 23e-3 * 10e-3 * 1.5, ... % FIXME factor 150% was chosen at random - 'DragCoefficients', [1, 1] .* 1e-3, ... % TODO - 'LiftCoefficient', 1e-3 ... % TODO + 'ThrustOmegaProp', k_T, ... + 'FlapArea', 23e-3 * 10e-3 * 1.5, ... % FIXME factor 150% was chosen at random + 'DragCoefficients', [1, 1] .* 1e-3, ... % TODO + 'LiftCoefficient', 1e-3 ... % TODO ); % ------------------------------------------------------------------------ @@ -71,11 +71,11 @@ k = params.aerodynamics.ThrustOmegaProp; omega_hover = sqrt(m * g / k); params.linearization = struct(... - 'Position', [0; 0; -3], ... % in inertial frame, z points down - 'Velocity', [0; 0; 0], ... % in inertial frame - 'Angles', [0; 0; 0], ... % in body frame - 'AngularVelocities', [0; 0; 0], ... % in body frame - 'Inputs', [0; 0; 0; 0; omega_hover] ... % Flaps at rest and turbine at X + 'Position', [0; 0; -3], ... % in inertial frame, z points down + 'Velocity', [0; 0; 0], ... % in inertial frame + 'Angles', [0; 0; 0], ... % in body frame + 'AngularVelocities', [0; 0; 0], ... % in body frame + 'Inputs', [0; 0; 0; 0; omega_hover] ... % Flaps at rest and turbine at X ); end
\ No newline at end of file |