Added a nonlinear simulation model and some demo controllers

software/README.md

@@ -2,4 +2,6 @@
 
 - `quanser_original` is the code that can be obtained on the dedicated page on [the product web](https://www.quanser.com/products/active-suspension/) page on Quanser website. Just for archiving purposes here, possibly for use with some older (pre R2021a) release of Matlab and Simulink. **Do not use!**
 
-- `quanser_updated` is the Quanser code after some modifications and upgrades useful or even necessary for our setup. In particular, the data acquisition board was set correctly to our `QPID` DAQ, the configuration file `quanser_win64.tlc` for 64-bit Windows was correctly set, and the Simulink files were saved in the new SLX format (corresponding to R2021a).
\ No newline at end of file
+- `quanser_updated` is the Quanser code after some modifications and upgrades useful or even necessary for our setup. In particular, the data acquisition board was set correctly to our `QPID` DAQ, the configuration file `quanser_win64.tlc` for 64-bit Windows was correctly set, and the Simulink files were saved in the new SLX format (corresponding to R2021a).
+
+- `students` contains code created by students of B3M35ORR for simulating the model and for designing some controllers for it.

software/students/README.md

+# Active suspension simulation & control
+
+This set of scripts and models attempts to create a more faithful simulation
+than what the stock Quanser Simulink schema provides. It also contains some
+controllers that can be used for demonstration purposes.
+
+## How to use this
+
+1. Run the `design_controllers.m` script.
+2. Open the `simulator.slx` Simulink model. This model does not depend on
+   QUARC and can hopefully be opened in any Matlab version R2024a and newer.
+3. Select the controller to use through the manual switches in the bottom
+   left of the schema.
+4. Run the model.
+
+You can also run the provided demonstration controllers on the physical device
+in the lab. To do so, just open the `lab_device.slx` model instead of the
+`simulator.slx` one. To start up the physical model, follow the instructions
+in the top-level README of this repository.
+
+## Used controllers
+
+The demo contains three controllers:
+
+ - A LQR controller. This controller attempts to minimize a linear-quadratic
+   criterion. The weights are chosen such that the relative motion of the
+   upper and lower masses is penalized the most. Out of the three controllers,
+   this one behaves the best when deployed on the real device in the lab.
+ - A H-infinity controller. This controller uses the signal-based approach
+   to design a controller that minimizes the acceleration of the upper mass
+   and also some other metrics. This controller behaves slightly worse --
+   it is a bit too aggressive when trying to damp the upper mass oscillations,
+   leading to oscillations of the lower mass. However, it does work when
+   compared to the open-loop response.
+ - A lag controller. This controller was designed through the `sisotool` to
+   damp the system based on the accelerometer signal from the model.
+   It works fairly well. However, it has some overshoot and the return
+   from the over-shot position is kinda slow.
+
+## State space formats
+
+Two state space formats are used here:
+ - "internal"/"physical" states. This state space is what one would derive
+   through bond graphs or other means. The states are:
+   - `zs` = position of the upper mass wrt. its idle position.
+   - `dzs` = speed of the upper mass.
+   - `zus` = position of the lower mass wrt. its idle position.
+   - `dzus` = speed of the lower mass.
+   This state space is fairly intuitive and corresponds to what one would
+   see in the lab (road steps up -> both masses step up; the same holds for
+   the graphs).
+ - "observed"/"Quanser" states. This state space is slightly different and
+   contains these states:
+   - `zs-zus` = distance of the upper and lower masses wrt. its idle value.
+   - `dzs` = speed of the upper mass.
+   - `zus-zr` = distance of lowre mass and the road platform wrt. its idle value.
+   - `dzus` = speed of the lower mass.
+
+The "Quanser" state space might be more useful for controller design. Its advantage
+is that when the physical model reaches steady state, all states here should
+be zero. This is advantageous for LQR design: the task can then be
+formulated as a regulation task and not a tracking task.
+
+These state spaces *cannot* be converted one to another using a state-space
+similarity transformation. The issue is that the model with "observed" states
+cannot take road height `zr` as an input directly: inputs cannot directly set
+state values (e.g. `zus-zr`). Instead, it must have `dzr`, the derivative of road
+height, as an input; this input is then integrated via the `zus-zr` state.
+The "physical" state-space does not have this limitation, as `zr` only controls
+how the lower mass will accelerate (i.e. it feeds into the `dzus` state through
+a spring factor).
+
+## Matlab files
+
+ - `design_controllers.m` prepares the three controllers described above.
+   It also automatically runs the `load_simulator_constants.m` script
+ - `load_simulator_constants.m` contains the model physical constants for
+   `simulator.slx`.
+ - `simulator.slx` is a Simulink model for simulating the active suspension
+   when not in the lab.
+ - `lab_device.slx` is a Simulink model for running the controllers in the
+   lab. It is derived from the `software/quanser_updated/q_as_lqr.slx` model.
+ - `quanser_order2_filter.m` is a helper script for designing a low-pass filter.
+ - `get_suspension_linear_model.m` is a script that generates a linearized
+   Control System Toolbox state-space model of the active suspension. There
+   are various formats of the model available; see the documentation and
+   code inside.
+
+## Generic notes
+
+- The model constants were determined using scripts in the `/data/students` directory.
+- The H-infinity controller could be improved by using frequency-dependent weights.
+  Currently the weights are just static gains.
+- There are various places where friction can be modelled:
+  - Viscous friction parallel to springs (equivalent to a shock absorber in cars).
+  - Viscous friction in the bearings between masses and the base.
+  - Static friction in the bearings between the masses and the base.
+  The Quanser lab model only assumes the first type of friction.
+  This seemed unrealistic to me -- there is no shock absorber mounted on the
+  physical model in the lab. The Simulink model here uses only the last two
+  types of friction - I assume the bearings are where most of the losses are happening.
+- The way static friction is implemented in the simulation model is not ideal.
+  Currently, it is effectively just a signum function in negative feedback.
+  This breaks Simulink's step-size control. To work around this, a fixed-step
+  ODE4 solver is used with a 1 kHz sample rate. This might be improved
+  by implementing https://www.mathworks.com/help/simulink/slref/friction-model-with-hard-stops.html
+  here or choosing a different approach.

software/students/design_controllers.m

+% Script that designs controllers for both simulation and real model
+clear variables;
+
+%% LQR controller
+model = get_suspension_linear_model("quanser_lqr");
+
+% goal = suppress body movement
+Q = diag([100,100,1,10]);
+R = 0.01;
+
+K = lqr(model.A, model.B(:, 2), Q, R);
+lqr_controller = ss(-K, 'InputName', {'zs_zus', 'dzs', 'zus_zr', 'dzus'}, 'OutputName', {'actuator'});
+
+
+%% H-infinity controller without acceleration input and with mu-synthesis
+model = get_suspension_linear_model("quanser_lqr");
+
+% weighing filters. Only static weight were used for simplicity.
+Wroad   = ss(50,    'InputName', 'road_normed', 'OutputName', 'droad'  );  % scale road disturbance
+Weffort = ss(1,     'InputName', 'actuator',    'OutputName', 'Weffort');  % penalize control effort
+Waccel  = ss(100,   'InputName', 'body_accel',  'OutputName', 'Waccel' );  % penalize body acceleration
+Wspeed  = ss(200,   'InputName', 'dzus',        'OutputName', 'Wspeed' );  % penalize body velocity
+Wsusp   = ss(10000, 'InputName', 'zs_zus',      'OutputName', 'Wsusp'  );  % penalize suspension travel (speeds the system up)
+
+% model input uncertainty
+in_unc = ultidyn('input_uncertainty', [1 1], 'InputName', 'actuator_cmd', 'OutputName', 'actuator_ucmd');
+in_w   = makeweight(0.4, [20*2*pi, 1], 10);
+in_w.InputName = 'actuator_ucmd';
+in_w.OutputName = 'actuator_wucmd';
+in_add = sumblk('actuator = actuator_cmd + actuator_wucmd');
+
+% create augmented plant
+P = connect(model, Wroad, Weffort, Waccel, Wsusp, Wspeed, in_unc, in_w, in_add, ...
+    {'road_normed', 'actuator_cmd'}, ...
+    {'Weffort', 'Waccel', 'Wsusp', 'Wspeed', 'zs_zus', 'dzs', 'zus_zr', 'dzus'});
+
+% design the controller
+hinf_noacc_controller = musyn(P, 4, 1);
+hinf_noacc_controller.InputName = {'zs_zus', 'dzs', 'zus_zr', 'dzus'};
+hinf_noacc_controller.OutputName = {'actuator'};
+
+%% Lag controller
+
+% controller designed in sisotool by trial and error
+s = tf('s');
+lag_controller = -3.2 * (s+50) / (s+4);
+lag_controller.InputName = {'acc'};
+lag_controller.OutputName = {'actuator'};
+
+%% Simulink parameters
+load_simulator_constants;

software/students/get_suspension_linear_model.m

+function sys = get_suspension_linear_model(type)
+    % GET_MODEL(type) Return a CsTbx state space model describing the
+    % active suspension system.
+    % The type field can be used to select model structure and outputs:
+    % - 'quanser_lqr' - this returns a state model with states identical
+    %   to the ones in the Lab Guide for this model. Outputs are just
+    %   these states + an auxiliary output for body acceleration.
+    % - 'physical_state_outputs' - this returns a model with "physical"
+    %   states (position and velocity of both masses individually).
+    %   Outputs are just pass-through of this state.
+    % - 'quanser_state_outputs' - this returns a model with "physical"
+    %   states, but the outputs are combined to look like outputs from
+    %   'quanser_lqr'. (historical note: this was created before
+    %   'quanser_lqr').
+    % - 'scope_signal_outputs' - this returns a model with "physical"
+    %   states and with outputs that correspond to what is in the .mat
+    %   files in the calibration dataset (zr, zus, zs).
+    % - 'hinf_outputs' - this returns a model with "physical" states and
+    %   with various combinations of above outputs. These additional
+    %   outputs are intended to help with construction of performance
+    %   outputs for H-infinity controller design process.
+ 
+    % linear fit
+    ks = 973.6;
+    ku = 2513;
+    r  = 1.771;
+    bs = 7.671;
+    bu = 5.091;
+    gf = 0.7325;
+
+    % nonlinear fit
+    % ks = 987.69; % or 1040 Suspension Stiffness (N/m) 
+    % ku = 2268.7; % or 2300
+    % r  = 1.8917;
+    % bs = 1.4261; % Suspension Inherent Damping coefficient (sec/m)
+    % bu = 2.8573; % Tire Inhenrent Damping coefficient (sec/m)
+    % gf = 0.73662;
+
+    ms = 2.5;
+    mu = ms / r;
+
+    if nargin >= 1 && strcmp(type, 'quanser_lqr')
+        A = [
+            0, 1, 0, -1;
+            -ks/ms, -bs/ms, 0, 0;
+            0,0,0,1;
+            ks/mu, 0, -ku/mu, -bu/mu;
+        ];
+        B = [
+            0,0;
+            0,+gf/ms;
+            -1,0;
+            0,-gf/mu
+        ];
+        C = [
+            eye(4);
+            -ks/ms, -bs/ms, 0, 0;
+        ];
+        D = [
+            zeros(4,2);
+            0,+gf/ms;
+        ];
+
+        sys = ss(A,B,C,D);
+        sys.name = 'Active suspension (internal Quanser state)';
+        sys.StateName = {'zs_zus', 'dzs', 'zus_zr', 'dzus'};
+        sys.StateUnit = {'m', 'm/s', 'm', 'm/s'};
+        sys.InputName = {'droad', 'actuator'};
+        sys.InputUnit = {'m/s', 'N'};
+        sys.OutputName = {'zs_zus', 'dzs', 'zus_zr', 'dzus', 'body_accel'};
+        sys.OutputUnit = {'m', 'm/s', 'm', 'm/s', 'm/s^2'};
+        return;
+    end
+
+
+    acc_filt = quanser_order2_filter(70, 1, 'lpf');
+
+    E = diag([1, ms, 1, mu, 1, 1]);
+    A = [
+          0,   1,      0,   0,    0,    0;
+        -ks, -bs,    +ks,   0,    0,    0;
+          0,   0,      0,   1,    0,    0;
+        +ks,   0, -ks-ku, -bu,    0,    0;
+        [acc_filt.B * [-ks, -bs,    +ks,   0]/ms, acc_filt.A]
+    ];
+    B = [
+          0,  0;
+          0, +gf;
+          0,  0;
+        +ku, -gf;
+        [acc_filt.B * [ 0, +gf/ms]]
+    ];
+    if nargin < 1 || strcmp(type, 'physical_state_outputs')
+        C = [eye(4), zeros(4,2)];
+        D = zeros(4,2);
+        model_name = 'Active suspension (physical state outputs)';
+        output_names = {'zs', 'dzs', 'zus', 'dzus'};
+        output_units = {'m', 'm/s', 'm', 'm/s'};
+    elseif strcmp(type, 'quanser_state_outputs')
+        C = [
+            1, 0, -1, 0, 0, 0;
+            0, 1,  0, 0, 0, 0;
+            0, 0,  1, 0, 0, 0;
+            0, 0,  0, 1, 0, 0;
+            [zeros(1,4), acc_filt.C]
+        ];
+        D = [
+            0, 0;
+            0, 0;
+            -1, 0;
+            0, 0;
+            zeros(1,2)
+        ];
+        model_name = 'Active suspension (Quanser state outputs)';
+        output_names = {'zs_zus', 'dzs', 'zus_zr', 'dzus', 'acc'};
+        output_units = {'m', 'm/s', 'm', 'm/s', 'm/s^2'};
+    elseif strcmp(type, 'scope_signal_outputs')
+        C = [
+            0 0 0 0, 0, 0;
+            0 0 1 0, 0, 0;
+            1 0 0 0, 0, 0;
+            [zeros(1,4), acc_filt.C]
+        ];
+        D = [
+            1 0;
+            0 0;
+            0 0;
+            0 0
+        ];
+        model_name = 'Active suspension (MAT file signal outputs)';
+        output_names = {'zr', 'zus', 'zs', 'acc'};
+        output_units = {'m', 'm', 'm', 'm/s^2'};
+    elseif strcmp(type, 'hinf_outputs')
+        C = [
+            % susp travel
+            1, 0, -1, 0, 0, 0;
+            % tyre travel
+            0, 0, +1, 0, 0, 0;
+            % body accel
+            -ks/ms, -bs/ms, +ks/ms,   0, 0, 0;
+            % quanser state
+            1, 0, -1, 0, 0, 0;
+            0, 1,  0, 0, 0, 0;
+            0, 0,  1, 0, 0, 0;
+            0, 0,  0, 1, 0, 0;
+            % physical state
+            1, 0, 0, 0, 0, 0;
+            0, 0, 1, 0, 0, 0
+            % accel output
+            [zeros(1,4), acc_filt.C];
+        ];
+        D = [
+            % susp travel
+            0, 0;
+            % tyre travel
+            -1, 0;
+            % body accel
+            0, +gf/ms;
+            % quanser state
+            0, 0;
+            0, 0;
+            -1, 0;
+            0, 0;
+            % physical state
+            0, 0;
+            0, 0;
+            % accel output
+            0 0
+        ];
+        model_name = 'Active suspension (Hinfinity outputs)';
+        output_names = {
+            'susp_travel';
+            'tyre_deflect';
+            'body_accel';
+            'zs_zus'; 'dzs'; 'zus_zr'; 'dzus';
+            'zs'; 'zus';
+            'acc'
+        };
+        output_units = {
+            'm'; 'm'; 'm/s^2';
+            'm'; 'm/s'; 'm'; 'm/s';
+            'm'; 'm';
+            'm/s^2'
+        };
+    else
+        error("Unknown model format: %s", type);
+    end
+    sys = ss(E\A, E\B, C, D);
+    sys.name = model_name;
+    sys.StateName = {'zs', 'dzs', 'zus', 'dzus', 'accfilt1', 'accfilt2'};
+    sys.StateUnit = {'m', 'm/s', 'm', 'm/s', '-', '-'};
+    sys.InputName = {'road', 'actuator'};
+    sys.InputUnit = {'m', 'N'};
+    sys.OutputName = output_names;
+    sys.OutputUnit = output_units;
+end

software/students/load_simulator_constants.m

+% Script that contains model physical constants for the simulator
+
+% linear friction
+% ks = 973.6;
+% ku = 2513;
+% r  = 1.771;
+% bs = 7.671;
+% bu = 5.091;
+% fs = 0;
+% fu = 0;
+% gf = 0.7325;
+
+% nonlinear friction
+ks = 987.69; % Stiffness of the spring between upper and lower masses (N/m) 
+ku = 2268.7; % Stiffness of the spring between lower mass and road (N/m)
+r  = 1.8917; % Ratio of masses ... ms/mu
+bs = 1.4261; % Coefficient of viscous friction between the upper mass and the base/rail (N/(m/s))
+bu = 2.8573; % Coefficient of viscous friction between the lower mass and the base/rail (N/(m/s))
+fs = 0.52327; % Coefficient of static friction between the upper mass and the base/rail (N)
+fu = 0.45537; % Coefficient of static friction between the lower mass and the base/rail (N)
+gf = 0.73662; % Actuator strength per unit signal (N/-)
+
+ms = 2.5;    % Mass of the upper mass
+mu = ms / r; % Mass of the lower mass
+
+% Various filters from the QUARC testbench
+accelerometer_filter = quanser_order2_filter(70, 1, 'lpf');
+speed_estimator = quanser_order2_filter(500, 1, 'hpf');

software/students/quanser_order2_filter.m

+function system = quanser_order2_filter(Wn, z, type)
+    % QUANSER_ORDER2_FILTER(Wn, z, type) Create a 2nd order HPF/LPF filter.
+    %
+    % The Wn parameter corresponds to filter natural resonance frequency.
+    % The z parameter corresponds to the filter damping ratio (theta).
+    % The type parameter determines what type of filter will be returned.
+    % Possible options are 'lpf', 'hpf' and 'both' (the last option will
+    % return a system with two outputs). The last parameter can be omitted.
+    %
+    % This filter is equivalent to the one in the default QUARC Simulink
+    % testbench. Parameters Wn and z are typically put into Simulink as
+    % constant blocks there.
+
+    diff_A = [
+        -2*z*Wn, -Wn;
+          Wn,    0
+    ];
+    diff_B = [
+        Wn;
+        0
+    ];
+    diff_C = [
+        0, 1;
+        Wn, 0
+    ];
+    diff_D = [0; 0];
+
+    if nargin < 3 || strcmp(type, 'both')
+        system = ss(diff_A, diff_B, diff_C, diff_D);
+        system.InputName = 'input';
+        system.OutputName = {'lowpass', 'highpass'};
+
+    elseif strcmp(type, 'lpf')
+        system = ss(diff_A, diff_B, diff_C(1,:), diff_D(1,:));
+        system.InputName = 'input';
+        system.OutputName = {'lowpass'};
+
+    elseif strcmp(type, 'hpf')
+        system = ss(diff_A, diff_B, diff_C(2,:), diff_D(2,:));
+        system.InputName = 'input';
+        system.OutputName = {'highpass'};
+
+    else
+        error("Unknown filter type: " + type);
+    end
+end