diff options
Diffstat (limited to 'uav_model.m')
| -rw-r--r-- | uav_model.m | 147 |
1 files changed, 122 insertions, 25 deletions
diff --git a/uav_model.m b/uav_model.m index 1f8f751..fda2745 100644 --- a/uav_model.m +++ b/uav_model.m @@ -9,7 +9,8 @@ % principles (equations of motion). % % * A linear model obtained by linearizing the the non linear plant model at -% an operating point specified in the params struct argument. +% an operating point specified in the params struct argument. And adding +% models for the actuators. % % * A uncertain linear model with reference trcking built atop of the linear % model using SIMULINK. The uncertain model contains the performance and @@ -219,9 +220,22 @@ B = double(vpa(B)); C = eye(size(A)); D = zeros(12, 5); -% Number of states, inputs and outputs -[nx, nu] = size(B); -[ny, ~] = size(C); +% ------------------------------------------------------------------------ +% Model actuators + +% TODO: better model? +w = params.actuators.ServoNominalAngularVelocity; +zeta = 1; +G_servo = tf(w^2, [1, 2 * zeta * w, w^2]); + +w = 60; +zeta = 1; +G_prop = tf(w^2, [1, 2 * zeta * w, w^2]); + +model.actuators = struct( ... + 'FlapServo', G_servo, ... + 'ThrustPropeller', G_prop, ... + 'StateSpace', blkdiag(eye(4) * G_servo, G_prop)); % ------------------------------------------------------------------------ % Scale inputs and outputs of linearized model @@ -247,21 +261,31 @@ D = D * inv(S_actuators); % Create state space object T = params.measurements.SensorFusionDelay; n = params.linearization.PadeApproxOrder; -sys = minreal(pade(ss(A, B, C, D, 'OutputDelay', T), n), [], false); % slient +sys = pade(ss(A, B, C, D, 'OutputDelay', T), n); + +% Add actuators +sys = sys * model.actuators.StateSpace; + +% Remove unnecessary states +sys = minreal(sys, [], false); % slient + +% Number of states, inputs and outputs +[nx, nu] = size(sys.B); +[ny, ~] = size(sys.C); % Save 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 + 'StateSpace', sys, ... % state space objec 'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ... ); % ------------------------------------------------------------------------ % Check properties of linearized model -eigvals = eig(A); +eigvals = eig(sys.A); % Check system controllability / stabilizability Wc = ctrb(sys); @@ -274,7 +298,7 @@ if rank(Wc) < nx for i = 1:nx if real(eigvals(i)) >= 0 % PBH test - W = [(A - eigvals(i) * eye(size(A))), B]; + W = [(sys.A - eigvals(i) * eye(nx)), sys.B]; if rank(W) < nx % fprintf(' State %d is not stabilizable\n', i); unstabilizable = unstabilizable + 1; @@ -299,7 +323,7 @@ if rank(Wo) < nx for i = 1:nx if real(eigvals(i)) >= 0 % PBH test - W = [C; (A - eigvals(i) * eye(size(A)))]; + W = [sys.C; (sys.A - eigvals(i) * eye(nx))]; if rank(W) < nx undetectable = undetectable + 1; end @@ -313,31 +337,37 @@ if rank(Wo) < nx end end -% ------------------------------------------------------------------------ -% Compute absolute value of error caused by linearization around set point +stable = 0; +unstable = 0; +for i = 1:nx + if real(eigvals(i)) < 0 + stable = stable + 1; + else + unstable = unstable + 1; + end +end +fprintf(' - Linearized system has %d states:\n', nx); +fprintf(' %d controllable modes, %d observable modes.\n', rank(Wc), rank(Wo)); +fprintf(' %d stable modes, %d unstable modes.\n', stable, unstable); -% ------------------------------------------------------------------------ -% Model actuators - -% TODO: better model, this was tuned "by looking" -w = 2 * params.actuators.ServoNominalAngularVelocity; -zeta = .7; -G_servo = tf(w^2, [1, 2 * zeta * w, w^2]); +if rank(Wc) < 12 + error('Uncertain model has less than 12 controllable modes'); +end -w = 6e3; -zeta = 1; -G_prop = tf(w^2, [1, 2 * zeta * w, w^2]); +% ------------------------------------------------------------------------ +% Compute absolute value of error caused by linearization around set point -model.actuators = struct('FlapServo', G_servo, 'ThrustPropeller', G_prop); +% TODO % ------------------------------------------------------------------------ % Add uncertainties using SIMULINK model % Load simulink model with uncertainties and pass in parameters h = load_system('uav_model_uncertain'); -hws = get_param('uav_model_uncertain', 'modelworkspace'); +set_param('uav_model_uncertain', SimulationMode='Normal'); +hws = get_param('uav_model_uncertain', 'modelworkspace'); hws.assignin('params', params); hws.assignin('model', model); hws.assignin('perf', perf); @@ -375,8 +405,8 @@ S_uncert_out = blkdiag(... S_state(1:9, 1:9)); % Save uncertain model -model.uncertain = struct(... - 'Simulink', ulmod, ... +model.uncertain = struct(... + ... % 'Simulink', ulmod, ... 'BlockStructure', blk_stab, ... 'BlockStructurePerf', blk_perf, ... 'StateSpace', usys ... @@ -425,6 +455,73 @@ model.uncertain.Ny = max(size(idx.OutputNominal)); % ------------------------------------------------------------------------ % Check properties of uncertain model +eigvals = eig(usys.A); +nx = size(usys.A, 1); + +% Check system controllability / stabilizability +Wc = ctrb(usys); +if rank(Wc) < nx + fprintf(' - Uncertain 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 = [(usys.A - eigvals(i) * eye(nx)), usys.B]; + if rank(W) < nx + % fprintf(' State %d is not stabilizable\n', i); + unstabilizable = unstabilizable + 1; + end + end + end + if unstabilizable > 0 + fprintf(' - Uncertain system has %d unstabilizable modes!\n', ... + unstabilizable); + else + fprintf(' However, it is stabilizable.\n'); + end +end + +% Check system observability / detectability +Wo = obsv(usys); +if rank(Wo) < nx + fprintf(' - Uncertain system has %d unobservable states!\n', ... + (nx - rank(Wo))); + % is the system at least detectable? + undetectable = 0; + for i = 1:nx + if real(eigvals(i)) >= 0 + % PBH test + W = [usys.C; (usys.A - eigvals(i) * eye(nx))]; + if rank(W) < nx + undetectable = undetectable + 1; + end + end + end + if undetectable > 0 + fprintf(' - Uncertain system has %d undetectable modes!\n', ... + undetectable); + else + fprintf(' However, it is detectable.\n') + end +end + +stable = 0; +unstable = 0; +for i = 1:nx + if real(eigvals(i)) < 0 + stable = stable + 1; + else + unstable = unstable + 1; + end +end + +fprintf(' - Uncertain system has %d states:\n', nx); +fprintf(' %d controllable modes, %d observable modes.\n', rank(Wc), rank(Wo)); +fprintf(' %d stable modes, %d unstable modes.\n', stable, unstable); + % % Check that (A, B_u, C_y) is stabilizable and detectable % A = model.uncertain.StateSpace(... % model.uncertain.index.OutputUncertain, ... |