diff options
Diffstat (limited to '')
| -rw-r--r-- | pbhtest.m | 8 | ||||
| -rw-r--r-- | uav.m | 197 | ||||
| -rw-r--r-- | uav_model.m | 309 | ||||
| -rw-r--r-- | uav_model_uncertain.slx | bin | 62600 -> 62649 bytes | |||
| -rw-r--r-- | uav_model_uncertain_clp.slx | bin | 67378 -> 66499 bytes | |||
| -rw-r--r-- | uav_params.m | 31 | ||||
| -rw-r--r-- | uav_performance_hinf.m | 45 | ||||
| -rw-r--r-- | uav_performance_musyn.m | 57 | ||||
| -rw-r--r-- | uav_sim_step.m | 260 | ||||
| -rw-r--r-- | uav_uncertainty.m | 27 |
10 files changed, 579 insertions, 355 deletions
@@ -1,4 +1,4 @@ -% Do A PBH test on system. Given SYS returns: +% Do A PBH test on system. Given a state space model SYS returns: % % - Nr. of states % - Nr. of stable states @@ -23,9 +23,9 @@ end % Check system controllability / stabilizability Wc = ctrb(sys); nctrb = rank(Wc); +nustab = 0; if nctrb < nx % Is the system at least stabilizable? - nustab = 0; for i = 1:nx if real(eigvals(i)) >= 0 % PBH test @@ -40,13 +40,13 @@ end % Check system observability / detectability Wo = obsv(sys); nobsv = rank(Wo); +nudetb = 0; if nobsv < nx % is the system at least detectable? - nudetb = 0; for i = 1:nx if real(eigvals(i)) >= 0 % PBH test - W = [sys.C; (sys.A - eigvals(i) * eye(nx))]; + W = [(sys.A' - eigvals(i) * eye(nx)), sys.C']; if rank(W) < nx nudetb = nudetb + 1; end @@ -9,8 +9,8 @@ clear; clc; close all; s = tf('s'); do_plots = true; % runs faster without -do_hinf = false; % midterm -do_musyn = true; % endterm +do_hinf = true; +do_musyn = true; fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n') fprintf('Will do:\n') @@ -47,15 +47,12 @@ fprintf('Generating stability requirements...\n') uncert = uav_uncertainty(params, do_plots); %% ------------------------------------------------------------------------ -% Create UAV model - -fprintf('Generating system model...\n'); -model = uav_model(params, perf_musyn, uncert); - -%% ------------------------------------------------------------------------ % Perform H-infinity design if do_hinf + fprintf('Generating system model for H-infinity design...\n'); + model = uav_model(params, perf_hinf, uncert); + fprintf('Performing H-infinty controller design...\n') idx = model.uncertain.index; @@ -125,17 +122,17 @@ if do_hinf fprintf('Simulating closed loop...\n'); - T = 40; - nsamples = 800; + T = 60; + nsamples = 5000; do_noise = false; - simout = uav_sim_step(params, model, ctrl.hinf, nsamples, T, do_plots, do_noise); + simout = uav_sim_step(params, model, ctrl.hinf, uncert, nsamples, T, do_plots, do_noise); fprintf(' - Writing simulation results...\n'); cols = [ simout.StepX(:, simout.index.Position), ... simout.StepX(:, simout.index.Velocity), ... - simout.StepX(:, simout.index.FlapAngles) * 180 / pi, ... - simout.StepX(:, simout.index.Angles) * 180 / pi]; + simout.StepX(:, simout.index.PlotAlpha) * 180 / pi, ... + simout.StepX(:, simout.index.EulerAngles) * 180 / pi]; writematrix([simout.Time', cols], 'fig/stepsim.dat', 'Delimiter', 'tab') end @@ -144,6 +141,8 @@ end drawnow; if do_musyn + fprintf('Generating system model for mu-synthesis design...\n'); + model = uav_model(params, perf_musyn, uncert); fprintf('Performing mu-synthesis controller design...\n') @@ -157,12 +156,12 @@ if do_musyn % Options for H-infinity nmeas = model.uncertain.Ny; nctrl = model.uncertain.Nu; - hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ... + hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ... 'AutoScale', 'on', 'RelTol', 1e-2); % Frequency raster resolution to fit D scales - omega_max = 3; - omega_min = -2; + omega_max = 2.5; + omega_min = -3.5; nsamples = (omega_max - omega_min) * 100; omega = logspace(omega_min, omega_max, nsamples); @@ -172,10 +171,6 @@ if do_musyn D_left = tf(eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny)); D_right = tf(eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu)); - % Maximum number of D-K iterations - niters = 5; - fprintf(' - Will do (max) %d iterations.\n', niters); - % Maximum degree of D-scales and error d_scales_max_degree = 4; d_scales_max_err = 15; @@ -203,21 +198,25 @@ if do_musyn gamma_max = 20; gamma_min = .8; + % Maximum number of D-K iterations + niters = 4; + fprintf(' - Will do (max) %d iterations.\n', niters); + %% Run D-K iteration % hand tuned degrees, inf means will pick automatically best fit % according to parameters given above d_scales_degrees = { - 0, 0, 0, inf, inf; % alpha - 0, inf, inf, inf, inf; % omega - 0, inf, inf, inf, inf; % state - 0, 0, 0, inf, inf; % perf + 0, 0, 0, 0, 0, inf, inf; % alpha + 1, 1, 3, 3, 0, inf, inf; % omega + 0, 1, 0, 0, 0, inf, inf; % state + 0, 0, 0, 0, 0, inf, inf; % perf }; if size(d_scales_degrees, 2) < niters error('Number of columns of d_scales_degrees must match niter'); end - + % if iterations fails set warmit and re-run this code section if warmit > 0 fprintf("\n"); @@ -225,7 +224,7 @@ if do_musyn K = K_prev{warmit}; gamma_max = gamma_prev{warmit}; - + mu_bounds_rp = mu_bounds_rp_prev{warmit}; mu_bounds_rs = mu_bounds_rs_prev{warmit}; @@ -239,6 +238,7 @@ if do_musyn dkstart = tic; for it = s:niters + fprintf('\n'); fprintf(' * Running D-K iteration %d / %d...\n', it, niters); itstart = tic(); @@ -250,8 +250,7 @@ if do_musyn P_scaled = minreal(D_left * P * inv(D_right), [], false); [P_scaled, ~] = prescale(P_scaled, omega_range); n = size(P_scaled.A, 1); - - % disabled + if false && n > scaled_plant_reduce_ord R = reducespec(P_scaled, 'balanced'); % or ncf R.Options.FreqIntervals = [omega_range{1}, omega_range{2}]; @@ -265,56 +264,66 @@ if do_musyn fprintf(' Scaled plant was reduced from order %d to %d\n', ... n, size(P_scaled.A, 1)) + + % update n + n = size(P_scaled.A, 1); end - % Check scaled plant meets H-infinity assumptions - A = P_scaled(idx.OutputUncertain, idx.InputUncertain); - Bu = P_scaled(idx.OutputUncertain, idx.InputNominal); - Cy = P_scaled(idx.OutputNominal, idx.InputUncertain); + % For higher number of states we run into numerical problems + if n < 70 + % Scaled plant (whole) + [nx, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(P_scaled); - [~, ~, ~, nustab, ~, nudetb] = pbhtest(A); - if nustab > 0 || nudetb > 0 - fprintf(' Scaled plant has unstabilizable or undetectable A.\n'); - end + fprintf(' Scaled plant has %d modes:\n', nx); + fprintf(' %d stable, %d unstable\n', nsta, nx - nsta); + fprintf(' %d controllable, %d unstabilizable\n', nctrb, nustab); + fprintf(' %d observable, %d undetectable\n', nobsv, nudetb); - [~, ~, ~, nustab, ~, nudetb] = pbhtest(Bu); - if nustab > 0 || nudetb > 0 - fprintf(' Scaled plant has unstabilizable or undetectable Bu.\n'); - end + % Check scaled plant parts meet H-infinity assumptions + A = P_scaled(idx.OutputUncertain, idx.InputUncertain); + Bu = P_scaled(idx.OutputUncertain, idx.InputNominal); + Cy = P_scaled(idx.OutputNominal, idx.InputUncertain); - [~, ~, ~, nustab, ~, nudetb] = pbhtest(Cy); - if nustab > 0 || nudetb > 0 - fprintf(' Scaled plant has unstabilizable or undetectable Cy.\n'); - end - - Deu = P_scaled(idx.OutputError, idx.InputNominal); - Dyw = P_scaled(idx.OutputNominal, idx.InputExogenous); + [~, ~, ~, A_nustab, ~, A_nudetb] = pbhtest(A); + [~, ~, ~, Bu_nustab, ~, Bu_nudetb] = pbhtest(Bu); + [~, ~, ~, Cy_nustab, ~, Cy_nudetb] = pbhtest(Cy); - if rank(ctrb(Deu)) < length(Deu.A) || rank(obsv(Deu)) < lenth(Deu.A) - fprintf(' Scaled plant has Deu that is not full rank.\n'); - end + Deu = P_scaled(idx.OutputError, idx.InputNominal); + Dyw = P_scaled(idx.OutputNominal, idx.InputExogenous); - if rank(ctrb(Dyw)) < length(Dyw.A) || rank(obsv(Dyw)) < lenth(Dyw.A) - fprintf(' Scaled plant has Dyw that is not full rank.\n'); - end + Deu_fullrank = rank(ctrb(Deu)) < length(Deu.A) || rank(obsv(Deu)) < lenth(Deu.A); + Dyw_fullrank = rank(ctrb(Dyw)) < length(Dyw.A) || rank(obsv(Dyw)) < lenth(Dyw.A); - % Scaled pant - eigvals = eig(P_scaled.A); - stable_modes = 0; - for i = 1:n - if real(eigvals(i)) < 0 - stable_modes = stable_modes +1; + if A_nustab > 0 || A_nudetb > 0 + fprintf(' A has %d unstabilizable modes and %d undetectable modes!\n', ... + A_nustab, A_nudetb); + end + + if Bu_nustab > 0 || Bu_nudetb > 0 + fprintf(' Bu has %d unstabilizable modes and %d undetectable modes!\n', ... + Bu_nustab, Bu_nudetb); end - end - % For higher number of states we get numerical problems - if n < 100 - ctrb_modes = rank(ctrb(P_scaled)); - obsv_modes = rank(obsv(P_scaled)); + if Cy_nustab > 0 || Cy_nudetb > 0 + fprintf(' Cy has %d unstabilizable modes and %d undetectable modes!\n', ... + Cy_nustab, Cy_nudetb); + end - fprintf(' Scaled plant has %d modes: %d ctrb %d obsv %d stable %d unstable.\n', ... - n, ctrb_modes, obsv_modes, stable_modes, n - stable_modes); + if ~ Deu_fullrank + fprintf(' Deu is not full rank!\n'); + end + + if ~ Dyw_fullrank + fprintf(' Dyw is not full rank!\n'); + end else + eigvals = eig(P_scaled.A); + stable_modes = 0; + for i = 1:n + if real(eigvals(i)) < 0 + stable_modes = stable_modes +1; + end + end fprintf(' Scaled plant has %d modes: %d stable %d unstable.\n', ... n, stable_modes, n - stable_modes); end @@ -325,12 +334,12 @@ if do_musyn gamma_prev{it} = gamma; end - gamma_max=inf; - fprintf(' Running H-infinity with gamma in [%g, %g]\n', ... - gamma_min, gamma_max); + [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, hinfopt); + + % fprintf(' Running H-infinity with gamma in [%g, %g]\n', gamma_min, gamma_max); + % [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, [gamma_min, gamma_max], hinfopt); + % gamma_max = 1.2 * gamma; - [K, ~, gamma, ~] = hinfsyn(P_scaled, nmeas, nctrl, [gamma_min, gamma_max], hinfopt); - gamma_max = 2 * gamma; fprintf(' H-infinity synthesis gamma: %g\n', gamma); if gamma == inf error('Failed to synethesize H-infinity controller'); @@ -364,13 +373,13 @@ if do_musyn if do_plots fprintf(' Plotting SSV mu\n'); figure(100); hold on; - + bodemag(mu_bounds_rp(1,1)); mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{P,%d}$', it)}; - + bodemag(mu_bounds_rs(1,1), 'k:'); mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{S,%d}$', it)}; - + title('\bfseries $\mu_\Delta(\omega)$ for both Stability and Performance', ... 'interpreter', 'latex'); legend(mu_plot_legend, 'interpreter', 'latex'); @@ -396,6 +405,11 @@ if do_musyn 'mu_rp', mu_rp, 'mu_rs', mu_rs); end + % No need to find scales for last iteration + if it == niters + break; + end + % Fit D-scales [D_left_frd, D_right_frd] = mussvunwrap(mu_info_rp); @@ -428,7 +442,7 @@ if do_musyn % for each block for j = 1:4 fprintf(' %s', D_names{j}); - + % tuned by hand? if d_scales_degrees{j, it} < inf % D_fit = fitmagfrd(D_frd{j}, d_scales_degrees{j, it}); @@ -451,11 +465,11 @@ if do_musyn % 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 @@ -464,7 +478,7 @@ if do_musyn % the order of the D-scales. improvement = abs(best_fit_err - fit_err); improvement_p = improvement / best_fit_err; - + if improvement_p > d_scales_improvement_p ... || best_fit_err == inf @@ -473,7 +487,7 @@ if do_musyn D_fitted{j} = D_fit; end end - + if fit_err / D_max_sv{j} < d_scales_max_err_p ... || fit_err < d_scales_max_err break; @@ -489,15 +503,15 @@ if do_musyn D_left_prev{it} = D_left; D_right_prev{it} = D_right; - D_left = blkdiag(D_fitted{1} * eye(4), ... - D_fitted{2} * eye(1), ... - D_fitted{3} * eye(12), ... + D_left = blkdiag(D_fitted{1} * eye(4), ... % alpha uncert + D_fitted{2} * eye(1), ... % omega uncert + D_fitted{3} * eye(9), ... % state uncert D_fitted{4} * eye(14), ... eye(12)); - D_right = blkdiag(D_fitted{1} * eye(4), ... - D_fitted{2} * eye(1), ... - D_fitted{3} * eye(12), ... + D_right = blkdiag(D_fitted{1} * eye(4), ... % alpha uncert + D_fitted{2} * eye(1), ... % omega uncert + D_fitted{3} * eye(9), ... % state uncert D_fitted{4} * eye(10), ... eye(5)); @@ -581,6 +595,15 @@ if do_musyn %% ------------------------------------------------------------------------ % Measure Performance of mu synthesis design + 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 ... + ); + if do_plots fprintf(' - Plotting resulting controller...\n'); @@ -611,11 +634,11 @@ if do_musyn fprintf('Simulating nominal closed loop...\n'); - T = 40; + T = 60; nsamples = 5000; do_noise = false; - simout = uav_sim_step(params, model, ctrl.musyn, nsamples, T, do_plots, do_noise); + simout = uav_sim_step(params, model, ctrl.musyn, uncert, nsamples, T, do_plots, do_noise); fprintf('Simulating worst-case closed loop...\n'); diff --git a/uav_model.m b/uav_model.m index 6a7f9da..af0be20 100644 --- a/uav_model.m +++ b/uav_model.m @@ -221,16 +221,13 @@ C = eye(size(A)); D = zeros(12, 5); % ------------------------------------------------------------------------ -% Model actuators +% Model actuators (normalized to [-1, 1]!) -% TODO: better model? -w = params.actuators.ServoNominalAngularVelocity; -zeta = 1; -G_servo = tf(w^2, [1, 2 * zeta * w, w^2]); +T = params.actuators.ServoSecondsTo60Deg; +G_servo = tf(1, [T, 1]); -w = 60; -zeta = 1; -G_prop = tf(w^2, [1, 2 * zeta * w, w^2]); +% TODO: measurements for propeller +G_prop = tf(1, [.1, 1]); model.actuators = struct( ... 'FlapServo', G_servo, ... @@ -240,28 +237,43 @@ model.actuators = struct( ... % ------------------------------------------------------------------------ % Scale inputs and outputs of linearized model +% All scaling matrices scale UP FROM RANGE (-1, 1), i.e. for outputs they can +% be used as-is, while for inputs they need to be inverted + S_actuators = blkdiag(... - eye(4) * 1 / params.actuators.ServoAbsMaxAngle, ... - eye(1) * 1 / params.actuators.PropellerMaxAngularVelocity); + eye(4) * params.normalization.FlapAngle, ... + eye(1) * params.normalization.ThrustAngularVelocity ... +); -S_state = blkdiag(... +S_positions = blkdiag(... eye(2) * params.normalization.HPosition, ... - eye(1) * params.normalization.VPosition, ... + eye(1) * params.normalization.VPosition ... +); + +S_velocities = blkdiag(... eye(2) * params.normalization.HSpeed, ... - eye(1) * params.normalization.VSpeed, ... + eye(1) * params.normalization.VSpeed ... +); + +S_angles = blkdiag(... eye(2) * params.normalization.PitchRollAngle, ... - eye(1) * params.normalization.YawAngle, ... - eye(3) * params.normalization.AngularRate); + eye(1) * params.normalization.YawAngle ... +); + +S_angular_velocities = eye(3) * params.normalization.AngularRate; + +S_state = blkdiag(S_positions, S_velocities, S_angles, S_angular_velocities); % Scale system matrices to have inputs and outputs between zero and one -B = B * inv(S_actuators); +B = B * S_actuators; C = inv(S_state) * C; -D = D * inv(S_actuators); +D = D * S_actuators; % Create state space object T = params.measurements.SensorFusionDelay; n = params.linearization.PadeApproxOrder; sys = pade(ss(A, B, C, D, 'OutputDelay', T), n); +% sys = ss(A, B, C, D); % Add actuators sys = sys * model.actuators.StateSpace; @@ -279,7 +291,7 @@ model.linear = struct(... 'State', xi, 'Inputs', u, ... % state and input variables 'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized 'StateSpace', sys, ... % state space objec - 'InputScalingMatrix', S_actuators, 'OutputScalingMatrix', S_state ... + 'InputScaling', S_actuators, 'OutputScaling', S_state ... ); % ------------------------------------------------------------------------ @@ -293,7 +305,7 @@ fprintf(' %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab); fprintf(' %d observable modes, %d undetectable modes.\n', nobsv, nustab); if nctrb < 12 - error('Linearized model has less than 12 controllable modes!'); + fprintf(' Linearized model has less than 12 controllable modes!\n'); end if nustab > 0 || nudetb > 0 @@ -301,11 +313,6 @@ if nustab > 0 || nudetb > 0 end % ------------------------------------------------------------------------ -% Compute absolute value of error caused by linearization around set point - -% TODO - -% ------------------------------------------------------------------------ % Add uncertainties using SIMULINK model % Load simulink model with uncertainties and pass in parameters @@ -320,13 +327,13 @@ hws.assignin('uncert', uncert); % Get uncertain model ulmod = linmod('uav_model_uncertain'); -usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d); +usys = minreal(ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d), [], false); % Specify uncertainty block structure for mussv command blk_stab = [ 4, 4; % alpha uncert, full 1, 1; % omega uncert, full - 12, 12; % state uncert, full + 9, 9; % state uncert, full ]; blk_perf = [ @@ -334,29 +341,17 @@ blk_perf = [ 10, 14 % always full ]; -% ------------------------------------------------------------------------ -% Scale inputs and outputs of uncertain model - -% Scaling of reference is same as position -S_ref = blkdiag(... - eye(2) * 1 / params.normalization.HPosition, ... - eye(1) * 1 / params.normalization.VPosition); - -% TODO: finish -S_uncert_out = blkdiag(... - S_actuators, ... - S_state, ... - S_actuators, ... - S_state(1:9, 1:9)); - % Save uncertain model model.uncertain = struct(... - ... % 'Simulink', ulmod, ... + 'Simulink', ulmod, ... 'BlockStructure', blk_stab, ... 'BlockStructurePerf', blk_perf, ... 'StateSpace', usys ... ); +% ------------------------------------------------------------------------ +% Create indices to take subsystems + % The uncertain system is partitioned into the following matrix % % [ z ] [ A B_w B_u ] [ v ] @@ -369,33 +364,189 @@ model.uncertain = struct(... % - model.uncertain.Simulink.InputName(model.uncertain.index.InputX) % - model.uncertain.Simulink.OutputName(model.uncertain.index.OutputX) % -% Function make_idx(start, size) is defined below. -model.uncertain.index = struct(... - 'InputUncertain', make_idx( 1, 17), ... % 'v' inputs - 'InputDisturbance', make_idx(18, 7), ... % 'w' inputs for noise - 'InputReference', make_idx(25, 3), ... % 'w' inputs for reference - 'InputExogenous', make_idx(18, 10), ... % 'w' inputs (all of them) - 'InputNominal', make_idx(28, 5), ... % 'u' inputs - 'OutputUncertain', make_idx( 1, 17), ... % 'z' outputs - 'OutputError', make_idx(18, 14), ... % 'e' outputs - 'OutputNominal', make_idx(32, 12), ... % 'y' outputs - 'OutputPlots', make_idx(44, 10) ... % 'p' outputs for plots in closed loop + +% Size of signals to generate indices +input_dims = struct(... + 'InputUncertainAlpha', 4, ... % 'v' inputs + 'InputUncertainOmega', 1, ... % . + 'InputUncertainState', 9, ... % . + 'InputDisturbanceWind', 3, ... % 'w' inputs + 'InputDisturbanceAlpha', 4, ... % . + 'InputReference', 3, ... % . + 'InputNominalAlpha', 4, ... % 'u' inputs + 'InputNominalOmega', 1 ... % . ); -idx = model.uncertain.index; +output_dims = struct(... + 'OutputUncertainAlpha', 4, ... % 'z' outputs + 'OutputUncertainOmega', 1, ... % . + 'OutputUncertainState', 9, ... % . + 'OutputErrorAlpha', 4, ... % 'e' outputs + 'OutputErrorOmega', 1, ... % . + 'OutputErrorPosition', 3, ... % . + 'OutputErrorVelocity', 3, ... % . + 'OutputErrorEulerAngles', 3, ... % . + 'OutputNominalPosition', 3, ... % 'y' outputs + 'OutputNominalVelocity', 3, ... % . + 'OutputNominalEulerAngles', 3, ... % . + 'OutputNominalAngularRates', 3, ... % . + 'OutputPlotAlpha', 4, ... % plot / "debugging" outputs + 'OutputPlotOmega', 1, ... % . + 'OutputPlotReference', 3, ... % . + 'OutputPlotPosition', 3 ... % . +); -% Number of inputs -model.uncertain.Nv = max(size(idx.InputUncertain)); -model.uncertain.Nw = max(size(idx.InputExogenous)); -model.uncertain.Nu = max(size(idx.InputNominal)); +model.uncertain.dims = struct(... + 'input', input_dims, ... + 'output', output_dims ... +); + +% model.uncertain.dims.OutputError = sum([ +% model.uncertain.dims.OutputErrorAlpha; +% model.uncertain.dims.OutputErrorOmega; +% model.uncertain.dims.OutputErrorPosition; +% model.uncertain.dims.OutputErrorVelocity; +% model.uncertain.dims.OutputErrorEulerAngles; +% ]); +% +% model.uncertain.dims.OutputNominal; = sum([ +% model.uncertain.dims.OutputNominalPosition; +% model.uncertain.dims.OutputNominalVelocity; +% model.uncertain.dims.OutputNominalEulerAngles; +% model.uncertain.dims.OutputNominalAngularRates; +% ]); +% +% model.uncertain.dims.OutputPlot = sum([ +% model.uncertain.dims.OutputPlotAlpha; +% model.uncertain.dims.OutputPlotOmega; +% model.uncertain.dims.OutputPlotReference; +% ]); + +% Create indices +model.uncertain.index = struct(); + +% Create indices for inputs +start = 1; fields = fieldnames(input_dims); +for f = fields' + dim = getfield(input_dims, f{1}); + i = (start:(start + dim - 1))'; + model.uncertain.index = setfield(model.uncertain.index, f{1}, i); + start = start + dim; +end + +% Create indices for output +start = 1; fields = fieldnames(output_dims); +for f = fields' + dim = getfield(output_dims, f{1}); + i = (start:(start + dim - 1))'; + model.uncertain.index = setfield(model.uncertain.index, f{1}, i); + start = start + dim; +end + +% Create groups of inputs +model.uncertain.index.InputUncertain = [ + model.uncertain.index.InputUncertainAlpha; + model.uncertain.index.InputUncertainOmega; + model.uncertain.index.InputUncertainState; +]; + +model.uncertain.index.InputDisturbance = [ + model.uncertain.index.InputDisturbanceWind; + model.uncertain.index.InputDisturbanceAlpha; +]; + +model.uncertain.index.InputExogenous = [ + model.uncertain.index.InputDisturbance; + model.uncertain.index.InputReference; +]; + +model.uncertain.index.InputNominal = [ + model.uncertain.index.InputNominalAlpha; + model.uncertain.index.InputNominalOmega; +]; -model.uncertain.Nr = max(size(idx.InputReference)); +% Create groups of outputs +model.uncertain.index.OutputUncertain = [ + model.uncertain.index.OutputUncertainAlpha; + model.uncertain.index.OutputUncertainOmega; + model.uncertain.index.OutputUncertainState; +]; + +model.uncertain.index.OutputError = [ + model.uncertain.index.OutputErrorAlpha; + model.uncertain.index.OutputErrorOmega; + model.uncertain.index.OutputErrorPosition; + model.uncertain.index.OutputErrorVelocity; + model.uncertain.index.OutputErrorEulerAngles; +]; + +model.uncertain.index.OutputNominal = [ + model.uncertain.index.OutputNominalPosition; + model.uncertain.index.OutputNominalVelocity; + model.uncertain.index.OutputNominalEulerAngles; + model.uncertain.index.OutputNominalAngularRates; +]; + +model.uncertain.index.OutputPlot = [ + model.uncertain.index.OutputPlotAlpha; + model.uncertain.index.OutputPlotOmega; + model.uncertain.index.OutputPlotReference; +]; + +% Number of inputs +model.uncertain.Nv = max(size(model.uncertain.index.InputUncertain)); +model.uncertain.Nw = max(size(model.uncertain.index.InputExogenous)); +model.uncertain.Nu = max(size(model.uncertain.index.InputNominal)); +model.uncertain.Nr = max(size(model.uncertain.index.InputReference)); % size of noise is (Nw - Nr) % Number of outputs -model.uncertain.Nz = max(size(idx.OutputUncertain)); -model.uncertain.Ne = max(size(idx.OutputError)); -model.uncertain.Ny = max(size(idx.OutputNominal)); +model.uncertain.Nz = max(size(model.uncertain.index.OutputUncertain)); +model.uncertain.Ne = max(size(model.uncertain.index.OutputError)); +model.uncertain.Ny = max(size(model.uncertain.index.OutputNominal)); + +% ------------------------------------------------------------------------ +% Scaling matrices for uncertain model + +% Note howerver that the uncertain (SIMULINK) model is entirely described in +% normalized coordinates, so these matrices are to scale the result e.g. for +% plotting. + +% Input matrices scale DOWN TO range (-1, 1) +% Output matrices scale UP FROM range (-1, 1) + +model.uncertain.scaling = struct(); + +% TODO Scale for 'v' inputs if necessary + +% Scale for 'w' inputs +model.uncertain.scaling.InputUncertainScaling = blkdiag(... + S_actuators, ... + S_positions ... % reference use same as position +); + +% Scale for 'u' inputs +model.uncertain.scaling.InputNominalScaling = S_actuators; + +% TODO Scale for 'z' outputs if necessary + +% Scale for 'e' outputs +model.uncertain.scaling.OutputErrorScaling = blkdiag(... + S_actuators, ... + S_positions, ... + S_velocities, ... + S_angles ... +); + +% Scale for 'y' outputs +model.uncertain.scaling.OutputNominalScaling = S_state; + +% Scale for 'p' outputs +model.uncertain.scaling.OutputPlotScaling = blkdiag(... + S_actuators, ... + S_positions, ... % reference same as position + S_positions ... +); % ------------------------------------------------------------------------ % Check properties of uncertain model @@ -408,7 +559,7 @@ fprintf(' %d controllable modes, %d unstabilizable modes.\n', nctrb, nustab); fprintf(' %d observable modes, %d undetectable modes.\n', nobsv, nustab); if nctrb < 12 - error('Uncertain model has less than 12 controllable modes!'); + fprintf(' Uncertain model has less than 12 controllable modes!'); end if nustab > 0 || nudetb > 0 @@ -424,8 +575,14 @@ A = model.uncertain.StateSpace(... [~, ~, ~, nustab, ~, nudetb] = pbhtest(A); -if nustab > 0 || nudetb > 0 - fprintf(' Uncertain system has undetectable or uncontrollable A.\n'); +if nustab > 0 + fprintf(' Uncertain system has unstabilizable A! (%d modes)\n', ... + nustab); +end + +if nudetb > 0 + fprintf(' Uncertain system has undetectable A! (%d modes)\n', ... + nudetb); end B_u = model.uncertain.StateSpace(... @@ -435,8 +592,14 @@ B_u = model.uncertain.StateSpace(... [~, ~, ~, nustab, ~, nudetb] = pbhtest(B_u); -if nustab > 0 || nudetb > 0 - fprintf(' Uncertain system has undetectable or uncontrollable Bu.\n'); +if nustab > 0 + fprintf(' Uncertain system has unstabilizable Bu! (%d modes)\n', ... + nustab); +end + +if nudetb > 0 + fprintf(' Uncertain system has undetectable Bu! (%d modes)\n', ... + nudetb); end C_y = model.uncertain.StateSpace(... @@ -446,8 +609,12 @@ C_y = model.uncertain.StateSpace(... [~, ~, ~, nustab, ~, nudetb] = pbhtest(C_y); -if nustab > 0 || nudetb > 0 - fprintf(' Uncertain system has undetectable or uncontrollable Cy.\n'); +if nustab > 0 + fprintf(' Uncertain system has unstabilizable Cy!\n'); +end + +if nudetb > 0 + fprintf(' Uncertain system has undetectable Cy!\n'); end @@ -474,8 +641,4 @@ end end -function [indices] = make_idx(start, size) - indices = (start:(start + size - 1))'; -end - % vim: ts=2 sw=2 et: diff --git a/uav_model_uncertain.slx b/uav_model_uncertain.slx Binary files differindex fd94980..6f1b050 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 36be668..10df3a6 100644 --- a/uav_model_uncertain_clp.slx +++ b/uav_model_uncertain_clp.slx diff --git a/uav_params.m b/uav_params.m index fdd519a..2ac187e 100644 --- a/uav_params.m +++ b/uav_params.m @@ -35,7 +35,7 @@ J = [ ] * 1e-9; % Approximate propeller with a disk -m_prop = 200e-3; % mass of propeller +m_prop = 500e-3; % mass of propeller r_prop = 85e-3; % radius of propeller J_prop = .5 * m_prop * r_prop^2; @@ -46,26 +46,23 @@ params.mechanical = struct(... 'FlapZDistance', 98e-3, ... % flap distance along z from center of mass 'InertiaTensor', J, ... 'GyroscopicInertiaZ', J_prop, ... % assume small angle - 'GyroscopicInertiaZUncertainty', .01 ... % in % + 'GyroscopicInertiaZUncertainty', .05 ... % in % ); % ------------------------------------------------------------------------ % Actuator limits and measurements % Servos usually give a "speed" in seconds / 60 degrees without load -servo_speed = 0.13; % seconds / 60 deg -servo_angular_velocity = (60 / servo_speed) * (pi / 180); % rad / s - params.actuators = struct(... 'PropellerMaxAngularVelocity', 620.7, ... % in rad / s 'ServoAbsMaxAngle', 20 * pi / 180, ... % in radians 'ServoMaxTorque', 4.3 * 1e-2, ... % in kg / m - 'ServoNominalAngularVelocity', servo_angular_velocity ... + 'ServoSecondsTo60Deg', 0.13 ... % in s / 60 deg ); % IMU runs in NDOF mode params.measurements = struct(... - 'SensorFusionDelay', 30e-3, ... % in s + 'SensorFusionDelay', 10e-3, ... % in s, from 100 Hz sampling rate 'LIDARAccuracy', 10e-3, ... % in m 'LIDARMaxDistance', 40, ... % inm 'LIDARBandwidth', 100, ... % in Hz for z position @@ -91,13 +88,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', .01, ... % in % + 'ThrustOmegaPropUncertainty', .05, ... % in % 'FlapArea', 23e-3 * 10e-3, ... % in m^2 - 'FlapAreaUncertainty', .02, ... % in % + 'FlapAreaUncertainty', .1, ... % in % 'DragCoefficients', [0, 0], ... % TODO 'DragCoefficientsUncertainties', [.1, .1], ... % in % 'LiftCoefficient', 2 * pi, ... % TODO - 'LiftCoefficientUncertainty', .05 ...% in % + 'LiftCoefficientUncertainty', .1 ...% in % ); @@ -126,14 +123,16 @@ params.linearization = struct(... % Normalization (maximum values) params.normalization = struct(... - 'HPosition', 1, ... % m - 'VPosition', 1, ...% m - 'HSpeed', 2, ... % m / s - 'VSpeed', 2, ... % m / s + 'FlapAngle', params.actuators.ServoAbsMaxAngle, ... + 'ThrustAngularVelocity', params.actuators.PropellerMaxAngularVelocity, ... + 'HPosition', .5, ... % m + 'VPosition', .5, ...% m + 'HSpeed', .5, ... % m / s + 'VSpeed', .5, ... % m / s 'PitchRollAngle', 10 * pi / 180, ... % rad 'YawAngle', 5 * pi / 180, ... % rad - 'AngularRate', 2 * pi / 180, ... % rad / s - 'WindSpeed', 2 ... % m / s + 'AngularRate', 1 * pi / 180, ... % rad / s + 'WindSpeed', .01 ... % m / s ); end % vim: ts=2 sw=2 et: diff --git a/uav_performance_hinf.m b/uav_performance_hinf.m index ba2fd3b..4a6de8a 100644 --- a/uav_performance_hinf.m +++ b/uav_performance_hinf.m @@ -19,41 +19,43 @@ function [perf] = uav_performance_hinf(params, do_plots) s = tf('s'); % Bandwitdhs -bw_alpha = .7 * params.actuators.ServoNominalAngularVelocity; -bw_omega = 8; +T_alpha = params.actuators.ServoSecondsTo60Deg; +T_omega = 0.1; -bw_xy = .1; -bw_z = .4; +T_xy = 5; +T_z = 10; -bw_xydot = .5; -bw_zdot = .1; +% Inverse performance functions +W_Palpha = make_weight(1/T_alpha, 10, 2); +W_Pomega = make_weight(1/T_omega, 10, 2); -bw_phitheta = bw_xy; -bw_psi = .08; +W_Rxy = 1 / ((s * T_xy)^2 + 2 * T_xy * .8 * s + 1); +W_Rz = 1 / (s * T_z + 1); -% Inverse performance functions -W_Palpha = .25 / (s / bw_alpha + 1); -W_Pomega = .1 / (s / bw_omega + 1); +W_Pxy = tf(5); +W_Pz = tf(1); -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_Pxydot = tf(.01); +W_Pzdot = tf(.01); -W_Pxydot = tf(.1); % .2 / (s / bw_xydot + 1); -W_Pzdot = tf(.1); % .5 / (s / bw_zdot + 1); - -W_Pphitheta = .01 / (s / bw_phitheta + 1); -W_Ppsi = tf(.1); % .1 / (s / bw_psi + 1); +W_Pphitheta = .001 / (s * T_xy + 1); +W_Ppsi = tf(.1); % Construct performance vector by combining xy and z +W_ref = blkdiag(W_Rxy * eye(2), W_Rz); + 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, ... + 'ReferenceFilter', W_ref, ... 'Position', W_PP, ... 'Velocity', W_PPdot, ... - 'Angles', W_PTheta); + 'Angles', W_PTheta ... +); if do_plots % Bode plots of performance requirements @@ -61,6 +63,8 @@ if do_plots bodemag(W_Palpha); bodemag(W_Pomega); + bodemag(W_Rxy); + bodemag(W_Rz); bodemag(W_Pxy); bodemag(W_Pz); bodemag(W_Pxydot); @@ -70,6 +74,7 @@ if do_plots grid on; legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ... + '$W_{R,xy}$', '$W_{R,z}$', ... '$W_{P,xy}$', '$W_{P,z}$', ... '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ... '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ... @@ -80,11 +85,13 @@ if do_plots % Step response of position requirements figure; hold on; step(W_Pxy); step(W_Pz); + step(W_Rxy); step(W_Rz); step(W_Pxydot); step(W_Pzdot); step(W_Palpha); step(W_Pomega); grid on; legend('$W_{P,xy}$', '$W_{P,z}$', ... + '$W_{R,xy}$', '$W_{R,z}$', ... '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ... '$W_{P,\alpha}$', '$W_{P,\omega}$', ... 'interpreter', 'latex', 'fontSize', 8); diff --git a/uav_performance_musyn.m b/uav_performance_musyn.m index 57237fa..5ad5fa5 100644 --- a/uav_performance_musyn.m +++ b/uav_performance_musyn.m @@ -19,32 +19,32 @@ function [perf] = uav_performance_musyn(params, do_plots) s = tf('s'); % Bandwitdhs -bw_alpha = .7 * params.actuators.ServoNominalAngularVelocity; -bw_omega = 8; +T_alpha = params.actuators.ServoSecondsTo60Deg; +T_omega = 0.1; -bw_xy = .1; -bw_z = .4; +T_xy = 8; +T_z = 10; -bw_xydot = .5; -bw_zdot = .1; +% Inverse performance functions -bw_phitheta = bw_xy; -bw_psi = .08; +W_Palpha = make_weight(1/T_alpha, 10, 4); +W_Pomega = make_weight(1/T_omega, 10, 2); -% Inverse performance functions -W_Palpha = .25 / (s / bw_alpha + 1); -W_Pomega = .1 / (s / bw_omega + 1); +W_Rxy = 1 / ((s * T_xy)^2 + 2 * T_xy * .8 * s + 1); +W_Rz = 1 / (s * T_z + 1); + +W_Pxy = tf(5); +W_Pz = tf(1); -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 = tf(.01); +W_Pzdot = tf(.1); -W_Pxydot = tf(.1); % .2 / (s / bw_xydot + 1); -W_Pzdot = tf(.1); % .5 / (s / bw_zdot + 1); - -W_Pphitheta = .01 / (s / bw_phitheta + 1); -W_Ppsi = tf(.1); % .1 / (s / bw_psi + 1); +W_Pphitheta = .001 / (s * T_xy + 1); +W_Ppsi = tf(.1); % Construct performance vector by combining xy and z +W_ref = blkdiag(W_Rxy * eye(2), W_Rz); + 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); @@ -52,6 +52,7 @@ W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi); perf = struct(... 'FlapAngle', W_Palpha * eye(4), ... 'Thrust', W_Pomega, ... + 'ReferenceFilter', W_ref, ... 'Position', W_PP, ... 'Velocity', W_PPdot, ... 'Angles', W_PTheta); @@ -100,4 +101,24 @@ if do_plots end end + +% Make a n-order performance weight function +% +% Arguments: +% OMEGA Cutting frequency (-3dB) +% A Magnitude at DC, i.e. |Wp(0)| +% M Magnitude at infinity, i.e. |Wp(inf)| +% ORD Order +function [Wp] = make_weight(omega, A, M, ord) + +if nargin > 3 + n = ord; +else + n = 1; +end + +s = tf('s'); +Wp = (s / (M^(1/n)) + omega)^n / (s + omega * A^(1/n))^n; + +end % vim: ts=2 sw=2 et: diff --git a/uav_sim_step.m b/uav_sim_step.m index 82c05e9..65fb36f 100644 --- a/uav_sim_step.m +++ b/uav_sim_step.m @@ -3,127 +3,141 @@ % Copyright (C) 2024, Naoki Sean Pross, ETH Zürich % This work is distributed under a permissive license, see LICENSE.txt -function [simout] = uav_sim_step_musyn(params, model, ctrl, nsamp, T, do_plots, do_noise) +function [simout] = uav_sim_step(params, model, ctrl, uncert, nsamp, T, do_plots, do_noise) % Load closed loop model and add controller % more or less equivalent to doing usys = lft(Pnom, K) h = load_system('uav_model_uncertain_clp'); hws = get_param('uav_model_uncertain_clp', 'modelworkspace'); -hws.assignin('K', ctrl.K); -ulmod_clp = linmod('uav_model_uncertain_clp'); -usys_clp = ss(ulmod_clp.a, ulmod_clp.b, ulmod_clp.c, ulmod_clp.d); +if isfield(ctrl, 'K') + hws.assignin('K', ctrl.K); +else + error('You need to provide a controller ctrl.K'); +end -% Take dynamics without uncertainty -% Also nominal ouput to make plots -P_nom_clp = minreal(usys_clp(... - [model.uncertain.index.OutputError; - model.uncertain.index.OutputNominal; - model.uncertain.index.OutputPlots], ... - model.uncertain.index.InputExogenous), [], false); +% There is an uncertainty block +if isfield(uncert, 'Delta') + hws.assignin('Delta', uncert.Delta); +else + fprintf(' - No uncertainty error Delta was provided, setting to zero.\n'); + Delta = tf(0) * zeros(sum(model.uncertain.BlockStructure)); + hws.assignin('Delta', Delta); +end -% Check that closed loop is actually stable -eigvals = eig(P_nom_clp.A); -nx = size(P_nom_clp.A, 1); +ulmod_clp = linmod('uav_model_uncertain_clp'); +P_clp = minreal(ss(ulmod_clp.a, ulmod_clp.b, ulmod_clp.c, ulmod_clp.d), [], false); -for i = 1:nx - if real(eigvals(i)) >= 0 +% Check that closed loop is actually stable +nx = size(P_clp.A, 1); +fprintf(' - Closed loop dynamics have %d states\n', nx); +if nx < 60 + [~, nsta, nctrb, nustab, nobsv, nudetb] = pbhtest(P_clp); + if nx ~= nsta error('Closed loop is not stable!'); end -end - -% Indices for exogenous inputs -Iwwind = (1:3)'; -Iwalpha = (4:7)'; -Ir = (8:10)'; -% Indices for outputs -Iealpha = (1:4)'; -Ieomega = 5; -IeP = (6:8)'; -IePdot = (9:11)'; -IeTheta = (12:14)'; - -% Indices for y outputs (for plots) -IP = (15:17)'; -IPdot = (18:20)'; -ITheta = (21:23)'; -IOmega = (24:26)'; + fprintf(' %d stable modes, %d unstable modes\n', nsta, nx - nsta); + fprintf(' %d controllable modes, %d unstabilizable\n', nctrb, nustab); + fprintf(' %d observable modes, %d undetectable\n', nobsv, nudetb); +else + eigvals = eig(P_clp.A); + for i = 1:nx + if real(eigvals(i)) >= 0 + error('Closed loop is not stable!'); + end + end +end -% Indices for p outputs (for plots) -Ialpha = (27:30)'; -Iomega = 31; +% Generate indices for closed loop plant +o = model.uncertain.dims.output; +i = model.uncertain.dims.input; + +dims = struct(... + 'ErrorAlpha', o.OutputErrorAlpha, ... + 'ErrorOmega', o.OutputErrorOmega, ... + 'ErrorPosition', o.OutputErrorPosition, ... + 'ErrorVelocity', o.OutputErrorVelocity, ... + 'ErrorEulerAngles', o.OutputErrorEulerAngles, ... + 'Position', o.OutputNominalPosition, ... + 'Velocity', o.OutputNominalVelocity, ... + 'EulerAngles', o.OutputNominalEulerAngles, ... + 'AngularRates', o.OutputNominalAngularRates, ... + 'PlotAlpha', o.OutputPlotAlpha, ... + 'PlotOmega', o.OutputPlotOmega, ... + 'PlotReference', o.OutputPlotReference, ... + 'PlotPosition', o.OutputPlotPosition, ... + 'InputAlpha', i.InputNominalAlpha, ... + 'InputOmega', i.InputNominalOmega ... +); + +idx = struct(); +start = 1; fields = fieldnames(dims); +for f = fields' + dim = getfield(dims, f{1}); + i = (start:(start + dim - 1))'; + idx = setfield(idx, f{1}, i); + start = start + dim; +end -Iualpha = (32:35)'; -Iuomega = 36; +% Input indices +idx.InputDisturbanceWind = (1:3)'; +idx.InputDisturbanceFlaps = (4:7)'; +idx.InputReference = (8:10)'; -noise = zeros(7, nsamp); % no noise +% Create noise +noise = zeros(7, nsamp); if do_noise - % Noise in percentage + % Noise (normalized) noise_alpha_amp = (.5 * (pi / 180)) / params.actuators.ServoAbsMaxAngle; - noise_wind_amp = .1; + noise_wind_amp = .01; noise = [noise_wind_amp * randn(3, nsamp); noise_alpha_amp * randn(4, nsamp)]; end +% Step size +step_size_h = .5 / params.normalization.HPosition; +step_size_v = .5 / params.normalization.VPosition; + % Create step inputs (normalized) -ref_step = .5 * ones(1, nsamp); % 1d step function +ref_step = ones(1, nsamp); % 1d step function -in_step_x = [ noise; ref_step; zeros(2, nsamp) ]; -in_step_y = [ noise; zeros(1, nsamp); ref_step; zeros(1, nsamp) ]; -in_step_z = [ noise; zeros(2, nsamp); ref_step ]; +in_step_x = [ noise; step_size_h * ref_step; zeros(2, nsamp) ]; +in_step_y = [ noise; zeros(1, nsamp); step_size_h * ref_step; zeros(1, nsamp) ]; +in_step_z = [ noise; zeros(2, nsamp); step_size_v * ref_step ]; % Simulation time -n_settle_times = 10; t = linspace(0, T, nsamp); +% Scale simulation outputs +S = blkdiag(... + model.uncertain.scaling.OutputErrorScaling, ... + model.uncertain.scaling.OutputNominalScaling, ... + model.uncertain.scaling.OutputPlotScaling, ... + model.uncertain.scaling.InputNominalScaling ... +); + % Simulate step responses -out_step_x_norm = lsim(P_nom_clp, in_step_x, t); -out_step_y_norm = lsim(P_nom_clp, in_step_y, t); -out_step_z_norm = lsim(P_nom_clp, in_step_z, t); - -% Scale outputs -S_actuators = blkdiag(... - eye(4) * params.actuators.ServoAbsMaxAngle, ... - eye(1) * params.actuators.PropellerMaxAngularVelocity); - -S_state = blkdiag(... - eye(2) * params.normalization.HPosition, ... - eye(1) * params.normalization.VPosition, ... - eye(2) * params.normalization.HSpeed, ... - eye(1) * params.normalization.VSpeed, ... - eye(2) * params.normalization.PitchRollAngle, ... - eye(1) * params.normalization.YawAngle, ... - eye(3) * params.normalization.AngularRate); - -S = blkdiag(S_actuators, S_state(1:9, 1:9), S_state, S_actuators, S_actuators); - -out_step_x = out_step_x_norm * S'; -out_step_y = out_step_y_norm * S'; -out_step_z = out_step_z_norm * S'; +out_step_x_norm = lsim(P_clp, in_step_x, t, 'foh'); +out_step_y_norm = lsim(P_clp, in_step_y, t, 'foh'); +out_step_z_norm = lsim(P_clp, in_step_z, t, 'foh'); + +out_step_x = out_step_x_norm * S; +out_step_y = out_step_y_norm * S; +out_step_z = out_step_z_norm * S; % Return simulation simout = struct(... 'Time', t, ... - 'StepX', out_step_x_norm, ... - 'StepY', out_step_y_norm, ... - 'StepZ', out_step_z_norm, ... + 'StepX', out_step_x, ... + 'StepY', out_step_y, ... + 'StepZ', out_step_z, ... + 'StepXNorm', out_step_x_norm, ... + 'StepYNorm', out_step_y_norm, ... + 'StepZNorm', out_step_z_norm, ... 'Simulink', ulmod_clp, ... - 'StateSpace', P_nom_clp); - -simout.index = struct(... - 'ErrorFlapAngles', Iealpha, ... - 'ErrorThrust', Ieomega , ... - 'ErrorPos', IeP, ... - 'ErrorVel', IePdot, ... - 'ErrorAngles', IeTheta, ... - 'Position', IP, ... - 'Velocity', IPdot, ... - 'Angles', ITheta, ... - 'FlapAngles', Ialpha, ... - 'Thruster', Iomega, ... - 'InputFlapAngles', Iualpha, ... - 'InputThruster', Iuomega); + 'StateSpace', P_clp, ... + 'index', idx); if do_plots % Conversion factors @@ -137,7 +151,6 @@ if do_plots ctrl.Name), 'Interpreter', 'latex'); % Plot limits - ref_value = .5; alpha_max_deg = params.actuators.ServoAbsMaxAngle * to_deg; euler_lim_deg = 1.5; omega_max_rpm = (params.actuators.PropellerMaxAngularVelocity ... @@ -147,10 +160,10 @@ if do_plots % Plot step response from x to alpha subplot(2, 3, 1); hold on; - plot(t, out_step_x(:, Ialpha(1)) * to_deg); - plot(t, out_step_x(:, Ialpha(2)) * to_deg); - plot(t, out_step_x(:, Ialpha(3)) * to_deg); - plot(t, out_step_x(:, Ialpha(4)) * to_deg); + plot(t, out_step_x(:, idx.PlotAlpha(1)) * to_deg); + plot(t, out_step_x(:, idx.PlotAlpha(2)) * to_deg); + plot(t, out_step_x(:, idx.PlotAlpha(3)) * to_deg); + plot(t, out_step_x(:, idx.PlotAlpha(4)) * to_deg); plot([0, T], [1, 1] * alpha_max_deg, 'r--'); plot([0, T], [-1, -1] * alpha_max_deg, 'r--'); grid on; @@ -164,10 +177,10 @@ if do_plots % Plot step response from y to alpha subplot(2, 3, 2); hold on; - plot(t, out_step_y(:, Ialpha(1)) * to_deg); - plot(t, out_step_y(:, Ialpha(2)) * to_deg); - plot(t, out_step_y(:, Ialpha(3)) * to_deg); - plot(t, out_step_y(:, Ialpha(4)) * to_deg); + plot(t, out_step_y(:, idx.PlotAlpha(1)) * to_deg); + plot(t, out_step_y(:, idx.PlotAlpha(2)) * to_deg); + plot(t, out_step_y(:, idx.PlotAlpha(3)) * to_deg); + plot(t, out_step_y(:, idx.PlotAlpha(4)) * to_deg); plot([0, T], [1, 1] * alpha_max_deg, 'r--'); plot([0, T], [-1, -1] * alpha_max_deg, 'r--'); grid on; @@ -181,10 +194,10 @@ if do_plots % Plot step response from z to alpha subplot(2, 3, 3); hold on; - plot(t, out_step_z(:, Ialpha(1)) * to_deg); - plot(t, out_step_z(:, Ialpha(2)) * to_deg); - plot(t, out_step_z(:, Ialpha(3)) * to_deg); - plot(t, out_step_z(:, Ialpha(4)) * to_deg); + plot(t, out_step_z(:, idx.PlotAlpha(1)) * to_deg); + plot(t, out_step_z(:, idx.PlotAlpha(2)) * to_deg); + plot(t, out_step_z(:, idx.PlotAlpha(3)) * to_deg); + plot(t, out_step_z(:, idx.PlotAlpha(4)) * to_deg); plot([0, T], [1, 1] * alpha_max_deg, 'r--'); plot([0, T], [-1, -1] * alpha_max_deg, 'r--'); grid on; @@ -198,9 +211,9 @@ if do_plots % Plot step response from x to Theta subplot(2, 3, 4); hold on; - plot(t, out_step_x(:, ITheta(1)) * to_deg); - plot(t, out_step_x(:, ITheta(2)) * to_deg); - plot(t, out_step_x(:, ITheta(3)) * to_deg); + plot(t, out_step_x(:, idx.EulerAngles(1)) * to_deg); + plot(t, out_step_x(:, idx.EulerAngles(2)) * to_deg); + plot(t, out_step_x(:, idx.EulerAngles(3)) * to_deg); grid on; xlim([0, T]); ylim([-euler_lim_deg, euler_lim_deg]); @@ -212,9 +225,9 @@ if do_plots % Plot step response from y to Theta subplot(2, 3, 5); hold on; - plot(t, out_step_y(:, ITheta(1)) * to_deg); - plot(t, out_step_y(:, ITheta(2)) * to_deg); - plot(t, out_step_y(:, ITheta(3)) * to_deg); + plot(t, out_step_y(:, idx.EulerAngles(1)) * to_deg); + plot(t, out_step_y(:, idx.EulerAngles(2)) * to_deg); + plot(t, out_step_y(:, idx.EulerAngles(3)) * to_deg); grid on; xlim([0, T]); ylim([-euler_lim_deg, euler_lim_deg]); @@ -226,9 +239,9 @@ if do_plots % Plot step response from z to Theta subplot(2, 3, 6); hold on; - plot(t, out_step_z(:, ITheta(1)) * to_deg); - plot(t, out_step_z(:, ITheta(2)) * to_deg); - plot(t, out_step_z(:, ITheta(3)) * to_deg); + plot(t, out_step_z(:, idx.EulerAngles(1)) * to_deg); + plot(t, out_step_z(:, idx.EulerAngles(2)) * to_deg); + plot(t, out_step_z(:, idx.EulerAngles(3)) * to_deg); grid on; xlim([0, T]); ylim([-euler_lim_deg, euler_lim_deg]); @@ -245,7 +258,7 @@ if do_plots ctrl.Name), 'Interpreter', 'latex'); hold on; - step(P_nom_clp(Iomega, Ir(3)) * to_rpm, T); + step(P_clp(idx.PlotOmega, idx.InputReference(3)) * to_rpm, T); % plot([0, T], [1, 1] * omega_min_rpm, 'r--'); % plot([0, T], [1, 1] * omega_max_rpm, 'r--'); grid on; @@ -263,20 +276,19 @@ if do_plots % Plot step response from horizontal reference to horizontal position subplot(2, 2, 1); hold on; - plot(t, out_step_x(:, IP(1))); - plot(t, out_step_y(:, IP(2))); - % plot([0, T], [1, 1] * ref_value, 'r:'); - % plot(t, out_step_xydes, 'r--'); + plot(t, out_step_x(:, idx.PlotPosition(1))); + plot(t, out_step_y(:, idx.PlotPosition(2))); + plot(t, out_step_x(:, idx.PlotReference(1)), '--'); grid on; - title('Horizontal Position Error', 'Interpreter', 'latex'); - ylabel('Error (meters)', 'Interpreter', 'latex'); + title('Horizontal Position', 'Interpreter', 'latex'); + ylabel('Distance (meters)', 'Interpreter', 'latex'); xlabel('Time (seconds)', 'Interpreter', 'latex'); legend('$x(t)$', '$y(t)$', 'Interpreter', 'latex'); % Plot step response horizontal reference to horizontal speed subplot(2, 2, 2); hold on; - plot(t, out_step_x(:, IPdot(1))); - plot(t, out_step_y(:, IPdot(2))); + plot(t, out_step_x(:, idx.Velocity(1))); + plot(t, out_step_y(:, idx.Velocity(2))); grid on; title('Horizontal Velocity', 'Interpreter', 'latex'); ylabel('Velocity (m / s)', 'Interpreter', 'latex'); @@ -285,18 +297,16 @@ if do_plots % Plot step response from vertical reference to vertical position subplot(2, 2, 3); hold on; - plot(t, out_step_z(:, IP(3))); - % plot([0, T], [1, 1] * ref_value, 'r:'); - % plot(t, out_step_zdes, 'r--'); + plot(t, out_step_z(:, idx.PlotPosition(3))); grid on; - title('Vertical Position Error', 'Interpreter', 'latex'); - ylabel('Error (meters)', 'Interpreter', 'latex'); + title('Vertical Position', 'Interpreter', 'latex'); + ylabel('Distance (meters)', 'Interpreter', 'latex'); xlabel('Time (seconds)', 'Interpreter', 'latex'); legend('$z(t)$', 'Interpreter', 'latex'); % Plot step response vertical reference to vertical speed subplot(2, 2, 4); hold on; - plot(t, out_step_z(:, IPdot(3))); + plot(t, out_step_z(:, idx.ErrorVelocity(3))); grid on; title('Vertical Velocity', 'Interpreter', 'latex'); ylabel('Velocity (m / s)', 'Interpreter', 'latex'); diff --git a/uav_uncertainty.m b/uav_uncertainty.m index 7cd7e38..03b7b3b 100644 --- a/uav_uncertainty.m +++ b/uav_uncertainty.m @@ -26,18 +26,18 @@ eps_d = params.aerodynamics.DragCoefficientsUncertainties(1); % eps_0 = params.aerodynamics.DragCoefficients(2); eps_omega = max(.5 * eps_T, eps_r); -eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega); +eps_alpha = max(eps_l + eps_S + 2 * eps_omega, eps_S + eps_d + eps_omega) b = 12; -G = make_weight(b, 2, .2); - -W_malpha = .1 * eps_alpha * G; -W_momega = .1 * eps_omega * G; -W_mState = blkdiag( ... - .05 * tf(1) * eye(3), ... - .01 * tf(1) * eye(3), ... - .001 * tf(1) * eye(3), ... - .05 * tf(1) * eye(3) ... +T = 1; +G = make_weight(b, 4, 1); + +W_malpha = eps_alpha * tf(1); +W_momega = eps_omega * tf(1); +W_mState = (1 - tf(1, [T, 1])) * blkdiag( ... + .2 * eye(3), ... + .1 * eye(3), ... + .2 * eye(3) ... ); uncert = struct(... @@ -56,12 +56,13 @@ if do_plots bodemag(W_mState(1,1)); bodemag(W_mState(4,4)); bodemag(W_mState(7,7)); - bodemag(W_mState(11,11)); grid on; legend('$W_{m,\alpha}$', '$W_{m,\omega}$', ... - '$W_{m,\mathbf{P}}$', '$W_{m,\mathbf{\dot{P}}}$', ... - '$W_{m,\mathbf{\Theta}}$', '$W_{m,\mathbf{\Omega}}$', ... + ... % '$W_{m,\mathbf{P}}$', ... + '$W_{m,\mathbf{\dot{P}}}$', ... + '$W_{m,\mathbf{\Theta}}$', ... + '$W_{m,\mathbf{\Omega}}$', ... 'interpreter', 'latex') title('\bfseries Stability Requirement (only for $\mu$-Synthesis)', ... 'interpreter', 'latex') |