diff options
Diffstat (limited to '')
| -rw-r--r-- | uav_model.m | 309 |
1 files changed, 236 insertions, 73 deletions
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: |