diff options
| -rw-r--r-- | uav.m | 214 | ||||
| -rw-r--r-- | uav_model.m | 147 | ||||
| -rw-r--r-- | uav_model_uncertain.slx | bin | 56206 -> 62600 bytes | |||
| -rw-r--r-- | uav_model_uncertain_clp.slx | bin | 163944 -> 67378 bytes | |||
| -rw-r--r-- | uav_params.m | 16 | ||||
| -rw-r--r-- | uav_performance_hinf.m | 47 | ||||
| -rw-r--r-- | uav_performance_musyn.m | 45 | ||||
| -rw-r--r-- | uav_sim_step.m | 16 | ||||
| -rw-r--r-- | uav_uncertainty.m | 18 |
9 files changed, 361 insertions, 142 deletions
@@ -9,7 +9,7 @@ clear; clc; close all; s = tf('s'); do_plots = true; % runs faster without -do_hinf = true; % midterm +do_hinf = false; % midterm do_musyn = true; % endterm fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n') @@ -37,7 +37,8 @@ params = uav_params(); % Define performance requirements fprintf('Generating performance requirements...\n') -perf = uav_performance_musyn(params, do_plots); +perf_hinf = uav_performance_hinf(params, do_plots); +perf_musyn = uav_performance_musyn(params, do_plots); %% ------------------------------------------------------------------------ % Define stability requirements @@ -49,7 +50,7 @@ uncert = uav_uncertainty(params, do_plots); % Create UAV model fprintf('Generating system model...\n'); -model = uav_model(params, perf, uncert); +model = uav_model(params, perf_musyn, uncert); %% ------------------------------------------------------------------------ % Perform H-infinity design @@ -71,7 +72,21 @@ if do_hinf 'AutoScale', 'off', 'RelTol', 1e-3); [K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt); fprintf(' - H-infinity synthesis gamma: %g\n', gamma); - ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf); + + index = struct( ... + 'Ix', 1, 'Iy', 2, 'Iz', 3, ... + 'IPdot', (4:6)', ... + 'Iroll', 7, 'Ipitch', 8, 'Iyaw', 9, ... + 'ITheta', (10:12)', ... + 'Ialpha', (1:4)', ... + 'Iomega', 5 ... + ); + + ctrl.hinf = struct( ... + 'Name', '$\mathcal{H}_{\infty}$', ... + 'K', K_inf, ... + 'index', index ... + ); if gamma >= 1 error('Failed to syntesize controller (closed loop is unstable).') @@ -80,11 +95,39 @@ if do_hinf %% ------------------------------------------------------------------------ % Measure Performance of H-infinity design - fprintf(' - Simulating closed loop...\n'); + if do_plots + fprintf(' - Plotting resulting controller...\n'); + + % Plot transfer functions + figure; hold on; + bode(ctrl.hinf.K(index.Ialpha(1), index.Ix)); + bode(ctrl.hinf.K(index.Ialpha(2), index.Ix)); + + bode(ctrl.hinf.K(index.Ialpha(1), index.Iy)); + bode(ctrl.hinf.K(index.Ialpha(2), index.Iy)); + + bode(ctrl.hinf.K(index.Iomega, index.Ix)); + bode(ctrl.hinf.K(index.Iomega, index.Iy)); + bode(ctrl.hinf.K(index.Iomega, index.Iz)); + + title(sprintf('\\bfseries %s Controller', ctrl.hinf.Name), ... + 'interpreter', 'latex'); + legend('$x \rightarrow \alpha_1$', ... + '$x \rightarrow \alpha_2$', ... + '$y \rightarrow \alpha_1$', ... + '$y \rightarrow \alpha_2$', ... + '$x \rightarrow \omega$', ... + '$y \rightarrow \omega$', ... + '$z \rightarrow \omega$', ... + 'interpreter', 'latex'); + grid on; + end + + fprintf('Simulating closed loop...\n'); - T = 60; - nsamples = 500; - do_noise = true; + T = 40; + nsamples = 800; + do_noise = false; simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise); fprintf(' - Writing simulation results...\n'); @@ -116,10 +159,10 @@ if do_musyn nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... - 'AutoScale', 'on', 'RelTol', 1e-3); + 'AutoScale', 'on', 'RelTol', 1e-2); % Frequency raster resolution to fit D scales - nsamples = 600; + nsamples = 300; omega_max = 3; omega_min = -3; @@ -132,12 +175,28 @@ if do_musyn % Maximum number of D-K iterations niters = 5; - fprintf(' - Will do at most %d iterations.\n', niters); + fprintf(' - Will do (max) %d iterations.\n', niters); % Maximum degree of D-scales and error - d_scales_max_degree = 3; - d_scales_max_err_p = .4; % in percentage - d_scales_improvement_p = .15; % in percentage + d_scales_max_degree = 10; + d_scales_max_err_p = .2; % in percentage + d_scales_improvement_p = .1; % in percentage, avoid diminishing returns + + % hand tuned degrees, inf means will pick automatically best fit + % according to parameters given above + d_scales_degrees = { + 0, 0, inf, inf, inf; % alpha + 1, inf, inf, inf, inf; % omega + 2, inf, inf, inf, inf; % state + 0, 0, inf, inf, inf; + }; + + if size(d_scales_degrees, 2) < niters + error(''); + end + + scaled_plant_reduce_ord = 100; + scaled_plant_reduce_maxerr = 1e-3; % for plotting later mu_plot_legend = {}; @@ -148,9 +207,31 @@ if do_musyn fprintf(' * Running D-K iteration %d / %d...\n', it, niters); itstart = tic(); - % Find controller using H-infinity + % Scale plant and reduce order, fitting the D-scales adds a lot of near + % zero modes that cause the plant to become very numerically ill + % conditioned. Since the D-scales are an approximation anyways (i.e. + % there is not mathematical proof for the fitting step), we limit the + % order of the scaled system to prevent poor scaling issues. P_scaled = minreal(D_left * P * inv(D_right), [], false); [P_scaled, ~] = prescale(P_scaled, omega_range); + n = size(P_scaled.A, 1); + + if n > scaled_plant_reduce_ord + R = reducespec(P_scaled, 'balanced'); % or ncf + R.Options.FreqIntervals = [omega_range{1}, omega_range{2}]; + R.Options.Goal = 'absolute'; % better hinf norm + if do_plots + figure(102); + view(R); + drawnow; + end + P_scaled = getrom(R, MaxError=scaled_plant_reduce_maxerr); + + fprintf(' Scaled plant was reduced from order %d to %d\n', ... + n, size(P_scaled.A, 1)) + end + + % Find controller using H-infinity [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt); fprintf(' H-infinity synthesis gamma: %g\n', gamma); @@ -241,36 +322,55 @@ if do_musyn for j = 1:4 % for each in left and right fprintf(' %s', D_names{j}); - best_fit_deg = inf; - best_fit_err = inf; - for deg = 0:d_scales_max_degree - % Fit D-scale - D_fit = fitmagfrd(D_frd{j}, deg); - % D_fit = fitfrd(genphase(D_frd{j}), deg); - - % Check if it is a good fit + + % tuned by hand? + if d_scales_degrees{j, it} < inf + % D_fit = fitmagfrd(D_frd{j}, d_scales_degrees{j, it}); + D_fit = fitfrd(genphase(D_frd{j}), d_scales_degrees{j, it}); + max_sv = max(max(sigma(D_fit, omega))); fit_err = abs(D_max_sv{j} - max_sv); - - if fit_err < best_fit_err - % Choose higher degree only if we improve by at least a specified - % percentage over the previous best fit (or we are at the first - % iteration). This is a heuristic to avoid adding too many states - % to the controller as it depends on the order of the D-scales. - if abs(best_fit_err - fit_err) / best_fit_err > d_scales_improvement_p || best_fit_err == inf - best_fit_deg = deg; - best_fit_err = fit_err; - D_fitted{j} = D_fit; + D_fitted{j} = D_fit; + + fprintf(' tuned degree %d, error %g (%g %%)\n', ... + d_scales_degrees{j, it}, fit_err, ... + 100 * fit_err / D_max_sv{j}); + + else + % find best degree + best_fit_deg = inf; + best_fit_err = inf; + + for deg = 0:d_scales_max_degree + % Fit D-scale + % D_fit = fitmagfrd(D_frd{j}, deg); + D_fit = fitfrd(genphase(D_frd{j}), deg); + + % Check if it is a good fit + max_sv = max(max(sigma(D_fit, omega))); + fit_err = abs(D_max_sv{j} - max_sv); + + if fit_err < best_fit_err + % Choose higher degree only if we improve by at least a + % specified percentage over the previous best fit (or we are + % at the first iteration). This is a heuristic to prevent + % adding too many states to the controller as it depends on + % the order of the D-scales. + if abs(best_fit_err - fit_err) / best_fit_err > d_scales_improvement_p || best_fit_err == inf + best_fit_deg = deg; + best_fit_err = fit_err; + D_fitted{j} = D_fit; + end end + + if (fit_err / D_max_sv{j} < d_scales_max_err_p) + break; + end + fprintf('.'); end - - if (fit_err / D_max_sv{j} < d_scales_max_err_p) - break; - end - fprintf('.'); + fprintf(' degree %d, error %g (%g %%)\n', ... + best_fit_deg, best_fit_err, 100 * best_fit_err / D_max_sv{j}); end - fprintf(' degree %d, error %g (%g %%)\n', ... - best_fit_deg, best_fit_err, 100 * best_fit_err / D_max_sv{j}); end % Construct full matrices @@ -340,7 +440,7 @@ if do_musyn fprintf(' - D-K iteration took %.1f seconds\n', dkend); if mu_rp > 1 && mu_rs > 1 - error(' - Failed to synthesize robust controller that meets the desired performance.\n'); + error(' - Failed to synthesize robust controller that meets the desired performance.'); end %% Fit worst-case perturbation @@ -364,11 +464,39 @@ if do_musyn %% ------------------------------------------------------------------------ % Measure Performance of mu synthesis design + if do_plots + fprintf(' - Plotting resulting controller...\n'); + + % Plot transfer functions + figure; hold on; + bode(ctrl.musyn.K(index.Ialpha(1), index.Ix)); + bode(ctrl.musyn.K(index.Ialpha(2), index.Ix)); + + bode(ctrl.musyn.K(index.Ialpha(1), index.Iy)); + bode(ctrl.musyn.K(index.Ialpha(2), index.Iy)); + + bode(ctrl.musyn.K(index.Iomega, index.Ix)); + bode(ctrl.musyn.K(index.Iomega, index.Iy)); + bode(ctrl.musyn.K(index.Iomega, index.Iz)); + + title(sprintf('\\bfseries %s Controller', ctrl.musyn.Name), ... + 'interpreter', 'latex'); + legend('$x \rightarrow \alpha_1$', ... + '$x \rightarrow \alpha_2$', ... + '$y \rightarrow \alpha_1$', ... + '$y \rightarrow \alpha_2$', ... + '$x \rightarrow \omega$', ... + '$y \rightarrow \omega$', ... + '$z \rightarrow \omega$', ... + 'interpreter', 'latex'); + grid on; + end + fprintf('Simulating nominal closed loop...\n'); - T = 60; + T = 40; nsamples = 5000; - do_noise = true; + do_noise = false; simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise); 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, ... diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx Binary files differindex d432718..fd94980 100644 --- a/uav_model_uncertain.slx +++ b/uav_model_uncertain.slx diff --git a/uav_model_uncertain_clp.slx b/uav_model_uncertain_clp.slx Binary files differindex 7bcf2d9..36be668 100644 --- a/uav_model_uncertain_clp.slx +++ b/uav_model_uncertain_clp.slx diff --git a/uav_params.m b/uav_params.m index e87e4fd..fdd519a 100644 --- a/uav_params.m +++ b/uav_params.m @@ -40,7 +40,7 @@ r_prop = 85e-3; % radius of propeller J_prop = .5 * m_prop * r_prop^2; params.mechanical = struct(... - 'Mass', 3.0, ... + 'Mass', 3.5, ... 'DuctRadius', 46e-3, ... 'DuctHeight', 171e-3, ... 'FlapZDistance', 98e-3, ... % flap distance along z from center of mass @@ -65,7 +65,7 @@ params.actuators = struct(... % IMU runs in NDOF mode params.measurements = struct(... - 'SensorFusionDelay', 50e-3, ... % in s + 'SensorFusionDelay', 30e-3, ... % in s 'LIDARAccuracy', 10e-3, ... % in m 'LIDARMaxDistance', 40, ... % inm 'LIDARBandwidth', 100, ... % in Hz for z position @@ -91,13 +91,13 @@ k_T = F_max / omega_max^2; % https://scienceworld.wolfram.com/physics/LiftCoefficient.html params.aerodynamics = struct(... 'ThrustOmegaProp', k_T, ... % in s^2 / (rad * N) - 'ThrustOmegaPropUncertainty', .05, ... % in % + 'ThrustOmegaPropUncertainty', .01, ... % in % 'FlapArea', 23e-3 * 10e-3, ... % in m^2 - 'FlapAreaUncertainty', .01, ... % in % - 'DragCoefficients', [1, .1], ... % TODO + 'FlapAreaUncertainty', .02, ... % in % + 'DragCoefficients', [0, 0], ... % TODO 'DragCoefficientsUncertainties', [.1, .1], ... % in % 'LiftCoefficient', 2 * pi, ... % TODO - 'LiftCoefficientUncertainty', .1 ...% in % + 'LiftCoefficientUncertainty', .05 ...% in % ); @@ -114,7 +114,7 @@ k = params.aerodynamics.ThrustOmegaProp; omega_hover = sqrt(m * g / k); params.linearization = struct(... - 'PadeApproxOrder', 2, ... + 'PadeApproxOrder', 3, ... 'Position', [0; 0; -2], ... % in inertial frame, z points down 'Velocity', [0; 0; 0], ... % in inertial frame 'Angles', [0; 0; pi / 4], ... % in body frame @@ -123,7 +123,7 @@ params.linearization = struct(... ); % ------------------------------------------------------------------------ -% Performance requirements +% Normalization (maximum values) params.normalization = struct(... 'HPosition', 1, ... % m diff --git a/uav_performance_hinf.m b/uav_performance_hinf.m index 0a12b72..ba2fd3b 100644 --- a/uav_performance_hinf.m +++ b/uav_performance_hinf.m @@ -13,50 +13,41 @@ % PERF Struct performance transfer functions -function [perf] = uav_performance(params, do_plots) +function [perf] = uav_performance_hinf(params, do_plots) % Laplace variable s = tf('s'); -alpha_max = params.actuators.ServoAbsMaxAngle; -alpha_max_omega = params.actuators.ServoNominalAngularVelocity; +% Bandwitdhs +bw_alpha = .7 * params.actuators.ServoNominalAngularVelocity; +bw_omega = 8; -T_xy = params.performance.HorizontalSettleTime; -T_z = params.performance.VerticalSettleTime; +bw_xy = .1; +bw_z = .4; -omega_nxy = 5 / T_xy; -omega_nz = 10 / T_z; +bw_xydot = .5; +bw_zdot = .1; -% W_Palpha = 1 / (s^2 + 2 * alpha_max_omega * s + alpha_max_omega^2); -% W_Palpha = (1 - W_Palpha / dcgain(W_Palpha)) * .8; -% W_Pomega = (1 - 1 / (T_z / 5 * s + 1)) * .1; +bw_phitheta = bw_xy; +bw_psi = .08; -W_Palpha = make_weight(alpha_max_omega, 15, 1.1, 3); -W_Pomega = make_weight(omega_nz, 50, 10); +% Inverse performance functions +W_Palpha = .25 / (s / bw_alpha + 1); +W_Pomega = .1 / (s / bw_omega + 1); -% zeta = 1; % Almost critically damped -% W_Pxy = 1 / (s^2 + 2 * zeta * omega_nxy * s + omega_nxy^2); -% W_Pxy = 1 * W_Pxy / dcgain(W_Pxy); -% W_Pz = 1 / (s^2 + 2 * zeta * omega_nz * s + omega_nz^2); -% W_Pz = 1 * W_Pz / dcgain(W_Pz); +W_Pxy = 5 * bw_xy^2 / (s^2 + 2 * .9 * bw_xy * s + bw_xy^2); +W_Pz = bw_z^2 / (s^2 + 2 * 1 * bw_z * s + bw_z^2); -W_Pxy = make_weight(omega_nxy, 2, 5); -W_Pz = make_weight(omega_nz, 1, 10); - -% Set a speed limit -W_Pxydot = .2 * tf(1, 1); -W_Pzdot = .2 * tf(1, 1); +W_Pxydot = tf(.1); % .2 / (s / bw_xydot + 1); +W_Pzdot = tf(.1); % .5 / (s / bw_zdot + 1); -W_Pphitheta = .01 * tf(1, [.1, 1]); -W_Ppsi = .01 * tf(1, 1); % don't care - -W_PTheta = tf(1, [.1, 1]) * eye(3); +W_Pphitheta = .01 / (s / bw_phitheta + 1); +W_Ppsi = tf(.1); % .1 / (s / bw_psi + 1); % Construct performance vector by combining xy and z W_PP = blkdiag(W_Pxy * eye(2), W_Pz); W_PPdot = blkdiag(W_Pxydot * eye(2), W_Pzdot); W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi); - perf = struct(... 'FlapAngle', W_Palpha * eye(4), ... 'Thrust', W_Pomega, ... diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m index aba2160..57237fa 100644 --- a/uav_performance_musyn.m +++ b/uav_performance_musyn.m @@ -19,30 +19,30 @@ function [perf] = uav_performance_musyn(params, do_plots) s = tf('s'); % Bandwitdhs -bw_alpha = params.actuators.ServoNominalAngularVelocity; -bw_omega = 10; +bw_alpha = .7 * params.actuators.ServoNominalAngularVelocity; +bw_omega = 8; -bw_xy = .05; -bw_z = .05; +bw_xy = .1; +bw_z = .4; bw_xydot = .5; bw_zdot = .1; -bw_phitheta = .1; +bw_phitheta = bw_xy; bw_psi = .08; % Inverse performance functions -W_Palpha = .2 / (s / bw_alpha + 1); -W_Pomega = .2 / (s / bw_omega + 1); +W_Palpha = .25 / (s / bw_alpha + 1); +W_Pomega = .1 / (s / bw_omega + 1); -W_Pxy = 8 * bw_xy^2 / (s^2 + 2 * 1 * bw_xy * s + bw_xy^2); -W_Pz = 1 * bw_z^2 / (s^2 + 2 * 1 * bw_z * s + bw_z^2); +W_Pxy = 2 * bw_xy^2 / (s^2 + 2 * .9 * bw_xy * s + bw_xy^2); +W_Pz = bw_z^2 / (s^2 + 2 * 1 * bw_z * s + bw_z^2); -W_Pxydot = .2 / (s / bw_xydot + 1); -W_Pzdot = .5 / (s / bw_zdot + 1); +W_Pxydot = tf(.1); % .2 / (s / bw_xydot + 1); +W_Pzdot = tf(.1); % .5 / (s / bw_zdot + 1); -W_Pphitheta = 1 / (s / bw_phitheta + 1); -W_Ppsi = .1 / (s / bw_psi + 1); +W_Pphitheta = .01 / (s / bw_phitheta + 1); +W_Ppsi = tf(.1); % .1 / (s / bw_psi + 1); % Construct performance vector by combining xy and z W_PP = blkdiag(W_Pxy * eye(2), W_Pz); @@ -60,14 +60,14 @@ if do_plots % Bode plots of performance requirements figure; hold on; - bodemag(W_Palpha); - bodemag(W_Pomega); - bodemag(W_Pxy); - bodemag(W_Pz); - bodemag(W_Pxydot); - bodemag(W_Pzdot); - bodemag(W_Pphitheta); - bodemag(W_Ppsi); + bodemag(1/W_Palpha); + bodemag(1/W_Pomega); + bodemag(1/W_Pxy); + bodemag(1/W_Pz); + bodemag(1/W_Pxydot); + bodemag(1/W_Pzdot); + bodemag(1/W_Pphitheta); + bodemag(1/W_Ppsi); grid on; legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ... @@ -75,7 +75,8 @@ if do_plots '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ... '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ... 'interpreter', 'latex', 'fontSize', 8); - title('Performance Requirements'); + title('\bfseries Inverse Performance Requirements', ... + 'interpreter', 'latex'); % Step response of position requirements figure; hold on; diff --git a/uav_sim_step.m b/uav_sim_step.m index 973a928..82c05e9 100644 --- a/uav_sim_step.m +++ b/uav_sim_step.m @@ -22,6 +22,16 @@ P_nom_clp = minreal(usys_clp(... model.uncertain.index.OutputPlots], ... model.uncertain.index.InputExogenous), [], false); +% Check that closed loop is actually stable +eigvals = eig(P_nom_clp.A); +nx = size(P_nom_clp.A, 1); + +for i = 1:nx + if real(eigvals(i)) >= 0 + error('Closed loop is not stable!'); + end +end + % Indices for exogenous inputs Iwwind = (1:3)'; Iwalpha = (4:7)'; @@ -236,10 +246,10 @@ if do_plots hold on; step(P_nom_clp(Iomega, Ir(3)) * to_rpm, T); - plot([0, T], [1, 1] * omega_min_rpm, 'r--'); - plot([0, T], [1, 1] * omega_max_rpm, 'r--'); + % plot([0, T], [1, 1] * omega_min_rpm, 'r--'); + % plot([0, T], [1, 1] * omega_max_rpm, 'r--'); grid on; - ylim([omega_min_rpm - 1, omega_max_rpm + 1]); + % ylim([omega_min_rpm - 1, omega_max_rpm + 1]); title('Vertical $z$ to Thruster $\omega$', 'Interpreter', 'latex'); ylabel('Angular Velocity (RPM)', 'Interpreter', 'latex'); xlabel('Time (seconds)', 'Interpreter', 'latex'); diff --git a/uav_uncertainty.m b/uav_uncertainty.m index b664cee..53cd975 100644 --- a/uav_uncertainty.m +++ b/uav_uncertainty.m @@ -28,20 +28,12 @@ eps_d = params.aerodynamics.DragCoefficientsUncertainties(1); eps_omega = max(.5 * eps_T, eps_r); eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega); -% TODO: set proper constraints -% band pass parameters for W_malpha -% wh = 20; % high freq -% wl = .1; % low freq +b = 10; +G_highpass = make_weight(b, 1, .1); -% W_malpha = eps_alpha * (s / wl) / ((s / wh + 1) * (s / wl + 1)); -% W_momega = eps_omega * 10 / (s + 10); -% W_mState = .01 * 10 / (s + 10); - -b = 1e3; - -W_malpha = make_weight(b, 1 / eps_alpha, 1e-2); -W_momega = tf(0); -W_mState = 1 - 1 / (s / 50 + 1); +W_malpha = eps_alpha * G_highpass; +W_momega = eps_omega * G_highpass; +W_mState = .1 * G_highpass; uncert = struct(... 'FlapAngle', W_malpha * eye(4), ... |