Update some Simscape models
This commit is contained in:
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,79 +0,0 @@
|
|||||||
function [sys] = identifyPlant(opts_param)
|
|
||||||
%% Default values for opts
|
|
||||||
opts = struct();
|
|
||||||
|
|
||||||
%% Populate opts with input parameters
|
|
||||||
if exist('opts_param','var')
|
|
||||||
for opt = fieldnames(opts_param)'
|
|
||||||
opts.(opt{1}) = opts_param.(opt{1});
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
%% Options for Linearized
|
|
||||||
options = linearizeOptions;
|
|
||||||
options.SampleTime = 0;
|
|
||||||
|
|
||||||
%% Name of the Simulink File
|
|
||||||
mdl = 'sim_nano_station_id';
|
|
||||||
|
|
||||||
%% Input/Output definition
|
|
||||||
io(1) = linio([mdl, '/Fn'], 1, 'input'); % Cartesian forces applied by NASS
|
|
||||||
io(2) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion
|
|
||||||
io(3) = linio([mdl, '/Fs'], 1, 'input'); % External forces on the sample
|
|
||||||
io(4) = linio([mdl, '/Fnl'], 1, 'input'); % Forces applied on the NASS's legs
|
|
||||||
io(5) = linio([mdl, '/Fd'], 1, 'input'); % Disturbance Forces
|
|
||||||
io(6) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample
|
|
||||||
io(7) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
|
|
||||||
io(8) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
|
|
||||||
io(9) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base
|
|
||||||
io(10) = linio([mdl, '/Vlm'], 1, 'output'); % Measured absolute velocity of the legs
|
|
||||||
|
|
||||||
%% Run the linearization
|
|
||||||
G = linearize(mdl, io, options);
|
|
||||||
G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
|
|
||||||
'Dgx', 'Dgy', 'Dgz', ...
|
|
||||||
'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
|
|
||||||
'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
|
|
||||||
'Frzz', 'Ftyx', 'Ftyz'};
|
|
||||||
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
|
|
||||||
'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
|
|
||||||
'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
|
|
||||||
'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ...
|
|
||||||
'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
|
|
||||||
|
|
||||||
%% Create the sub transfer functions
|
|
||||||
minreal_tol = sqrt(eps);
|
|
||||||
% From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
|
|
||||||
sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
|
|
||||||
% From ground motion to Sample displacement
|
|
||||||
sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false);
|
|
||||||
% From direct forces applied on the sample to displacement of the sample
|
|
||||||
sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false);
|
|
||||||
% From forces applied on NASS's legs to force sensor in each leg
|
|
||||||
sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
|
||||||
% From forces applied on NASS's legs to displacement of each leg
|
|
||||||
sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
|
||||||
% From forces/torques applied by the NASS to position error
|
|
||||||
sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
|
|
||||||
% From forces/torques applied by the NASS to velocity of the actuator
|
|
||||||
sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
|
||||||
% From various disturbance forces to position error
|
|
||||||
sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false);
|
|
||||||
|
|
||||||
%% We remove low frequency and high frequency dynamics that are usually unstable
|
|
||||||
% using =freqsep= is risky as it may change the shape of the transfer functions
|
|
||||||
% f_min = 0.1; % [Hz]
|
|
||||||
% f_max = 1e4; % [Hz]
|
|
||||||
|
|
||||||
% [~, sys.G_cart] = freqsep(freqsep(sys.G_cart, 2*pi*f_max), 2*pi*f_min);
|
|
||||||
% [~, sys.G_gm] = freqsep(freqsep(sys.G_gm, 2*pi*f_max), 2*pi*f_min);
|
|
||||||
% [~, sys.G_fs] = freqsep(freqsep(sys.G_fs, 2*pi*f_max), 2*pi*f_min);
|
|
||||||
% [~, sys.G_iff] = freqsep(freqsep(sys.G_iff, 2*pi*f_max), 2*pi*f_min);
|
|
||||||
% [~, sys.G_dleg] = freqsep(freqsep(sys.G_dleg, 2*pi*f_max), 2*pi*f_min);
|
|
||||||
% [~, sys.G_plant] = freqsep(freqsep(sys.G_plant, 2*pi*f_max), 2*pi*f_min);
|
|
||||||
|
|
||||||
%% We finally verify that the system is stable
|
|
||||||
if ~isstable(sys.G_cart) || ~isstable(sys.G_gm) || ~isstable(sys.G_fs) || ~isstable(sys.G_iff) || ~isstable(sys.G_dleg) || ~isstable(sys.G_plant)
|
|
||||||
warning('One of the identified system is unstable');
|
|
||||||
end
|
|
||||||
end
|
|
||||||
@@ -1,72 +0,0 @@
|
|||||||
% identifyPlantFRF
|
|
||||||
% :PROPERTIES:
|
|
||||||
% :header-args:matlab+: :tangle ../src/identifyPlantFRF.m
|
|
||||||
% :header-args:matlab+: :comments org :mkdirp yes
|
|
||||||
% :header-args:matlab+: :eval no :results none
|
|
||||||
% :END:
|
|
||||||
% <<sec:identifyPlant>>
|
|
||||||
|
|
||||||
% This Matlab function is accessible [[file:../src/identifyPlant.m][here]].
|
|
||||||
|
|
||||||
|
|
||||||
function [sys] = identifyPlantFRF(opts_param)
|
|
||||||
%% Default values for opts
|
|
||||||
opts = struct();
|
|
||||||
|
|
||||||
%% Populate opts with input parameters
|
|
||||||
if exist('opts_param','var')
|
|
||||||
for opt = fieldnames(opts_param)'
|
|
||||||
opts.(opt{1}) = opts_param.(opt{1});
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
%% Name of the Simulink File
|
|
||||||
mdl = 'sim_nano_station_id';
|
|
||||||
|
|
||||||
%% Input/Output definition
|
|
||||||
io(1) = linio([mdl, '/Fn'], 1, 'input'); % Cartesian forces applied by NASS
|
|
||||||
io(2) = linio([mdl, '/Dw'], 1, 'input'); % Ground Motion
|
|
||||||
io(3) = linio([mdl, '/Fs'], 1, 'input'); % External forces on the sample
|
|
||||||
io(4) = linio([mdl, '/Fnl'], 1, 'input'); % Forces applied on the NASS's legs
|
|
||||||
io(5) = linio([mdl, '/Fd'], 1, 'input'); % Disturbance Forces
|
|
||||||
io(6) = linio([mdl, '/Dsm'], 1, 'output'); % Displacement of the sample
|
|
||||||
io(7) = linio([mdl, '/Fnlm'], 1, 'output'); % Force sensor in NASS's legs
|
|
||||||
io(8) = linio([mdl, '/Dnlm'], 1, 'output'); % Displacement of NASS's legs
|
|
||||||
io(9) = linio([mdl, '/Es'], 1, 'output'); % Position Error w.r.t. NASS base
|
|
||||||
io(10) = linio([mdl, '/Vlm'], 1, 'output'); % Measured absolute velocity of the legs
|
|
||||||
|
|
||||||
input = frest.Random('Ts',1e-4);
|
|
||||||
|
|
||||||
%% Run the linearization
|
|
||||||
G = frestimate(mdl, io, input);
|
|
||||||
% G.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz', ...
|
|
||||||
% 'Dgx', 'Dgy', 'Dgz', ...
|
|
||||||
% 'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz', ...
|
|
||||||
% 'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
|
|
||||||
% 'Frzz', 'Ftyx', 'Ftyz'};
|
|
||||||
% G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', ...
|
|
||||||
% 'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6', ...
|
|
||||||
% 'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6', ...
|
|
||||||
% 'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz', ...
|
|
||||||
% 'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
|
|
||||||
|
|
||||||
% %% Create the sub transfer functions
|
|
||||||
% minreal_tol = sqrt(eps);
|
|
||||||
% % From forces applied in the cartesian frame to displacement of the sample in the cartesian frame
|
|
||||||
% sys.G_cart = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
|
|
||||||
% % From ground motion to Sample displacement
|
|
||||||
% sys.G_gm = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Dgx', 'Dgy', 'Dgz'}), minreal_tol, false);
|
|
||||||
% % From direct forces applied on the sample to displacement of the sample
|
|
||||||
% sys.G_fs = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Fsx', 'Fsy', 'Fsz', 'Msx', 'Msy', 'Msz'}), minreal_tol, false);
|
|
||||||
% % From forces applied on NASS's legs to force sensor in each leg
|
|
||||||
% sys.G_iff = minreal(G({'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
|
||||||
% % From forces applied on NASS's legs to displacement of each leg
|
|
||||||
% sys.G_dleg = minreal(G({'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
|
||||||
% % From forces/torques applied by the NASS to position error
|
|
||||||
% sys.G_plant = minreal(G({'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'}, {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'}), minreal_tol, false);
|
|
||||||
% % From forces/torques applied by the NASS to velocity of the actuator
|
|
||||||
% sys.G_geoph = minreal(G({'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), minreal_tol, false);
|
|
||||||
% % From various disturbance forces to position error
|
|
||||||
% sys.G_dist = minreal(G({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'Frzz', 'Ftyx', 'Ftyz'}), minreal_tol, false);
|
|
||||||
|
|
||||||
end
|
|
||||||
Reference in New Issue
Block a user