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

@@ -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;

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);

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'};

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);

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

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);

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)

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

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

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

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

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

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

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]

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

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);