diff options
Diffstat (limited to 'uav_model.m')
| -rw-r--r-- | uav_model.m | 178 |
1 files changed, 89 insertions, 89 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 |