From 9bb230d7c8221bb69607d4310c5b679eec2ea0f0 Mon Sep 17 00:00:00 2001 From: Jason Moore Date: Wed, 30 Jan 2013 13:12:45 -0800 Subject: [PATCH] Added matlab code samples. All of these code samples currently are mis-identified in my repositories. I'm donating them to the cause. --- samples/Matlab/adapting_structural_model.m | 74 ++ samples/Matlab/bicycle_state_space.m | 168 +++ samples/Matlab/convert_variable.m | 68 ++ samples/Matlab/create_ieee_paper_plots.m | 1119 ++++++++++++++++++++ samples/Matlab/fit_adapt.m | 57 + samples/Matlab/fit_adapt_linear.m | 86 ++ samples/Matlab/fix_ps_linestyle.m | 109 ++ samples/Matlab/ieee.m | 13 + samples/Matlab/lane_change.m | 56 + samples/Matlab/load_bikes.m | 55 + samples/Matlab/load_data.m | 33 + samples/Matlab/overwrite_settings.m | 27 + samples/Matlab/par_text_to_struct.m | 27 + samples/Matlab/plant.m | 75 ++ samples/Matlab/test_system_state_space.m | 69 ++ samples/Matlab/varargin_to_structure.m | 37 + samples/Matlab/write_gains.m | 48 + 17 files changed, 2121 insertions(+) create mode 100644 samples/Matlab/adapting_structural_model.m create mode 100644 samples/Matlab/bicycle_state_space.m create mode 100644 samples/Matlab/convert_variable.m create mode 100644 samples/Matlab/create_ieee_paper_plots.m create mode 100644 samples/Matlab/fit_adapt.m create mode 100644 samples/Matlab/fit_adapt_linear.m create mode 100644 samples/Matlab/fix_ps_linestyle.m create mode 100644 samples/Matlab/ieee.m create mode 100644 samples/Matlab/lane_change.m create mode 100644 samples/Matlab/load_bikes.m create mode 100644 samples/Matlab/load_data.m create mode 100644 samples/Matlab/overwrite_settings.m create mode 100644 samples/Matlab/par_text_to_struct.m create mode 100644 samples/Matlab/plant.m create mode 100644 samples/Matlab/test_system_state_space.m create mode 100644 samples/Matlab/varargin_to_structure.m create mode 100644 samples/Matlab/write_gains.m diff --git a/samples/Matlab/adapting_structural_model.m b/samples/Matlab/adapting_structural_model.m new file mode 100644 index 00000000..5a4ae3cc --- /dev/null +++ b/samples/Matlab/adapting_structural_model.m @@ -0,0 +1,74 @@ +function [dx, y] = adapting_structural_model(t, x, u, varargin) +% +% Returns the time derivatives of the states and the output of the +% structural control model with an adapting controller. +% +% Parameters +% ---------- +% t : double +% The current time. +% x : double, size(8, 1) +% The current state. +% u : double, size(1, 1) +% The current input. +% varargin : cell array +% m1, m2, m3, m4, b1, b2, b3, b4 : double +% The slope of the four gains and the offset of the four gains. +% aux : cell array containing a single structure +% The structure contains: +% pars : double, size(1,9) +% The controller parameters. +% timeDelay : logical +% If true a 1st order Pade approximation of the human's time delay +% is included. +% plantFirst : integer +% The number of the first plant. +% plantSecond : integer +% The number of the second plant. +% m : double, size(2, 1) +% The slope of the transfer function adaption function. +% b : double, size(2, 1) +% The offset of the transfer function adaption function. +% +% Returns +% ------- +% dx : double, size(8, 1) +% The derivatives of the states. +% y : double, size(1, 1) +% The output, theta. + +% MATLAB SUCKS! This is unbelievable. On the first iteration varargin is 1x2 +% and after that it is 1x9. +%size(varargin, 2) + +% Unpack varargin. +aux = varargin{end}{1}; +m = zeros(4, 1); +b = zeros(4, 1); +for i=1:4 + if size(varargin, 2) == 2 + m(i) = varargin{1}(i); + b(i) = varargin{1}(i + 4); + elseif size(varargin, 2) == 9 + m(i) = varargin{i}; + b(i) = varargin{i + 4}; + else + display('Matlab is stupid.') + end +end + +% First compute the gains at this time. +aux.pars(1:4) = m .* t + b; +% Compute the controller. +Yp = human(aux.pars, aux.timeDelay); +% Compute the plant and this time. +c1 = aux.m(1) * t + aux.b(1) + 1e-10; +c2 = aux.m(2) * t + aux.b(2) + 1e-10; +Yc = parallel(c1 * plant(aux.plantFirst), c2 * plant(aux.plantSecond)); +% Compute the closed loop system. +Ys = feedback(Yp * Yc, 1); +% Convert to state space. +[A, B, C, D] = tf2ss(Ys.num{1}, Ys.den{1}); +% Compute the derivatives of the states and the outputs. +dx = A * x + B * u; +y = C * x + D * u; diff --git a/samples/Matlab/bicycle_state_space.m b/samples/Matlab/bicycle_state_space.m new file mode 100644 index 00000000..8bb965ac --- /dev/null +++ b/samples/Matlab/bicycle_state_space.m @@ -0,0 +1,168 @@ +function bicycle = bicycle_state_space(bicycle, speed, varargin) +% function bicycle = bicycle_state_space(bicycle, speed, varargin) +% +% Returns the state space system of the Whipple model linearized about the +% nominal configuration and the supplied speed. +% +% Parameters +% ---------- +% bicycle : char +% The name of a bicycle in the parameters directory. +% speed : double +% The forward speed at which to linearize the model about. +% varargin : char/cell array pairs, optional +% Specify a subset of states, inputs or outputs by setting one of the +% following: `states`, `inputs`, `outputs` as a cell array of +% chars which include the subset variable names. Beaware that not all +% state, input and output combinations are necessarily possible. +% Valid state names: 'xP', 'yP', 'psi', 'phi', 'thetaB', 'thetaR', 'delta', +% 'thetaF', 'phiDot', 'thetaRDot', 'deltaDot' +% Valid input names: 'tPhi', 'tDelta', 'fB' +% Valid output names: 'xP', 'yP', 'psi', 'phi', 'thetaB', 'thetaR', 'delta', +% 'thetaF', 'xPDot', 'yPDot', 'psiDot', 'phiDot', 'thetaBDot', +% 'thetaRDot', 'deltaDot', 'thetaFDot', 'xQ', 'yQ' +% +% Returns +% ------- +% bicycle : ss +% The state space model of the bicycle. +% +% Notes +% ----- +% The variable names are defined as in Meijaard2007. +% +% Examples +% -------- +% bicycle = bicycle_state_space('Benchmark', 5.0, ... +% 'states', {'phi', 'phiDot', 'delta', 'deltaDot'}, ... +% 'inputs', {'tDelta'}, ... +% 'outputs', {'delta', 'phi'}) + +% get the directory which this m-file is in +S = dbstack('-completenames'); +[CURRENT_DIRECTORY, ~, ~] = fileparts(S(1).file); + +% load the paramaters +par = par_text_to_struct([CURRENT_DIRECTORY filesep 'parameters' ... + filesep bicycle 'Par.txt']); + +% generate the state space matrices +[A, B, C, D] = whipple_pull_force_abcd(par, speed); + +% name the states, outputs and inputs +states = {'xP', + 'yP', + 'psi', + 'phi', + 'thetaB', + 'thetaR', + 'delta', + 'thetaF', + 'phiDot', + 'thetaRDot', + 'deltaDot'}; + +outputs = {'xP', + 'yP', + 'psi', + 'phi', + 'thetaB', + 'thetaR', + 'delta', + 'thetaF', + 'xPDot', + 'yPDot', + 'psiDot', + 'phiDot', + 'thetaBDot', + 'thetaRDot', + 'deltaDot', + 'thetaFDot', + 'xQ', + 'yQ'}; + +inputs = {'tPhi', + 'tDelta', + 'fB'}; + +defaultSettings.states = states; +defaultSettings.inputs = inputs; +defaultSettings.outputs = outputs; +% load in user supplied settings +if size(varargin, 2) >= 1 + userSettings = varargin_to_structure(varargin); +else + userSettings = struct(); +end +% combine the defaults with the user settings +settings = overwrite_settings(defaultSettings, userSettings); + +% Will the system have the bare minimum states? +minStates = {'phi', 'delta', 'phiDot', 'deltaDot'}; +if sum(ismember(settings.states, minStates)) < 4 + error(['You have not specified the minimum set of states. Please ' ... + 'include at least phi, delta, phiDot, and deltaDot']) +end + +% Have state derivatives been specified that can't be computed with the +% specified states? +keepStates = find(ismember(states, settings.states)); +removeStates = find(~ismember(states, settings.states)); +for row = keepStates' + for col = removeStates' + if abs(A(row, col)) > 1e-10 + s = sprintf(['It is not possible to compute the derivative ' ... + 'of state %s because it depends on state %s'], ... + states{row}, states{col}); + error(s) + end + end +end + +removeInputs = find(~ismember(inputs, settings.inputs)); + +% Have outputs been specified that can't be computed with the specified +% states and inputs? +keepOutputs = find(ismember(outputs, settings.outputs)); +for row = keepOutputs' + for col = removeStates' + if abs(C(row, col)) > 1e-10 + s = sprintf(['It is not possible to keep output %s because ' ... + 'it depends on state %s'], outputs{row}, ... + states{col}); + error(s) + end + end + for col = removeInputs' + if abs(D(row, col)) > 1e-10 + s = sprintf(['It is not possible to keep output %s because ' ... + 'it depends on input %s'], outputs{row}, ... + inputs{col}); + error(s) + end + end +end + +removeOutputs = find(~ismember(outputs, settings.outputs)); + +A(removeStates, :) = []; +A(:, removeStates) = []; + +B(removeStates, :) = []; +B(:, removeInputs) = []; + +C(removeOutputs, :) = []; +C(:, removeStates) = []; + +D(removeOutputs, :) = []; +D(:, removeInputs) = []; + +states(removeStates) = []; +inputs(removeInputs) = []; +outputs(removeOutputs) = []; + +% build the ss structure +bicycle = ss(A, B, C, D, ... + 'StateName', states, ... + 'OutputName', outputs, ... + 'InputName', inputs); diff --git a/samples/Matlab/convert_variable.m b/samples/Matlab/convert_variable.m new file mode 100644 index 00000000..aff2199f --- /dev/null +++ b/samples/Matlab/convert_variable.m @@ -0,0 +1,68 @@ +function [name, order] = convert_variable(variable, output) +% Returns the name and order of the given variable in the output type. +% +% Parameters +% ---------- +% variable : string +% A variable name. +% output : string. +% Either `moore`, `meijaard`, `data`. +% +% Returns +% ------- +% name : string +% The variable name in the given output type. +% order : double +% The order of the variable in the list. + +[coordinates, speeds, inputs] = get_variables(); + +columns = {'data', 'meijaard', 'moore'}; + +if find(ismember(coordinates, variable)) + + [order, ~] = find(ismember(coordinates, variable)); + name = coordinates{order, find(ismember(columns, output))}; + +elseif find(ismember(speeds, variable)) + + [order, ~] = find(ismember(speeds, variable)); + name = speeds{order, find(ismember(columns, output))}; + +elseif find(ismember(inputs, variable)) + + [order, ~] = find(ismember(inputs, variable)); + name = inputs{order, find(ismember(columns, output))}; + +else + error('Beep: Done typed yo variable name wrong') +end + +function [coordinates, speeds, inputs] = get_variables() + +coordinates = {'LongitudinalRearContact', 'xP', 'q1'; + 'LateralRearContact', 'yP','q2'; + 'YawAngle', 'psi','q3'; + 'RollAngle', 'phi','q4'; + 'PitchAngle', 'thetaB','q5'; + 'RearWheelAngle', 'thetaR','q6'; + 'SteerAngle', 'delta','q7'; + 'FrontWheelAngle', 'thetaF','q8'; + 'LongitudinalFrontContact', 'xQ','q9'; + 'LateralFrontContact', 'yQ', 'q10'}; + +speeds = {'LongitudinalRearContactRate', 'xPDot', 'u1'; + 'LateralRearContactRate', 'yPDot', 'u2'; + 'YawRate', 'psiDot', 'u3'; + 'RollRate', 'phiDot', 'u4'; + 'PitchRate', 'thetaDot', 'u5'; + 'RearWheelRate', 'thetaRDot', 'u6'; + 'SteerRate', 'deltaDot', 'u7'; + 'FrontWheelRate', 'thetaFDot','u8'; + 'LongitudinalFrontContactRate', 'xQDot', 'u9'; + 'LateralFrontContactRate', 'yQDot', 'u10'}; + +inputs = {'RollTorque', 'tPhi', 'T4'; + 'RearWheelTorque', 'tThetaR', 'T6'; + 'SteerTorque', 'tDelta', 'T7'; + 'PullForce', 'fB', 'F'}; diff --git a/samples/Matlab/create_ieee_paper_plots.m b/samples/Matlab/create_ieee_paper_plots.m new file mode 100644 index 00000000..0c4fd8d9 --- /dev/null +++ b/samples/Matlab/create_ieee_paper_plots.m @@ -0,0 +1,1119 @@ +function create_ieee_paper_plots(data, rollData) +% Creates all of the figures for the IEEE paper. +% +% Parameters +% ---------- +% data : structure +% A structure contating the data from generate_data.m for all of the bicycles +% and speeds for the IEEE paper. +% rollData : structure +% The data for a single bicycle at a single speed with roll torque as the +% input. + +global goldenRatio +% used for figure width to height ratio +goldenRatio = (1 + sqrt(5)) / 2; + +% create a plot directory if one doesn't already exist +if exist('plots/', 'dir') ~= 7 + mkdir('plots/') +end + +% Define some linestyles and colors for each of the six bicycles +linestyles = {'-', '-', '-.', ... + '--', '-.', '--'}; +colors = {'k', ... + [0.5, 0.5, 0.5], ... + [0.5, 0.5, 0.5], ... + 'k', ... + 'k', ... + [0.5, 0.5, 0.5]}; + +loop_shape_example(data.Benchmark.Medium, 'Steer') +loop_shape_example(rollData, 'Roll') +plot_io_roll(rollData, 'Distance') +plot_io_roll(rollData, 'Time') +open_loop_all_bikes(data, linestyles, colors) +handling_all_bikes(data, rollData, linestyles, colors) +path_plots(data, linestyles, colors) +var = {'delta', 'phi', 'psi', 'Tdelta'}; +io = {'output', 'output', 'output', 'input'}; +typ = {'Distance', 'Time'}; +for i = 1:length(var) + for j = 1:length(typ) + plot_io(var{i}, io{i}, typ{j}, data, linestyles, colors) + end +end +phase_portraits(data.Benchmark.Medium) +eigenvalues(data, linestyles, colors) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function loop_shape_example(bikeData, input) +% Creates the example loop shaping for the bicycle at medium speed. +% +% Parameters +% ---------- +% bikeData : structure +% Contains data for a single bicycle at a single speed. +% input : string +% 'Steer' or 'Roll' depending on what input was used to control the bicycle. + +global goldenRatio + +% closed loop bode plots +figure() +figWidth = 5.0; +figHeight = figWidth / goldenRatio; +set(gcf, ... + 'Color', [1, 1, 1], ... + 'PaperOrientation', 'portrait', ... + 'PaperUnits', 'inches', ... + 'PaperPositionMode', 'manual', ... + 'OuterPosition', [424, 305 - 50, 518, 465], ... + 'PaperPosition', [0, 0, figWidth, figHeight], ... + 'PaperSize', [figWidth, figHeight]) + +freq = {0.1, 20.0}; + +hold all + +closedLoops = bikeData.closedLoops; + +% make sure all bode plots display 'rad/s' instead of 'rad/sec' +bops = bodeoptions; +bops.FreqUnits = 'rad/s'; + +if strcmp(input, 'Steer') + linestyles = {'', '', '-.', '-.', '-', '-.', '-.', '-'}; + gray = [0.6, 0.6, 0.6]; + colors = {'k', 'k', 'k', gray, 'k', 'k', gray, 'k'}; + % the closed delta loop + deltaNum = closedLoops.Delta.num; + deltaDen = closedLoops.Delta.den; + bodeplot(tf(deltaNum, deltaDen), freq, bops); + % a typical neuromuscular model + neuroNum = 2722.5; + neuroDen = [1, 13.96, 311.85, 2722.5]; + bodeplot(tf(neuroNum, neuroDen), freq, bops); + whichLines = 5:-1:3; +elseif strcmp(input, 'Roll') + linestyles = {'', '', '-', '-'}; + colors = {'k', 'k', 'k', 'k'}; + whichLines = 4:-1:2; +else + error('Bad input, use Steer or Roll') +end + +% the closed phi dot loop +phiDotNum = closedLoops.PhiDot.num; +phiDotDen = closedLoops.PhiDot.den; +closedBode = bodeplot(tf(phiDotNum, phiDotDen), freq, bops); + +hold off + +% clean it up +opts = getoptions(closedBode); +if strcmp(input, 'Steer') + opts.YLim = {[-45, 20], [-360, 90]}; +else + opts.YLim = {[-30, 10], [-180, 90]}; +end +opts.PhaseMatching = 'on'; +opts.PhaseMatchingValue = 0; +opts.Title.String = ''; +setoptions(closedBode, opts) + +% find all the lines in the current figure +lines = findobj(gcf, 'type', 'line'); +for i = 3:length(lines) + set(lines(i), 'LineStyle', linestyles{i}, ... + 'Color', colors{i}, ... + 'LineWidth', 2.0) +end + +% there seems to be a bug such that the xlabel is too low, this is a hack to +% get it to work +raise = 0.05; +plotAxes = findobj(gcf, 'type', 'axes'); +set(plotAxes, 'XColor', 'k', 'YColor', 'k') +curPos1 = get(plotAxes(1), 'Position'); +curPos2 = get(plotAxes(2), 'Position'); +set(plotAxes(1), 'Position', curPos1 + [0, raise, 0, 0]) +set(plotAxes(2), 'Position', curPos2 + [0, raise, 0, 0]) +xLab = get(plotAxes(1), 'Xlabel'); +set(xLab, 'Units', 'normalized') +set(xLab, 'Position', get(xLab, 'Position') + [0, raise + 0.05, 0]) + +% make the tick labels smaller +set(plotAxes, 'Fontsize', 8) +if strcmp(input, 'Steer') + legWords = {'$\delta$ Loop', + 'Neuromuscular model from [27]', + '$\dot{\phi}$ Loop'}; +elseif strcmp(input, 'Roll') + legWords = {'$\dot{\phi}$ Loop'}; +end +closeLeg = legend(lines(whichLines), ... + legWords, ... + 'Location', 'Southwest', ... + 'Interpreter', 'Latex', ... + 'Fontsize', 8); + +% add the annotation showing a 10 dB peak +if strcmp(input, 'Steer') + axes(plotAxes(2)) + db1 = text(2.7, 5.0, '~10dB'); + db2 = text(2.5, -10.0, '~10dB'); + set([db1, db2], 'Fontsize', 8) + dArrow1 = annotation('doublearrow', ... + [0.7, 0.7], ... + [0.755 + raise, 0.818 + raise]); + annotation('line', [0.69, 0.87], [0.818 + raise, 0.818 + raise]) + dArrow2 = annotation('doublearrow', ... + [0.685, 0.685], ... + [0.665 + raise, 0.725 + raise]); + annotation('line', [0.675, 0.87], [0.725 + raise, 0.725 + raise]) + set([dArrow1, dArrow2], 'Head1width', 3, 'Head1length', 3, ... + 'Head2width', 3, 'Head2length', 3) +else + axes(plotAxes(2)) + db1 = text(0.67, -3.7, '~10dB'); + set(db1, 'Fontsize', 8) + dArrow = annotation('doublearrow', ... + [0.5, 0.5], ... + [0.697 + raise, 0.795 + raise]); + set(dArrow, 'Head1width', 3, 'Head1length', 3, ... + 'Head2width', 3, 'Head2length', 3) + annotation('line', [0.49, 0.75], [0.795 + raise, 0.795 + raise]) +end + +filename = ['benchmark' input 'Closed']; +pathToFile = ['plots' filesep filename]; +print(gcf, '-deps2c', '-loose', [pathToFile '.eps']) +fix_ps_linestyle([pathToFile '.eps']) + +% open loop plots +figure() +set(gcf, ... + 'Color', [1, 1, 1], ... + 'PaperOrientation', 'portrait', ... + 'PaperUnits', 'inches', ... + 'PaperPositionMode', 'manual', ... + 'PaperPosition', [0, 0, figWidth, figHeight], ... + 'PaperSize', [figWidth, figHeight]) + +openLoops = bikeData.openLoops; + +hold all + +num = openLoops.Phi.num; +den = openLoops.Phi.den; +bodeplot(tf(num, den), freq, bops); + +num = openLoops.Psi.num; +den = openLoops.Psi.den; +bodeplot(tf(num, den), freq, bops); + +num = openLoops.Y.num; +den = openLoops.Y.den; +openBode = bodeplot(tf(num, den), freq, bops); + +hold off + +% clean it up +opts = getoptions(openBode); +opts.Title.String = ''; +opts.YLim = {[-80, 20], [-540, -60]}; +opts.PhaseMatching = 'on'; +opts.PhaseMatchingValue = 0; +setoptions(openBode, opts) + +% find all the lines in the current figure +lines = findobj(gcf, 'type', 'line'); +linestyles = {'', '', '-.', '-', '--', '-.', '-', '--'}; +for i = 3:length(lines) + set(lines(i), 'LineStyle', linestyles{i}, ... + 'Color', 'k', ... + 'LineWidth', 2.0) +end + + +plotAxes = findobj(gcf, 'type', 'axes'); +set(plotAxes, 'Fontsize', 8, 'XColor', 'k', 'YColor', 'k') +closeLeg = legend(lines(8:-1:6), ... + {'$\phi$ Loop', '$\psi$ Loop','$y$ Loop'}, ... + 'Location', 'Southwest', ... + 'Interpreter', 'Latex'); + +% add zero crossing lines +%axes(plotAxes(1)) +%line([0.1, 20], [-180, -180], 'Color', 'k') +axes(plotAxes(2)) +line([0.1, 20], [0, 0], 'Color', 'k') + +% add some lines and labels for the cross over frequencies +if strcmp(input, 'Steer') + wc = 2; + wShift = [0.42, 0.35, 0.175]; +else strcmp(input, 'Roll') + wc = 1.5; + wShift = [0.31, 0.26, 0.1325]; +end +axes(plotAxes(2)) +hold on +gray = [0.5, 0.5, 0.5]; + +line([wc, wc], [-40, 0], 'Color', gray) +text(wc - wShift(1), -43, ['$\omega_c=' num2str(wc) '$'], ... + 'Interpreter', 'Latex', 'Fontsize', 8) + +line([wc / 2, wc / 2], [-30, 0], 'Color', gray) +text(wc / 2 - wShift(2), -33, ['$\omega_c/2=' num2str(wc / 2) '$'], ... + 'Interpreter', 'Latex', 'Fontsize', 8) + +line([wc / 4, wc / 4], [-20, 0], 'Color', gray) +text(wc / 4 - wShift(3), -23, ['$\omega_c/4=' num2str(wc / 4) '$'], ... + 'Interpreter', 'Latex', 'Fontsize', 8) + +hold off + +curPos1 = get(plotAxes(1), 'Position'); +curPos2 = get(plotAxes(2), 'Position'); +set(plotAxes(1), 'Position', curPos1 + [0, raise, 0, 0]) +set(plotAxes(2), 'Position', curPos2 + [0, raise, 0, 0]) +xLab = get(plotAxes(1), 'Xlabel'); +set(xLab, 'Units', 'normalized') +set(xLab, 'Position', get(xLab, 'Position') + [0, raise + 0.05, 0]) + +filename = ['benchmark' input 'Open.eps']; +pathToFile = ['plots' filesep filename]; +print(gcf, '-deps2c', '-loose', pathToFile) +fix_ps_linestyle(pathToFile) + +% handling qualities plot +num = bikeData.handlingMetric.num; +den = bikeData.handlingMetric.den; +w = linspace(0.01, 20, 200); +[mag, phase, freq] = bode(tf(num, den), w); +figure() +set(gcf, ... + 'Color', [1, 1, 1], ... + 'PaperOrientation', 'portrait', ... + 'PaperUnits', 'inches', ... + 'PaperPositionMode', 'manual', ... + 'PaperPosition', [0, 0, figWidth, figHeight], ... + 'PaperSize', [figWidth, figHeight]) + +hold on + +metricLine = plot(freq, mag(:)', 'k-', 'Linewidth', 2.0); +level1 = line([0, 20], [5, 5]); +level2 = line([0, 20], [8, 8]); +set(level1, 'Color', 'k', 'Linestyle', '--', 'Linewidth', 2.0) +set(level2, 'Color', 'k', 'Linestyle', '--', 'Linewidth', 2.0) + +ylim([0, 10]); + +ylabel('Handling Quality Metric') +xlabel('Frequency (rad/s)') +text(3, 3, 'Level 1') +text(3, 6.5, 'Level 2') +text(3, 9, 'Level 3') +box on + +filename = ['benchmark' input 'Handling.eps']; +pathToFile = ['plots' filesep filename]; +print(gcf, '-deps2', '-loose', pathToFile) +fix_ps_linestyle(pathToFile) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function open_loop_all_bikes(data, linestyles, colors) +% Creates open loop Bode plots of all the bikes. + +global goldenRatio + +bikes = fieldnames(data); + +figure() +figWidth = 5.0; +figHeight = figWidth / goldenRatio; +set(gcf, ... + 'Color', [1, 1, 1], ... + 'PaperOrientation', 'portrait', ... + 'PaperUnits', 'inches', ... + 'PaperPositionMode', 'manual', ... + 'PaperPosition', [0, 0, figWidth, figHeight], ... + 'PaperSize', [figWidth, figHeight]) + +freq = {0.1, 20.0}; + +% make sure all bode plots display 'rad/s' instead of 'rad/sec' +bops = bodeoptions; +bops.FreqUnits = 'rad/s'; + +hold all +for i = 2:length(bikes) + num = data.(bikes{i}).Medium.openLoops.Phi.num; + den = data.(bikes{i}).Medium.openLoops.Phi.den; + openBode = bodeplot(tf(num, den), freq, bops); +end +hold off + +% clean it up +opts = getoptions(openBode); +%opts.Title.String = '$\phi$ Open Loop Bode Diagrams at 5 m/s'; +opts.Title.String = ''; +%opts.Title.Interpreter = 'Latex'; +opts.YLim = {[-30, 10], [-540, -90]}; +opts.PhaseMatching = 'on'; +opts.PhaseMatchingValue = 0; +setoptions(openBode, opts) + +% find all the lines in the current figure +plotAxes = findobj(gcf, 'type', 'axes'); +magLines = findobj(plotAxes(2), 'type', 'line'); +phaseLines = findobj(plotAxes(1), 'type', 'line'); + +for i = 2:length(magLines) + set(magLines(i), ... + 'LineStyle', linestyles{i - 1}, ... + 'Color', colors{i - 1}, ... + 'LineWidth', 1.0) + set(phaseLines(i), ... + 'LineStyle', linestyles{i - 1}, ... + 'Color', colors{i - 1}, ... + 'LineWidth', 1.0) +end + +closeLeg = legend(magLines(2:7), ... + {'1', '2', '3', '4', '5', '6'}, ... + 'Location', 'Southwest', ... + 'Fontsize', 8); + +set(plotAxes, 'YColor', 'k', 'XColor', 'k', 'Fontsize', 8) + +% add a zero lines +axes(plotAxes(1)) +line([0.1, 20], [-180, -180], 'Color', 'k') +axes(plotAxes(2)) +line([0.1, 20], [0, 0], 'Color', 'k') + +% raise the axes cause the xlabel is cut off +raise = 0.05; +curPos1 = get(plotAxes(1), 'Position'); +curPos2 = get(plotAxes(2), 'Position'); +set(plotAxes(1), 'Position', curPos1 + [0, raise, 0, 0]) +set(plotAxes(2), 'Position', curPos2 + [0, raise, 0, 0]) +xLab = get(plotAxes(1), 'Xlabel'); +set(xLab, 'Units', 'normalized') +set(xLab, 'Position', get(xLab, 'Position') + [0, raise + 0.05, 0]) + +filename = 'openBode.eps'; +pathToFile = ['plots' filesep filename]; +print(pathToFile, '-deps2c', '-loose') +fix_ps_linestyle(pathToFile) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function handling_all_bikes(data, rollData, linestyles, colors) +% Creates handling quality metric for all bikes. +% +% Parameters +% ---------- +% data : structure +% Contains data for all bikes a the three speeds for steer input. +% rollData : structure +% Contains the data for the benchmark bike with roll input at medium speed. +% linestyles : cell array +% Linestyle strings, one for each of the six bikes. +% colors : cell array +% Colorspecs for each of the six bikes. + +global goldenRatio + +bikes = fieldnames(data); +figure() +figWidth = 5.0; +figHeight = figWidth / goldenRatio; +set(gcf, ... + 'Color', [1, 1, 1], ... + 'PaperOrientation', 'portrait', ... + 'PaperUnits', 'inches', ... + 'PaperPositionMode', 'manual', ... + 'PaperPosition', [0, 0, figWidth, figHeight], ... + 'PaperSize', [figWidth, figHeight]) + +w = linspace(0.01, 20, 200); +speedNames = fieldnames(data.Browser); +fillColors = {[0.82, 0.82, 0.82] + [0.68, 0.68, 0.68] + [0.95, 0.95, 0.95]}; +hold all + +% plot the background area for each family of curves +for j = 1:length(speedNames) + % get the max values for the set of curves + magnitudes = zeros(length(w), length(bikes) - 1); + for i = 2:length(bikes) + num = data.(bikes{i}).(speedNames{j}).handlingMetric.num; + den = data.(bikes{i}).(speedNames{j}).handlingMetric.den; + [mag, phase, freq] = bode(tf(num, den), w); + magnitudes(:, i - 1) = mag(:)'; + end + maxMag = max(magnitudes, [], 2); + % fill the area under the curve + area(freq, maxMag, ... + 'Facecolor', fillColors{j}, ... + 'Edgecolor', 'none') +end + +% this makes sure that the edges of the fill area don't cover the axes +set(gca, 'Layer', 'top') + +% plot the actual curves +for j = 1:length(speedNames) + metricLines = zeros(length(bikes) - 1, 1); + for i = 2:length(bikes) + num = data.(bikes{i}).(speedNames{j}).handlingMetric.num; + den = data.(bikes{i}).(speedNames{j}).handlingMetric.den; + [mag, phase, freq] = bode(tf(num, den), w); + metricLines(i - 1) = plot(freq, mag(:)', ... + 'Color', colors{i - 1}, ... + 'Linestyle', linestyles{i - 1}, ... + 'Linewidth', 2.0); + end +end + +% add the roll input bike +num = rollData.handlingMetric.num; +den = rollData.handlingMetric.den; +[mag, phase, freq] = bode(tf(num, den), w); +rollLine = plot(freq, mag(:)', 'k', 'Linewidth', 2.0, 'Linestyle', ':'); + +hold off + +% move the roll input line down so it shows on the legend +chil = get(gca, 'Children'); +legLines = [chil(end:-1:14)', rollLine]; +legend(legLines, [{'2.5 m/s', '5.0 m/s', '7.5 m/s'}, ... + {'1', '2', '3', '4', '5', '6', 'Hands-free @ 5 m/s'}], ... + 'Fontsize', 8) + +ylim([0, 20]); +level1 = line([0, 20], [5, 5]); +level2 = line([0, 20], [8, 8]); +set(level1, 'Color', 'k', 'Linestyle', '--', 'Linewidth', 1.0) +set(level2, 'Color', 'k', 'Linestyle', '--', 'Linewidth', 1.0) +ylabel('Handling Quality Metric') +xlabel('Frequency (rad/s)') +text(3.1, 4.3, 'Level 1') +text(1.9, 6.5, 'Level 2') +text(3, 15, 'Level 3') +box on + +set(gca, 'YColor', 'k') + +filename = 'handling.eps'; +pathToFile = ['plots' filesep filename]; +print(pathToFile, '-deps2c', '-loose') +fix_ps_linestyle(pathToFile) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function path_plots(data, linestyles, colors) +% Creates a plot of the path tracking for all bikes at all speeds. + +global goldenRatio + +bikes = fieldnames(data); +speedNames = fieldnames(data.Browser); + +figure() +figWidth = 5.0; +figHeight = figWidth / goldenRatio; +set(gcf, ... + 'Color', [1, 1, 1], ... + 'PaperOrientation', 'portrait', ... + 'PaperUnits', 'inches', ... + 'PaperPositionMode', 'manual', ... + 'PaperPosition', [0, 0, figWidth, figHeight], ... + 'PaperSize', [figWidth, figHeight]) + +hold all + +% shifts the paths by this many meters +shift = [0, 15, 35]; +for j = 1:length(speedNames) + time = data.(bikes{2}).(speedNames{j}).time; + path = data.(bikes{2}).(speedNames{j}).path; + speed = data.(bikes{2}).(speedNames{j}).speed; + plot(time * speed + shift(j), -path * j, 'k-') + for i = 2:length(bikes) + x = data.(bikes{i}).(speedNames{j}).outputs(:, 17); + x = x + shift(j); + y = data.(bikes{i}).(speedNames{j}).outputs(:, 18); + plot(x, -y * j, ... + 'Linestyle', linestyles{i - 1}, ... + 'Color', colors{i - 1}, ... + 'Linewidth', 0.75) + end + [minPath, minPathI] = min(-path * j); + dis = time * speed + shift(j); + lab = sprintf('%1.1f m/s', speed); + text(dis(minPathI) - 15, minPath - 0.4, lab) +end + +hold off + +% change the y tick labels to positive and to reflect the 2 meter width +set(gca, 'YTick', [-7, -6, -4, -2, 0, 1]) +set(gca, 'YTickLabel', {'', '2', '2', '2', '0', ''}) + +xlim([30 200]) +box on +legend(['Path', {'1', '2', '3', '4', '5', '6'}], ... + 'Fontsize', 8, 'Location', 'Southeast') +xlabel('Distance (m)') +ylabel('Lateral Deviation (m)') +filename = 'paths.eps'; +pathToFile = ['plots' filesep filename]; +print(pathToFile, '-deps2c', '-loose') +fix_ps_linestyle(pathToFile) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function plot_io(variable, io, xAxis, data, linestyles, colors) +% Creates a plot of the time histories of a particular output or input variable +% for three different speeds with either time or distance on the x axis. +% +% Parameters +% ---------- +% variable : string +% The name of the variable you'd like to plot. +% io : string +% 'input' for input and 'output' for output. +% data : structure +% Data for a set of bicycles, the first being the benchmark bicycle. +% xAxis : string +% 'Distance' or 'Time' on the x axis. +% linestyles : cell array +% An array of linestyle types, one for each bicycle. +% colors : cell array +% An array of colors, one for each bicycle. + +global goldenRatio + +if strcmp(io, 'input') + names = {'Tphi', + 'Tdelta', + 'F'}; + prettyNames = {'$T_\phi$', + '$T_\delta$', + '$F$'}; + units = {'(N-m)', + '(N-m)', + '(N)'}; +elseif strcmp(io, 'output') + names = {'xP', + 'yP', + 'psi', + 'phi', + 'thetaP', + 'thetaR', + 'delta', + 'thetaF', + 'xPDot', + 'ypDot', + 'psiDot', + 'phiDot', + 'thetaPDot', + 'thetaRDot', + 'deltaDot', + 'thetaFDot', + 'xQ', + 'yQ'}; + prettyNames = {'$x_P$', + '$y_P$', + '$\psi$', + '$\phi$', + '$\theta_P$', + '$\theta_R$', + '$\delta$', + '$\theta_F$', + '$\dot{x}_P$', + '$\dot{y}_P$', + '$\dot{\psi}$', + '$\dot{\phi}$', + '$\dot{\theta}_P$', + '$\dot{\theta}_R$', + '$\dot{\delta}$', + '$\dot{\theta}_F$', + '$x_Q$', + '$y_Q$'}; + units = {'(m)', + '(m)', + '(rad)', + '(rad)', + '(rad)', + '(rad)', + '(rad)', + '(rad)', + '(m/s)', + '(m/s)', + '(rad/s)', + '(rad/s)', + '(rad/s)', + '(rad/s)', + '(rad/s)', + '(rad/s)', + '(m)', + '(m)'}; +else + error('Please choose i or o') +end + +index = find(ismember(names, variable) == 1); + +bikes = fieldnames(data); +speedNames = fieldnames(data.Browser); + +figure() +figWidth = 5.0; +figHeight = figWidth / goldenRatio; +set(gcf, ... + 'Color', [1, 1, 1], ... + 'PaperOrientation', 'portrait', ... + 'PaperUnits', 'inches', ... + 'PaperPositionMode', 'manual', ... + 'PaperPosition', [0, 0, figWidth, figHeight], ... + 'PaperSize', [figWidth, figHeight]) + +% find the maximum value of the variable +maxValue = 0; +for i = 2:length(bikes) + for j = 1:length(speedNames) + oneSpeed = data.(bikes{i}).(speedNames{j}); + history = oneSpeed.([io 's'])(:, index); + if max(history) > maxValue + maxValue = max(history); + end + end +end + +m = round(maxValue * 100) / 100; +pad = 0.15 * m; +yShift = [0, 2 * (m + pad), 4 * (m + pad)]; + +% shifts the paths by this many meters along the x axis +xShift = [0, 15, 35]; +hold all +for j = 1:length(speedNames) + for i = 2:length(bikes) + oneSpeed = data.(bikes{i}).(speedNames{j}); + time = oneSpeed.time; + speed = oneSpeed.speed; + distance = time * speed + xShift(j); + % time history of the value + history = oneSpeed.([io 's'])(:, index) + yShift(j); + if strcmp(xAxis, 'Distance') + xData = distance; + textX = 165; + elseif strcmp(xAxis, 'Time') + xData = time; + textX = 2; + else + error('Choose Time or Distance, no other') + end + plot(xData, history, ... + 'Linestyle', linestyles{i - 1}, ... + 'Color', colors{i - 1}, ... + 'Linewidth', 0.75) + end + % add labels for the speeds + text(textX, yShift(j) + 4 * pad, [num2str(speed) ' m/s']) +end + +ylim([-m - pad, yShift(3) + m + pad]) +set(gca, 'YTick', ... + [-m, yShift(1), m, ... + yShift(2) - m, yShift(2), yShift(2) + m, ... + yShift(3) - m, yShift(3), yShift(3) + m]) +ticks = {num2str(-m), '0', num2str(m)}; +set(gca, 'YTickLabel', [ticks, ticks ticks]) + +if strcmp(xAxis, 'Distance') + xlabel('Distance (m)') + xLimits = [35, 190]; + xlim(xLimits) + loc = 'Northwest'; +else + xlabel('Time (s)') + xLimits = [0, 50]; + xlim(xLimits) + loc = 'Northeast'; +end +l1 = line(xLimits, [yShift(1) + m + pad, yShift(1) + m + pad]); +l2 = line(xLimits, [yShift(2) + m + pad, yShift(2) + m + pad]); +set([l1, l2], 'Color', 'k') +hold off +set(gca, 'Fontsize', 8) +first = [prettyNames{index} ' ' units{index}]; +ylabel(first, 'Interpreter', 'Latex') +box on +legend({'1', '2', '3', '4', '5', '6'}, 'Fontsize', 8, 'Location', loc) + +% if it is the steer angle plot for distance, add a magnifier for the +% countersteer +if strcmp(variable, 'delta') && strcmp(xAxis, 'Distance') + % Specify the position and the size of the rectangle + x_r = 37; y_r = 0; w_r = 4; h_r = 0.01; + rectangle('Position', [x_r-w_r/2, y_r-h_r/2, w_r, h_r], ... + 'EdgeColor', 'k'); + % Specify the position and the size of the 2. axis + x_a = 0.2; y_a = 0.29; w_a = 0.15; h_a = w_a * h_r / w_r * 20 / 0.05; + ax = axes('Units', 'Normalized', ... + 'Position', [x_a, y_a, w_a, h_a], ... + 'XTick', [], ... + 'YTick', [], ... + 'Box', 'on', ... + 'LineWidth', 0.5, ... + 'Color', 'w'); + hold on + j = 1; + for i = 2:length(bikes) + oneSpeed = data.(bikes{i}).(speedNames{j}); + time = oneSpeed.time; + speed = oneSpeed.speed; + distance = time * speed + xShift(j); + % time history of the value + history = oneSpeed.([io 's'])(:, index) + yShift(j); + plot(distance, history, ... + 'Linestyle', linestyles{i - 1}, ... + 'Color', colors{i - 1}, ... + 'Linewidth', 0.75) + end + hold off + axis([x_r-w_r/2, x_r+w_r/2, y_r-h_r/2, y_r+h_r/2]); + text(35.3, -0.003, 'Countersteer', 'Fontsize', 8) + % bottom left + annotation('line', [x_a, 0.129], [y_a, 0.235]) + % top left + annotation('line', [x_a, 0.132], [y_a + h_a, 0.26]) + % bottom right + annotation('line', [x_a + w_a, 0.15], [y_a, 0.235]) + % top right + annotation('line', [x_a, 0.15], [y_a + 0.02, 0.26]) +end + +% save the file +filename = [variable xAxis '.eps']; +print(['plots' filesep filename], '-deps2c', '-loose') +fix_ps_linestyle(['plots' filesep filename]) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function plot_io_roll(rollData, xAxis) + +global goldenRatio + +% closed loop bode plots +figure() +figWidth = 5.0; +figHeight = figWidth / goldenRatio; +set(gcf, ... + 'Color', [1, 1, 1], ... + 'PaperOrientation', 'portrait', ... + 'PaperUnits', 'inches', ... + 'PaperPositionMode', 'manual', ... + 'PaperPosition', [0, 0, figWidth, figHeight], ... + 'PaperSize', [figWidth, figHeight]) + +speed = rollData.speed; +time = rollData.time; +path = rollData.path; +frontWheel = rollData.outputs(:, 18); +rollAngle = rollData.outputs(:, 4); +steerAngle = rollData.outputs(:, 7); +rollTorque = rollData.inputs(:, 1); + +% plot the path +subplot(2, 1, 1) +hold all +if strcmp(xAxis, 'Distance') + plot(speed * time, -path, 'k-', 'Linewidth', 1.0) + plot(speed * time, -frontWheel, 'k:', 'Linewidth', 1.0) + xlabel('Distance (m)') + xlim([30, 150]) +elseif strcmp(xAxis, 'Time') + plot(time, -path, 'k-', 'Linewidth', 1.0) + plot(time, -frontWheel, 'k:', 'Linewidth', 1.0) + xlabel('Time (s)') + xlim([30 / speed, 150 / speed]) +else + error('Bad xAxis, choose Distance or Time') +end +hold off +box on +ylabel('Lateral Deviation (m)') +ylim([-2.2, 0.2]) +set(gca, 'YTickLabel', {'2', '1', '0'}) +legend({'Path'}, ... + 'Interpreter', 'Latex', ... + 'Fontsize', 8, ... + 'Location', 'Southeast') + +subplot(2, 1, 2) +hold all +if strcmp(xAxis, 'Distance') + plot(speed * time, rollAngle, 'k-', 'Linewidth', 1.0) + [ax, h1, h2] = plotyy(speed * time, steerAngle, speed * time, rollTorque); + xlabel('Distance (m)') + xlim(ax(1), [30, 150]) + xlim(ax(2), [30, 150]) +elseif strcmp(xAxis, 'Time') + plot(time, rollAngle, 'k-', 'Linewidth', 1.0) + [ax, h1, h2] = plotyy(time, steerAngle, time, rollTorque); + xlabel('Time (s)') + xlim(ax(1), [30 / speed, 150 / speed]) + xlim(ax(2), [30 / speed, 150 / speed]) +else + error('Bad xAxis, choose Distance or Time') +end +hold off +box on + +set(get(ax(1), 'Ylabel'), ... + 'String', 'Angle (rad)', ... + 'Color', 'k') +set(get(ax(2), 'Ylabel'), ... + 'String', 'Torque (N-m)', ... + 'Color', 'k') +set(ax, 'YColor', 'k', 'Fontsize', 8) +set(h1, 'Linestyle', '--', 'Color', 'k', 'Linewidth', 1.0) +set(h2, 'Linestyle', '-.', 'Color', 'k', 'Linewidth', 1.0) +legend({'$\phi$', '$\delta$', '$T_\phi$'}, ... + 'Interpreter', 'Latex', ... + 'Fontsize', 8, ... + 'Location', 'Northeast') + +if strcmp(xAxis, 'Distance') + axes(ax(2)) + % magnifier rectangle + x_r = 38; y_r = 0; w_r = 10; h_r = 0.3; + rectangle('Position', [x_r-w_r/2, y_r-h_r/2, w_r, h_r], ... + 'EdgeColor', 'k'); + % magnify it + x_a = 0.14; y_a = 0.432; w_a = 0.28; h_a = 0.12; + inset = axes('Units', 'Normalized', ... + 'Position', [x_a, y_a, w_a, h_a], ... + 'Box', 'on', ... + 'LineWidth', 0.5, ... + 'Color', [0.8, 0.8, 0.8]); + hold on + plot(inset, time, rollAngle, 'k-', 'Linewidth', 1.0) + [ax, h1, h2] = plotyy(inset, time, steerAngle, time, rollTorque); + set(ax, 'XTick', [], ... + 'YTick', [], ... + 'YColor', 'k') + set(h1, 'Linestyle', '--', 'Color', 'k', 'Linewidth', 1.0) + set(h2, 'Linestyle', '-.', 'Color', 'k', 'Linewidth', 1.0) + axis(ax(1), [7, 8.5, -0.0025, 0.0025]) + axis(ax(2), [7, 8.5, -0.0025 / 0.02, 0.0025 / 0.02]) + + % draw some lines connecting the corners + annotation('line', [x_a, 0.149], [y_a, 0.287]) + annotation('line', [x_a + w_a, 0.213], [y_a, 0.287]) + annotation('textbox', [0.26, 0.47, 0.1, 0.02], ... + 'String', 'Countersteer', ... + 'Fontsize', 8, ... + 'Edgecolor', 'none') +end + +filename = ['roll' xAxis '.eps']; +print(gcf, ['plots' filesep filename], '-deps2c', '-loose') +fix_ps_linestyle(['plots' filesep filename]) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function phase_portraits(bikeData) +% Creates four phase portrait plots to demonstrate the effects on the phase +% portraits when adjusting the gains. + +global goldenRatio + +figure() +figWidth = 6.0; +figHeight = figWidth / goldenRatio; +set(gcf, ... + 'Color', [1, 1, 1], ... + 'PaperOrientation', 'portrait', ... + 'PaperUnits', 'inches', ... + 'PaperPositionMode', 'manual', ... + 'OuterPosition', [424, 305 - 50, 518, 465], ... + 'PaperPosition', [0, 0, figWidth, figHeight], ... + 'PaperSize', [figWidth, figHeight]) + +% this is the gain multiplier for the non-nominal plot +gainChanges = [1.2, 1, 1, 1, 1; + 1, 1.2, 1, 1, 1; + 1, 1, 1.2, 1, 1; + 1, 1, 1.2, 0.8, 0.8]; + +loopNames = {'kDelta', 'kPhiDot', 'kPhi', 'kPhi'}; +% the x and y indices for each plot +xy = [7, 15; + 4, 12; + 4, 12; + 4, 12]; +% where to get the x and y data from +xySource = {'outputs', 'outputsDot', 'outputs', 'outputs'}; + +% axis labels +xlabels = {'(a) $\delta$ (rad)', + '(b) $\dot{\phi}$ (rad/s)', + '(c) $\phi$ (rad)', + '(d) $\phi$ (rad)'}; +ylabels = {'$\dot{\delta}$ (rad/s)', + '$\ddot{\phi}$ (rad/s$^2$)', + '$\dot{\phi}$ (rad/s)', + '$\dot{\phi}$ (rad/s)'}; + +% legend starts +legends = {'$k_\delta$ = ', + '$k_{\dot{\phi}}$ = ', + '$k_\phi$ = ', + '$k_\phi$ = '}; +% how many decimals to show for each legend +floatSpec = {'%1.1f', '%1.3f', '%1.1f', '%1.1f'}; + +for i = 1:length(loopNames) + + display(sprintf('Calculating phase portrait %d', i)) + + % adjust the gains and get the data + twentyPercent = generate_data('Benchmark', 5.0, ... + 'gainMuls', gainChanges(i, :)); + subplot(2, 2, i) + hold on + + if i == 4 + display('Phase portrait 4 comparison data.') + nominalData = generate_data('Benchmark', 5.0, ... + 'gainMuls', [1, 1, 1, 0.8, 0.8]); + x = nominalData.(xySource{i})(:, xy(i, 1)); + y = nominalData.(xySource{i})(:, xy(i, 2)); + else + x = bikeData.(xySource{i})(:, xy(i, 1)); + y = bikeData.(xySource{i})(:, xy(i, 2)); + end + plot(x, y, 'k-', 'Linewidth', 1.0) + + x = twentyPercent.(xySource{i})(:, xy(i, 1)); + y = twentyPercent.(xySource{i})(:, xy(i, 2)); + plot(x, y, 'k--', 'Linewidth', 1.0) + + hold off + + box on + axis equal + xlabel(xlabels{i}, 'Interpreter', 'Latex', 'Fontsize', 8) + ylabel(ylabels{i}, 'Interpreter', 'Latex', 'Fontsize', 8) + + % make the legend + leg1 = sprintf(floatSpec{i}, bikeData.modelPar.(loopNames{i})); + leg2 = sprintf(floatSpec{i}, twentyPercent.modelPar.(loopNames{i})); + legend({[legends{i} leg1], [legends{i} leg2]} , ... + 'Interpreter', 'Latex', ... + 'Fontsize', 6) +end + +plotAxes = findobj(gcf, 'Type', 'Axes'); +set(plotAxes, 'Fontsize', 8) + +% save the plot +filename = 'phasePortraits.eps'; +print(gcf, ['plots' filesep filename], '-deps2c', '-loose') +fix_ps_linestyle(['plots' filesep filename]) + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +function eigenvalues(data, linestyles, colors) + +global goldenRatio + +figure() +figWidth = 5.0; +figHeight = figWidth / goldenRatio; +set(gcf, ... + 'Color', [1, 1, 1], ... + 'PaperOrientation', 'portrait', ... + 'PaperUnits', 'inches', ... + 'PaperPositionMode', 'manual', ... + 'PaperPosition', [0, 0, figWidth, figHeight], ... + 'PaperSize', [figWidth, figHeight]) + +bikes = fieldnames(data); +bikes(1) = []; +speeds = 0:0.1:10; +eVals = zeros(length(bikes), length(speeds), 11); +for i = 1:length(bikes) + % load the bicycle parameters + pathToParFile = ['parameters' filesep bikes{i} 'Par.txt']; + par = par_text_to_struct(pathToParFile); + str = 'Calculating eigenvalues for the %s bicycle and rider.'; + display(sprintf(str, bikes{i})) + for j = 1:length(speeds) + % calculate the A, B, C, and D matrices of the bicycle model + [A, B, C, D] = whipple_pull_force_abcd(par, speeds(j)); + eigenValues = eig(A); + eVals(i, j, :) = real(eig(A)); + end +end + +% reduce to the maximum values +zeroIndices = find(abs(eVals) <= 0.000001); +eVals(zeroIndices) = -100 * ones(size(zeroIndices)); +maxEvals = max(eVals, [], 3); + +lines = plot(speeds, maxEvals); + +for i = 1:length(lines) + set(lines(i), ... + 'Linestyle', linestyles{i}, ... + 'Color', colors{i}, ... + 'Linewidth', 1) +end + +legend({'1', '2', '3', '4', '5', '6'}) +xlabel('Speed (m/s)') +ylabel('Maximum real part of the eigenvalue (1/s)') + +% set the tick marks differently +%set(gca, 'XTick', 0:0.5:10) +%set(gca, 'XTickLabel', {'0', '', '1', '', ... + %'2', '2.5', '3', '', ... + %'4', '', '5.0', '', ... + %'6', '', '7', '7.5', ... + %'8', '', '9', '', '10'}) +% +% add some lines and labels for the speeds we looked at +hold on +maxLine = max(maxEvals, [], 1); +minLine = min(maxEvals, [], 1); +lines = zeros(4, 1); +speedInd = find(speeds == 2.5); +lines(1) = line([2.5, 2.5], ... + [minLine(speedInd) - 0.4, maxLine(speedInd) + 0.4]); +text(2, maxLine(speedInd) + 0.7, '2.5 m/s') +speedInd = find(speeds == 5.0); +lines(2) = line([5.0, 5.0], ... + [minLine(speedInd) - 0.4, maxLine(speedInd) + 0.4]); +text(4.5, maxLine(speedInd) + 0.7, '5.0 m/s') +speedInd = find(speeds == 7.5); +lines(3) = line([7.5, 7.5], ... + [minLine(speedInd) - 0.4, maxLine(speedInd) + 0.4]); +text(7, maxLine(speedInd) + 0.7, '7.5 m/s') + +lines(4) = line([0, 10], [0, 0]); +hold off + +set(lines, 'Color', 'k', 'Linewidth', 2) + +% save the plot +filename = 'eigenvalues.eps'; +print(gcf, ['plots' filesep filename], '-deps2c', '-loose') +fix_ps_linestyle(['plots' filesep filename]) diff --git a/samples/Matlab/fit_adapt.m b/samples/Matlab/fit_adapt.m new file mode 100644 index 00000000..426c2c63 --- /dev/null +++ b/samples/Matlab/fit_adapt.m @@ -0,0 +1,57 @@ +data = load_data('jason_adapt.mat'); + +t0 = 0; +t1 = 30; +t2 = 60; +t3 = 90; + +dataPlantOne = data(1:t1 / data.Ts); +dataAdapting = data(t1 / data.Ts:t2 / data.Ts); +dataPlantTwo = data(t2 / data.Ts:end); + +% k1, k2, k3, k4, tau, zetanm, wnm, zetafs, wfs + +% ron's guess +guessPlantOne = [4.85, 1.79, 20, 20, 0.2, 0.707, 10, 0.707, 65]; +% best solution from ron's guess +%guessPlantOne = [4.1129, 2.1327, 52.3747, 48.6997, 0.2, 0.707, 10, 0.707, 65]; +resultPlantOne = find_structural_gains(dataPlantOne, guessPlantOne, 1); +[yh, fit, x0] = compare(dataPlantOne, resultPlantOne.fit); +display(sprintf('The self validation VAF is %f.', fit(1, 1, 1))) + +% ron's guess +guessPlantTwo = [3.36, 9.49, 20, 0, 0.2, 0.707, 10, 0.707, 65]; +% best solution from ron's guess +%guessPlantTwo = [2.6686, 7.0431, 14.4623, 3.1532, 0.2, 0.707, 10, 0.707, 65]; +resultPlantTwo = find_structural_gains(dataPlantTwo, guessPlantTwo, 5); +[yh, fit, x0] = compare(dataPlantTwo, resultPlantTwo.fit); +display(sprintf('The self validation VAF is %f.', fit(1, 1, 1))) + +% compute the slope and offset for each gain for initial guesses +kP1 = resultPlantOne.fit.par(1:4); +kP2 = resultPlantTwo.fit.par(1:4); +gainSlopeOffset = [t1 * eye(4), eye(4); t2 * eye(4), eye(4)] \ [kP1; kP2]; + +aux.pars = guessPlantOne; % this only uses tau through wfs +aux.timeDelay = true; +aux.plantFirst = 1; % 1 / s +aux.plantSecond = 5; % 5 / (s + 10) +% compute the slope and offset of the plant for t1 < t < t2 +plantOneSlopeOffset = [t1, 1; t2, 1] \ [1; 0]; +plantTwoSlopeOffset = [t1, 1; t2, 1] \ [0; 1]; +aux.m = [plantOneSlopeOffset(1); plantTwoSlopeOffset(1)]; +aux.b = [plantOneSlopeOffset(2); plantTwoSlopeOffset(2)]; + +%[dx, y] = adapting_structural_model(45, ones(8, 1), 10, gainSlopeOffset, {aux}); + +% NOTE: 'FileArgument' has to be a cell array, the is why aux is in +% brackets. Also, if you pass the parameters in as a vector here, as I have, +% they get mapped to a structure and your ode file/function must accept each +% parameter as individual arguments. +mod = idnlgrey('adapting_structural_model', [1, 1, 8], gainSlopeOffset, ... + zeros(8, 1), 0, 'FileArgument', {aux}, 'InputName', 'thetac', ... + 'OutputName', 'theta'); + +fit = pem(dataAdapting, mod); + +compare(dataAdapting, fit); diff --git a/samples/Matlab/fit_adapt_linear.m b/samples/Matlab/fit_adapt_linear.m new file mode 100644 index 00000000..16764289 --- /dev/null +++ b/samples/Matlab/fit_adapt_linear.m @@ -0,0 +1,86 @@ +% This file identifies a series of models for a time varying plant. + +% k1, k2, k3, k4, tau, zetanm, wnm, zetafs, wfs + +%data = load_data('jason_adapt.mat'); +%filename = 'adapt'; +%guess.plantOne = [4.85, 1.79, 20, 20, 0.2, 0.707, 10, 0.707, 65]; +%guess.plantTwo = [3.36, 9.49, 20, 0, 0.2, 0.707, 10, 0.707, 65]; +%plantNum.plantOne = 1; +%plantNum.plantTwo = 5; + +data = load_data('adapt_hard.mat'); +filename = 'hard-adapt'; +guess.plantOne = [1.23, 0.354, 0.2, 20.0, 0.2, 0.707, 10, 0.707, 65]; +guess.plantTwo = [4.85, 1.79, 20, 20, 0.2, 0.707, 10, 0.707, 65]; +plantNum.plantOne = 6; +plantNum.plantTwo = 1; + +t = [0, 30, 40, 50, 60, 90]; + +sections = {'plantOne', 'adaptOne', 'adaptTwo', 'adaptThree', 'plantTwo'}; +for i = 1:length(sections) + secData.(sections{i}) = data(t(i) / data.Ts + 1:t(i + 1) / data.Ts); +end + +% compute the slope and offset for each gain for initial guesses +kP1 = guess.plantOne(1:4)'; +kP2 = guess.plantTwo(1:4)'; +gainSlopeOffset = [t(2) * eye(4), eye(4); t(5) * eye(4), eye(4)] \ [kP1; kP2]; +m = gainSlopeOffset(1:4); +b = gainSlopeOffset(5:8); + +for i = 1:length(sections) + + if strcmp(sections{i}, sections{1}) || strcmp(sections{i}, sections{end}) + display('boo') + else + if i > 1 + % use the best fit gains from the previous section + guess.(sections{i}) = result.(sections{i - 1}).fit.par; + else + currentGuess = m .* (t(i) + t(i + 1)) / 2 + b; + guess.(sections{i}) = [currentGuess', guess.plantOne(5:end)]; + end + percent = ((t(i) + t(i + 1)) / 2 - t(2)) / (t(5) - t(2)); + plantNum.(sections{i}) = {plantNum.plantOne, plantNum.plantTwo, percent}; + end + + result.(sections{i}) = find_structural_gains(secData.(sections{i}), ... + guess.(sections{i}), plantNum.(sections{i}), 'warning', false, ... + 'randomGuess', true); + + [yh, vaf, x0] = compare(secData.(sections{i}), result.(sections{i}).fit); + result.(sections{i}).vaf = vaf(1, 1, 1); + display(sprintf('The self validation VAF is %f.', vaf(1, 1, 1))) + + p = plantNum.(sections{i}); + if size(plantNum.(sections{i}), 2) > 1 + result.(sections{i}).plant = plant(p{:}); + else + result.(sections{i}).plant = plant(p); + end +end + +save(['data/' filename '-results.mat'], 'sections', 'guess', 'plantNum', 'result') + +for i = 1:length(sections) + result.(sections{i}).fig = figure; + compare(secData.(sections{i}), result.(sections{i}).fit); + saveas(result.(sections{i}).fig, ['plots/' filename '-' sections{i} '.png']) + [yh, fit, x0] = compare(secData.(sections{i}), result.(sections{i}).fit); + display('-----------------') + display('The task plant is:') + display(result.(sections{i}).plant) + display(sprintf('The order of the closed loop system is %u.', ... + size(result.(sections{i}).mod.A, 1))) + display(sprintf('The gain guesses: k1=%f, k2=%f, k3=%f, k4=%f', ... + result.(sections{i}).mod.par(1:4))) + display(sprintf('The identified gains: k1=%f+\\-%f, k2=%f+\\-%f, k3=%f+\\-%f, k4=%f+\\-%f', ... + result.(sections{i}).fit.par(1), result.(sections{i}).uncert(1), ... + result.(sections{i}).fit.par(2), result.(sections{i}).uncert(2), ... + result.(sections{i}).fit.par(3), result.(sections{i}).uncert(3), ... + result.(sections{i}).fit.par(4), result.(sections{i}).uncert(4))) + display(sprintf('The self validation VAF is %f.', ... + result.(sections{i}).vaf)) +end diff --git a/samples/Matlab/fix_ps_linestyle.m b/samples/Matlab/fix_ps_linestyle.m new file mode 100644 index 00000000..3110afe1 --- /dev/null +++ b/samples/Matlab/fix_ps_linestyle.m @@ -0,0 +1,109 @@ +function fix_ps_linestyle(varargin) + +%FIXPSLINESTYLE Fix line styles in exported post script files +% +% FIXPSLINESTYLE(FILENAME) fixes the line styles in the postscript file +% FILENAME. The file will be over-written. This takes a .PS or .EPS file +% and fixes the dotted and dashed line styles to be a little bit more +% esthetically pleasing. It fixes the four default line styles (line, +% dotted, dashed, dashdot). +% +% FIXPSLINESTYLE(FILENAME, NEWFILENAME) creates a new file NEWFILENAME. +% +% This is meant to be used with postscript files created by MATLAB +% (print, export). +% +% Example: +% x = 1:.1:10; +% y1 = sin(x); +% y2 = cos(x); +% h = plot(x, y1, '--', x, y2, '-.'); +% set(h, 'LineWidth', 2); +% grid on; +% legend('line 1', 'line2'); +% +% print -depsc test.eps +% fixPSlinestyle('test.eps', 'fixed_test.eps'); +% +% See also PRINT. +% +% Copyright 2005-2010 The MathWorks, Inc. +% +% Copyright (c) 2010, The MathWorks, Inc. All rights reserved. +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are met: +% +% * Redistributions of source code must retain the above copyright +% notice, this list of conditions and the following disclaimer. +% * Redistributions in binary form must reproduce the above copyright +% notice, this list of conditions and the following disclaimer in the +% documentation and/or other materials provided with the distribution +% * Neither the name of the The MathWorks, Inc. nor the names +% of its contributors may be used to endorse or promote products derived +% from this software without specific prior written permission. +% +% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +% ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +% WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +% DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +% ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +% (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +% LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +% ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +% (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +% Error checking +error(nargchk(1, 2, nargin)); +if ~ischar(varargin{1}) || (nargin == 2 && ~ischar(varargin{2})) + error('Input arguments must be file names (char).'); +end + +% Make sure the files specified are postscript files +[p1, n1, e1] = fileparts(varargin{1}); +if isempty(e1) || ~ismember(lower(e1), {'.ps', '.eps'}) + error('The extension has to be .ps or .eps'); +end + +% Open file and read it in +fid = fopen(varargin{1}, 'r'); +str = fread(fid); +str = char(str'); +fclose(fid); + +% Find where the line types are defined +id = findstr(str, '% line types:'); +str1 = str(1:id-1); +[line1 , remline ] = strtok(str(id:end), '/'); +[replacestr, remline2] = strtok(remline , '%'); + +% Define the new line styles +solidLine = sprintf('/SO { [] 0 setdash } bdef\n'); +dotLine = sprintf('/DO { [3 dpi2point mul 3 dpi2point mul] 0 setdash } bdef\n'); +dashedLine = sprintf('/DA { [6 dpi2point mul] 0 setdash } bdef\n'); +dashdotLine = sprintf('/DD { [2 dpi2point mul 2 dpi2point mul 6 dpi2point mul 2 dpi2point mul] 0 setdash } bdef\n'); + +% Construct the new file with the new line style definitions +newText = [str1, line1, solidLine, dotLine, dashedLine, dashdotLine, remline2]; + +% Check for output file name +if nargin == 2 + [p2, n2, e2] = fileparts(varargin{2}); + if isempty(e2) + fname = fullfile(p2, [n2, e1]); + else + if strcmpi(e1, e2) + fname = varargin{2}; + else + error('Output file must have same file extension.'); + end + end +else % if not defined, over-write + fname = varargin{1}; +end + +% Write out to file +fid = fopen(fname, 'w'); +fprintf(fid, '%s', newText); +fclose(fid); diff --git a/samples/Matlab/ieee.m b/samples/Matlab/ieee.m new file mode 100644 index 00000000..a63afe44 --- /dev/null +++ b/samples/Matlab/ieee.m @@ -0,0 +1,13 @@ +% This script loads in the data used for the IEEE paper and creates the plots. +clc; close all; + +bikes = {'Benchmark', 'Browserins', 'Browser', 'Pista', ... + 'Fisher', 'Yellow', 'Yellowrev'}; + +data = load_bikes(bikes, 'Steer'); + +rollData = generate_data('Benchmark', 5.0, ... + 'input', 'Roll', ... + 'gains', [1, 55, 3.76, 0.413, 0.076]); + +create_ieee_paper_plots(data, rollData) diff --git a/samples/Matlab/lane_change.m b/samples/Matlab/lane_change.m new file mode 100644 index 00000000..75f06485 --- /dev/null +++ b/samples/Matlab/lane_change.m @@ -0,0 +1,56 @@ +function [x, y, t] = lane_change(start, width, slope, pathLength, speed, num, ... + type, varargin) +% Generates the time and coordinates for either a single or double lane change +% manuever at a particular speed. +% +% Parameters +% ---------- +% start : float +% The starting point along the x axis in meters. +% width : float +% The width of the lane deviation. +% slope : float +% The slope of the lane change. +% pathLength : float +% The length of path. +% speed : float +% Speed of travel. +% num : integer +% Number of time steps. +% type : string +% Either 'single' or 'double'. A double lane change return to x = 0. +% laneLength : float, optional +% Length of the lane for a double lane change. +% +% Returns +% ------- +% x : matrix, (num, 1) +% The longitudinal path. +% y : matrix, (num, 1) +% The lateral path. +% t : matrix, (num, 1) +% Time. + +x = 0:pathLength / num:pathLength; +x = x'; +t = x / speed; + +y = zeros(length(x), 1); +endOfSlope = width / slope + start; +slopeInd = find((x > start) & (x <= endOfSlope)); +y(slopeInd) = slope * (x(slopeInd) - start); +if strcmp(type, 'single') + theRest = slopeInd(end) + 1:length(y); + y(theRest) = width * ones(length(theRest), 1); +elseif strcmp(type, 'double'); + if length(varargin) < 1 + error('Double lane change needs length of lane.') + else + laneLength = varargin{1}; + startOfSlope = start + laneLength - width / slope; + lane = find((x > endOfSlope) & (x <= startOfSlope)); + y(lane) = width * ones(length(lane), 1); + downSlope = find((x > startOfSlope) & (x <= start + laneLength)); + y(downSlope) = slope * (start + laneLength - x(downSlope)); + end +end diff --git a/samples/Matlab/load_bikes.m b/samples/Matlab/load_bikes.m new file mode 100644 index 00000000..f71fd4ee --- /dev/null +++ b/samples/Matlab/load_bikes.m @@ -0,0 +1,55 @@ +function data = load_bikes(bikes, input) +% function data = load_bikes(bikes, input) +% Returns the data for a set of bicycles at three speeds. +% +% Parameters +% ---------- +% bikes : cell array +% A cell array that lists the bicyle short names. +% input : string +% 'Steer' or 'Roll' +% +% Returns +% ------- +% data : structure +% A structure containg a node for each bicycle and each speed. + +speeds = [2.5, 5.0, 7.5]; +speedNames = {'Slow', 'Medium', 'Fast'}; + +% these are the gains that were selected manually by Ron Hess, taken from +% the paper +gains.Benchmark.Slow = [22.0, -0.090, 23.3, 0.058, 0.195]; % place holder +gains.Browserins.Slow = [22.0, -0.090, 23.3, 0.058, 0.195]; +gains.Browser.Slow = [20.5, -0.086, 24.1, 0.053, 0.199]; +% the gains in the paper give an unstable bike, the uncommented one were the ones from his +% simulink model +gains.Pista.Slow = [22.3000,-0.1300,15.6410,0.0645,0.1990]; %[22.3, -0.130, 15.6, 0.662, 0.198]; +gains.Fisher.Slow = [23.0, -0.120, 17.7, 0.065, 0.198]; +gains.Yellow.Slow = [18.0, -0.110, 20.2, 0.062, 0.200]; +gains.Yellowrev.Slow = [48.0, -0.070, 27.9, 0.063, 0.191]; + +gains.Benchmark.Medium = [ 46.5, -0.052, 12.8, 0.177, 0.097]; +gains.Browserins.Medium = [ 48.0, -0.080, 9.03, 0.161, 0.097]; +gains.Browser.Medium = [ 43.0, -0.087, 8.50, 0.173, 0.100]; +gains.Pista.Medium = [ 49.0, -0.080, 8.06, 0.170, 0.101]; +gains.Fisher.Medium = [ 50.5, -0.084, 8.26, 0.168, 0.100]; +gains.Yellow.Medium = [ 39.0, -0.085, 8.61, 0.160, 0.101]; +gains.Yellowrev.Medium = [105.0, -0.070, 8.90, 0.165, 0.100]; + +gains.Benchmark.Fast = [ 74.0, -0.063, 6.31, 0.332, 0.065]; % place holder +gains.Browserins.Fast = [ 74.0, -0.063, 6.31, 0.332, 0.065]; +gains.Browser.Fast = [ 68.0, -0.060, 6.74, 0.330, 0.065]; +gains.Pista.Fast = [ 80.0, -0.058, 5.82, 0.321, 0.066]; +gains.Fisher.Fast = [ 82.0, -0.062, 5.83, 0.315, 0.065]; +gains.Yellow.Fast = [ 61.0, -0.063, 6.34, 0.345, 0.065]; +gains.Yellowrev.Fast = [170.0, -0.050, 6.45, 0.300, 0.066]; + +% load the data for all speeds for all the bikes +for i = 1:length(bikes) + for j = 1:length(speeds) + data.(bikes{i}).(speedNames{j}) = ... + generate_data(bikes{i}, speeds(j), 'input', input, ... + 'gains', gains.(bikes{i}).(speedNames{j})); + end +end diff --git a/samples/Matlab/load_data.m b/samples/Matlab/load_data.m new file mode 100644 index 00000000..e8c965db --- /dev/null +++ b/samples/Matlab/load_data.m @@ -0,0 +1,33 @@ +function data = load_data(filename, varargin) +% function data = load_data(filename, varargin) +% +% Returns an iddata object with the input thetac and output theta for the +% given file. +% +% Parameters +% ---------- +% filename : char +% Filename for the data file. +% varargin : char value pairs, optional +% sampleTime : double, default=0.0005 +% detread : boolean, default=true +% directory : char, default='data' + +parser = inputParser; +parser.addRequired('filename'); +parser.addParamValue('sampleTime', 0.0005); +parser.addParamValue('detrend', true); +parser.addParamValue('directory', 'data'); +parser.parse(filename, varargin{:}); +args = parser.Results; + +raw = load([args.directory filesep filename]); + +data = iddata(raw.theta, raw.theta_c, args.sampleTime, ... + 'InterSample', 'foh', ... + 'InputName', {'thetac'}, ... + 'OutputName', {'theta'}); + +if args.detrend + data = detrend(data); +end diff --git a/samples/Matlab/overwrite_settings.m b/samples/Matlab/overwrite_settings.m new file mode 100644 index 00000000..6cae4058 --- /dev/null +++ b/samples/Matlab/overwrite_settings.m @@ -0,0 +1,27 @@ +function settings = overwrite_settings(defaultSettings, overrideSettings) +% function settings = overwrite_settings(defaultSettings, overrideSettings) +% Returns the settings based on a combination of the defaults and the override settings. +% +% Parameters +% ---------- +% defaultSettings : structure +% A structure with all of the default settings. +% overrideSettings : structure +% Contains any settings that should override the defaults. +% +% Returns +% ------- +% settings : structure +% A stucture containing the overrideSettings and any defaults that weren't +% supplied in the overrideSettings. + +% add the default options if not specified by the user +overrideNames = fieldnames(overrideSettings); +defaultNames = fieldnames(defaultSettings); +notGiven = setxor(overrideNames, defaultNames); +settings = overrideSettings; +if length(notGiven) > 0 + for i = 1:length(notGiven) + settings.(notGiven{i}) = defaultSettings.(notGiven{i}); + end +end diff --git a/samples/Matlab/par_text_to_struct.m b/samples/Matlab/par_text_to_struct.m new file mode 100644 index 00000000..4c7de350 --- /dev/null +++ b/samples/Matlab/par_text_to_struct.m @@ -0,0 +1,27 @@ +function par = par_text_to_struct(pathToFile) +% function par = par_text_to_struct(pathToFile) +% Returns a structure of the parameters that were stored in a csv text file. +% +% Parameters +% ---------- +% pathToFile : string +% Path to a text file containing the benchmark parameters for a single +% bicycle. The parameters should be on seperate lines and take this form: +% +% c = 0.08+/-0.01 +% lam = 0.31 +% +% Returns +% ------- +% par : structure +% A structure containing the bicycle parameters. + +fid = fopen(pathToFile); +data = textscan(fid, '%s %s', 'delimiter', '='); +fclose(fid); +names = strtrim(data{1}); +vals = strtrim(regexp(data{2}, '+/-', 'split')); +for i = 1:length(names) + v = vals{i}; + par.(names{i}) = str2num(v{1}); +end diff --git a/samples/Matlab/plant.m b/samples/Matlab/plant.m new file mode 100644 index 00000000..0a597d57 --- /dev/null +++ b/samples/Matlab/plant.m @@ -0,0 +1,75 @@ +function Yc = plant(varargin) +% function Yc = plant(varargin) +% +% Returns the system plant given a number. +% +% Parameters +% ---------- +% varargin : variable +% Either supply a single argument {num} or three arguments {num1, num2, +% ratio}. If a single argument is supplied, then one of the six transfer +% functions will be chosen from the list. If three arguments are chosen, a +% parallel sum of the two plants will be returned. +% +% Option 1 +% -------- +% num : integer, {1, 2, 3, 4, 5, 6} +% A number between 1 and 6 corresponding to the five plants. +% Option 2 +% -------- +% num1 : integer, {1, 2, 3, 4, 5, 6} +% A number between 1 and 6 corresponding to the five plants. +% num2 : integer, {1, 2, 3, 4, 5, 6} +% A number between 1 and 6 corresponding to the five plants. +% percent : double +% The percentage multiplier of the first plant. Should be between 0 +% and 1. The percentage multiplier of the second plant will be 1 - +% percent. +% +% Returns +% ------- +% Either a single plant or a parallel sum of scaled plants. +% +% Option 1 +% -------- +% Yc : transfer function +% 1 : 1 / s +% 2 : 1 / s(s + 1) +% 3 : 1 / s(s + 0.2) +% 4 : 10 / (s + 10) +% 5 : 5 / (s + 10) +% 6 : 10 / (s^2 +.2 * s) +% +% Option 2 +% -------- +% Yc : transfer function +% Yc = percent * Yc1 + (1 - percent) * Yc2, where Yc1 and Yc2 are plants +% from the list shown in Option 1. + +if size(varargin, 2) > 1 + if 0 <= varargin{3} & varargin{3} <= 1 + Yc = parallel(varargin{3} * choose_plant(varargin{1}), ... + (1 - varargin{3}) * choose_plant(varargin{2})); + else + error('Ratio must be between 0 and 1.') + end +else + Yc = choose_plant(varargin{1}); +end + +function p = choose_plant(num) +if num == 1; + p = tf(1.0, [1.0, 0.0]); +elseif num == 2; + p = tf(1.0, [1.0, 1.0, 0.0]); +elseif num == 3; + p = tf(1.0, [1.0, 0.2, 0.0]); +elseif num == 4; + p = tf(10.0, [1.0, 10.0]); +elseif num == 5; + p = tf(5.0, [1.0, 10.0]); +elseif num == 6; + p = tf(10.0, [1.0, 0.2, 0.0]); +else + display('Invalid plant number.') +end diff --git a/samples/Matlab/test_system_state_space.m b/samples/Matlab/test_system_state_space.m new file mode 100644 index 00000000..89871cb9 --- /dev/null +++ b/samples/Matlab/test_system_state_space.m @@ -0,0 +1,69 @@ +gains = [76.3808, -0.0516, 7.2456, 0.2632, 0.0708]; +wnm = 30; +zetanm = 0.707; + +data = generate_data('Rigid', 7.0, 'gains', gains, 'neuroFreq', wnm, ... + 'loopTransfer', 0, 'handlingQuality', 0, 'simulate', 0); + +bicycle = ss(data.modelPar.A, data.modelPar.B, data.modelPar.C, ... + data.modelPar.D); + +bicycle.StateName = {'xP', 'yP', 'psi', 'phi', 'thetaB', 'thetaR', 'delta', ... + 'thetaF', 'phiDot', 'thetaRDot', 'deltaDot'}; +bicycle.OutputName = {'xP', 'yP', 'psi', 'phi', 'thetaB', 'thetaR', 'delta', ... + 'thetaF', 'xPDot', 'yPDot', 'psiDot', 'phiDot', ... + 'thetaBDot', 'thetaRDot', 'deltaDot', 'thetaFDot', 'xQ', 'yQ'}; +bicycle.InputName = {'tPhi', 'tDelta', 'fB'}; + +inputs = {'fB'}; +outputs = [bicycle.OutputName; 'tDelta']; + +analytic = system_state_space('lateral', bicycle, gains, [wnm, zetanm], inputs, outputs); + +numeric = ss(data.system.A, data.system.B, data.system.C, data.system.D); +%numeric.StateName = data.bicycle.states; +%numeric.InputName = data.bicycle.inputs; +%numeric.OutputName = data.bicycle.outputs; + +figure() +pzplot(analytic, numeric) + +% plot the transfer function roll-rate/lateral force for both +figure() +hold all +% plot my analytic model +[num, den] = ss2tf(analytic.A, analytic.B, analytic.C, analytic.D, 1); +mine = tf(num(find(strcmp('phiDot', outputs)), :), den); +bode(tf(num(find(strcmp('phiDot', outputs)), :), den)) +% plot the data from the simulink model +bode(tf(data.forceTF.PhiDot.num, data.forceTF.PhiDot.den)) +[num, den] = ss2tf(numeric.A, numeric.B, numeric.C, numeric.D, 1); +bode(tf(num(12, :), den)) + +display('Analytic Eigenvalues') +eig(analytic.A) +display('Numeric Eigenvalues') +eig(numeric.A) + +% Now see if the heading tracking works. +gains = [76.3808, -0.0516, 7.2456, 0.2632]; +wnm = 30; +zetanm = 0.707; + +par = par_text_to_struct('parameters/RigidPar.txt'); +[A, B, C, D] = whipple_pull_force_ABCD(par, 7.0); +bicycle = ss(A([3, 4, 7, 9, 11], [3, 4, 7, 9, 11]), ... + B([3. 4, 7, 9, 11], [2, 3]), eye(5), 0); +bicycle.StateName = {'psi', 'phi', 'delta', 'phiDot', 'deltaDot'}; +bicycle.OutputName = {'psi', 'phi', 'delta', 'phiDot', 'deltaDot'}; +bicycle.InputName = {'tDelta', 'fB'}; + +inputs = {'fB'}; +outputs = [bicycle.OutputName; 'tDelta']; + +analytic = system_state_space('heading', bicycle, gains, [wnm, zetanm], inputs, outputs); +% the following two should be the same +analytic.A(end, :) +bottomRow = [-wnm^2 * prod(gains), -wnm^2 * prod(gains(1:3)), ... + -wnm^2 * gains(1), -wnm^2 * prod(gains(1:2)), 0, -wnm^2, ... + -2 * wnm * zetanm] diff --git a/samples/Matlab/varargin_to_structure.m b/samples/Matlab/varargin_to_structure.m new file mode 100644 index 00000000..ca14794b --- /dev/null +++ b/samples/Matlab/varargin_to_structure.m @@ -0,0 +1,37 @@ +function options = varargin_to_structure(arguments) +% function options = varargin_to_structure(arguments) +% Returns a structure from a cell array of pairs. +% +% Parameters +% ---------- +% arguments : cell array (1, n) +% A cell array where n is an even number greater than or equal to 2. The odd +% cells must be a character string and the even cells can be any data type. +% +% Returns +% ------- +% options : structure +% The fields of the structure correspond to the odd cells in the array and +% the value for that field corresponds to the even cells. +% +% This is useful for functions that have varargin as an input where the +% variable inputs are keyword pairs. + +% make sure there are enough arguments +if length(arguments) <= 1 + error('Please supply 2 or more arguments') +end + +% make sure they provided and even number of inputs +if mod(length(arguments), 2) ~= 0 + error('There must be an even numbers of arguments') +end + +% store the values in the structure +for i = 1:2:length(arguments) + % make sure they have character strings as all the odd cells + if ~ischar(arguments{i}) + error('The odd arguments must be character strings.') + end + options.(arguments{i}) = arguments{i + 1}; +end diff --git a/samples/Matlab/write_gains.m b/samples/Matlab/write_gains.m new file mode 100644 index 00000000..371e5328 --- /dev/null +++ b/samples/Matlab/write_gains.m @@ -0,0 +1,48 @@ +function write_gains(pathToFile, speeds, gains) +% function write_gains(pathToFile, speeds, gains) +% +% Adds the provided gains to the file. +% +% Parameters +% ---------- +% pathToFile : string +% The path to a gain file. +% speeds : matrix(n, 1) +% gains : matrix (n, 5) +% A matrix of gains where each row corresponds to a speed and the columns +% correspond to the loops starting at the innermost loop. + +contents = importdata(pathToFile); + +speedsInFile = contents.data(:, 1); +gainsInFile = contents.data(:, 2:end); + +% remove any speeds that are very close to the speeds provided +sameSpeedIndices = []; +for i = 1:length(speedsInFile) + for j = 1:length(speeds) + if abs(speedsInFile(i) - speeds(j)) < 1e-3 + sameSpeedIndices = [sameSpeedIndices i]; + end + end +end +speedsInFile(sameSpeedIndices) = []; +gainsInFile(sameSpeedIndices, :) = []; + +% concatenate data +allGains = [gainsInFile; gains]; +allSpeeds = [speedsInFile; speeds]; + +% sort the data +[allSpeeds, order] = sort(allSpeeds); +allGains = allGains(order, :); + +% recombine +all = [allSpeeds, allGains]; + +% rewrite the file +fid = fopen(pathToFile, 'w'); +h = contents.colheaders; +fprintf(fid, '%s,%s,%s,%s,%s,%s\n', h{1}, h{2}, h{3}, h{4}, h{5}, h{6}); +fprintf(fid, '%1.3f,%1.4f,%1.4f,%1.4f,%1.4f,%1.4f\n', all'); +fclose(fid);