1802 lines
125 KiB
Matlab
1802 lines
125 KiB
Matlab
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%% BIRDy: Benchmark for Identification of Robot Dynamics %%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Benchmark initialization:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
% Reset workspace
|
|
clear
|
|
close all
|
|
|
|
% Automatically add the Benchmark folder and its subfolders to the path:
|
|
addpath('Utils');
|
|
addpath(genpath('Utils'));
|
|
addpath('Benchmark');
|
|
addpath(genpath('Benchmark'));
|
|
|
|
% Provide the name of the robot to be identified:
|
|
robotName = 'TX40_uncoupled'; % [TX40, TX40_uncoupled, RV2SQ]
|
|
|
|
% Identify the robot using a subset of the experiment trajectory [t_i, t_f]:
|
|
t_i = 0; % Start time.
|
|
t_f = 10; % End time.
|
|
numberOfInitialEstimates = 25; % Number of different initial estimates (25).
|
|
sdOfInitialEstimates = 0.15; % Standard deviation of the initial estimates.
|
|
freq_decim = 500; % Decimation frequency used to compute the decimation rate (see the Matlab 'decimate' documentation).
|
|
samplingBorder = 50; % Number of samples to be removed from the beginning and the end of the data sequence, in order to mitigate filtering artefacts... (50)
|
|
gammaReg = 1e0; % Regularization factor (for methods where regularization is activated)
|
|
|
|
% Real or Simulated robot ?
|
|
experimentOnTrueRobot = false; % If true, indicates that the benchmark is executed on experiment data collected on a true robot.
|
|
identifyDriveGains = false; % If true, indicates that the drive gain identification process should be executed.
|
|
|
|
% Important parameters:
|
|
functionTolerance = 2.5e-2; % Function tolerance 2.5%
|
|
stepTolerance = 2.5e-2; % Step tolerance 2.5%
|
|
iterationMax = 10; % Maximumm number of iterations
|
|
|
|
% Set the noise level for which the identification has to be done: 'oldNoise', 'lowPositionNoise', 'standardNoise', 'highPositionNoise', 'highTorqueNoise' and 'highPositionTorqueNoise'
|
|
noiseLevel = 'lowPositionNoise';
|
|
% noiseLevel = 'standardNoise';
|
|
% noiseLevel = 'highPositionNoise';
|
|
% noiseLevel = 'highTorqueNoise';
|
|
% noiseLevel = 'highPositionTorqueNoise';
|
|
% noiseLevel = 'oldNoise';
|
|
|
|
initialParamType = 'RefSd'; % Initial parameter estimate for algorithms that require it: 'RefSd', 'LS', 'LS_f'
|
|
regenerateModel = true; % If true, recomputes the robot symbolic model.
|
|
regenerateTrajectory = true; % If true, launch the generation routine for new excitation trajectory.
|
|
regenerateData = true; % If true, generate new robot simulation data.
|
|
generateROSTrajectory = false; % If true, generate a .yaml file containing the joint trajectory
|
|
runPostProcessing = true; % If true, runs the post processing function to graph the results.
|
|
integrationAlgorithm = 'rk1'; % Defines the integration algorithm used during Identification : 'rk1', 'rk2', 'rk4' or 'ode45'.
|
|
interpolator.Algorithm = 'makima'; % Defines the interpolation algorithm used for trajctory data: 'linear', 'nearest', 'next', 'previous', 'pchip', 'cubic', 'v5cubic', 'makima', or 'spline'.
|
|
interpolator.expAlgorithm = 'nearest'; % Defines the interpolation algorithm used for experiment data: 'linear', 'nearest', 'next', 'previous', 'pchip', 'cubic', 'v5cubic', 'makima', or 'spline'.
|
|
interpolator.visualizeInterpolator = true; % If true, triggers visualization of interpolated trajectories with the different available methods.
|
|
|
|
% MEX files compilation:
|
|
recompileMexFiles = true; % Recompile mex files of the project if true.
|
|
% buildList = [1 1 1 1 1 1 1 1 1 1 1 1 1]; % List of mex files to be regenerated [torqueVector_mex observationMatrix_mex integrateClosedLoopDynamics_mex ekf_opt_mex srekf_opt_mex ukf_opt_mex srukf_opt_mex cdkf_opt_mex srcdkf_opt_mex pf_opt_mex compute_ML_Cost_mex]
|
|
buildList = [0 0 0 0 0 0 0 0 0 0 0 0 0];
|
|
% Filter parameters:
|
|
% Low pass filter:
|
|
filter.lowpass.d = fdesign.lowpass('Fp,Fst,Ap,Ast', 50, 110, 0.01, 60, 1000); % Design of the filter for the LS with filtered joint acceleration.
|
|
filter.lowpass.Hd = design(filter.lowpass.d, 'equiripple');
|
|
% Butterworth filter:
|
|
filter.butterworth.freq_fil = 50;
|
|
filter.butterworth.nfilt = 4;
|
|
|
|
% Cosmetic parameters:
|
|
verboseFlag = true; % If true, the solver communicates the identification error at each step.
|
|
debugFlag = false; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
displayProgression = true; % If true, display progression GUI for each identification algorithm.
|
|
filter.visualizeFilter = true; % Filter tuning
|
|
displayTrajectory = true; % If true, display the robot joint trajectory.
|
|
displayControlPerformance = true; % If true, display the robot tracking performance on the generated trajectory.
|
|
getKinematicSymbolicExpressions = true; % If true, displays the robot homogeneous transforms and jacobians in symbolic form. Note that identificationMethods.isActivated should be zero for this to be working properly.
|
|
|
|
% Select the identification methods :
|
|
identificationMethods.algName = {
|
|
'OLS', 'OLS_f',...
|
|
'WLS', 'WLS_f', ...
|
|
'TLS', 'TLS_f', ...
|
|
'IRLS', 'IRLS_f', ...
|
|
'ML', 'ML_f','IV',...
|
|
'DIDIM', 'CLIE', 'CLOE', ...
|
|
'EKF', 'SREKF', ...
|
|
'UKF', 'SRUKF', ...
|
|
'CDKF', 'SRCDKF', 'PF', ...
|
|
'ANN', 'ANN_f', ...
|
|
'HTRNN', 'HTRNN_f',...
|
|
'PC_OLS', 'PC_OLS_f',...
|
|
'PC_WLS', 'PC_WLS_f',...
|
|
'PC_IRLS', 'PC_IRLS_f',...
|
|
'PC_OLS_Euclidean', 'PC_OLS_Euclidean_f',...
|
|
'PC_OLS_Entropic', 'PC_OLS_Entropic_f',...
|
|
'PC_OLS_ConstPullback', 'PC_OLS_ConstPullback_f',...
|
|
'PC_IV', 'PC_DIDIM'};
|
|
|
|
|
|
identificationMethods.isActivated = ...
|
|
[1,...% OLS
|
|
0,... % OLS_f
|
|
0,... % WLS
|
|
0,... % WLS_f
|
|
0,... % TLS
|
|
0,... % TLS_f
|
|
0,... % IRLS
|
|
0,... % IRLS_f
|
|
0,... % ML
|
|
0,... % ML_f
|
|
0,... % IV
|
|
0,... % DIDIM
|
|
0,... % CLIE
|
|
0,... % CLOE
|
|
0,... % EKF
|
|
0,... % SREKF
|
|
0,... % UKF
|
|
0,... % SRUKF
|
|
0,... % CDKF
|
|
0,... % SRCDKF
|
|
0,... % PF
|
|
0,... % ANN
|
|
0,... % ANN_f
|
|
0,... % HTRNN
|
|
0,... % HTRNN_f
|
|
0,... % PC_OLS
|
|
0,... % PC_OLS_f
|
|
0,... % PC_WLS
|
|
0,... % PC_WLS_f
|
|
0,... % PC_IRLS
|
|
0,... % PC_IRLS_f
|
|
0,... % PC_OLS_Euclidian
|
|
0,... % PC_OLS_Euclidian_f
|
|
0,... % PC_OLS_Entropic
|
|
0,... % PC_OLS_Entropic_f
|
|
0,... % PC_OLS_ConstPullback
|
|
0,... % PC_OLS_ConstPullback_f
|
|
0,... % PC_IV
|
|
0]; % PC_DIDIM
|
|
|
|
|
|
identificationMethods.showResults = ...
|
|
[1,...% OLS
|
|
0,... % OLS_f
|
|
0,... % WLS
|
|
0,... % WLS_f
|
|
0,... % TLS
|
|
0,... % TLS_f
|
|
0,... % IRLS
|
|
0,... % IRLS_f
|
|
0,... % ML
|
|
0,... % ML_f
|
|
0,... % IV
|
|
0,... % DIDIM
|
|
0,... % CLIE
|
|
0,... % CLOE
|
|
0,... % EKF
|
|
0,... % SREKF
|
|
0,... % UKF
|
|
0,... % SRUKF
|
|
0,... % CDKF
|
|
0,... % SRCDKF
|
|
0,... % PF
|
|
0,... % ANN
|
|
0,... % ANN_f
|
|
0,... % HTRNN
|
|
0,... % HTRNN_f
|
|
0,... % PC_OLS
|
|
0,... % PC_OLS_f
|
|
0,... % PC_WLS
|
|
0,... % PC_WLS_f
|
|
0,... % PC_IRLS
|
|
0,... % PC_IRLS_f
|
|
0,... % PC_OLS_Euclidian
|
|
0,... % PC_OLS_Euclidian_f
|
|
0,... % PC_OLS_Entropic
|
|
0,... % PC_OLS_Entropic_f
|
|
0,... % PC_OLS_ConstPullback
|
|
0,... % PC_OLS_ConstPullback_f
|
|
0,... % PC_IV
|
|
0]; % PC_DIDIM
|
|
|
|
% Run the initialization routine:
|
|
[robot, benchmarkSettings, experimentDataStruct, startBenchmark, progressBar] = ...
|
|
initBenchmark(robotName, identificationMethods, getKinematicSymbolicExpressions, recompileMexFiles, displayProgression, regenerateModel, ...
|
|
regenerateTrajectory, regenerateData, displayTrajectory, displayControlPerformance, t_i, t_f, integrationAlgorithm, interpolator, numberOfInitialEstimates, ...
|
|
sdOfInitialEstimates, freq_decim, samplingBorder, buildList, filter, generateROSTrajectory, experimentOnTrueRobot, identifyDriveGains, initialParamType, noiseLevel, gammaReg);
|
|
fprintf('Decimation rate: %d\n',benchmarkSettings.decimRate);
|
|
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Robot Drive Gains Identification Process (only for real robot data):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identifyDriveGains == true && experimentOnTrueRobot == true
|
|
|
|
disp("%%%%%%%% Starting Drive Gain Identification %%%%%%%%")
|
|
|
|
% Set Drive Gains Identification Options:
|
|
optionsDriveGainsId.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsDriveGainsId.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsDriveGainsId.solver = 'svd'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function
|
|
optionsDriveGainsId.alg = 'WLS'; % [LS]: classic Least Squares, [WLS]: Weighted Least Squares, [TLS]: Total Least Squares, [RR]: Ridge Regression, [WRR]: Weighted Ridge Regression
|
|
optionsDriveGainsId.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run Drive Gains Identification:
|
|
results_DriveGains = run_Drive_Gains_Id(robot, benchmarkSettings, experimentDataStruct, optionsDriveGainsId, progressBar);
|
|
|
|
disp("%%%%%%%% Finished Grive Gain Identification %%%%%%%%")
|
|
|
|
benchmarkSettings.identificationMethods.isActivated = 0*identificationMethods.isActivated; % Deactivate other identification methods
|
|
identificationMethods.isActivated = 0*identificationMethods.isActivated;
|
|
benchmarkSettings.identificationMethods.showResults = 0*identificationMethods.showResults;
|
|
runPostProcessing = false;
|
|
end
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if (startBenchmark==true) || (runPostProcessing == true)
|
|
|
|
methodIndex=1;
|
|
disp("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
|
|
disp("%%%%%%%%%%%% Starting Computations %%%%%%%%%%%%")
|
|
disp("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Inverse Dynamic Identification Model and Ordinary Least Square (IDIM-OLS):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % IDIM_OLS
|
|
|
|
disp("%%%%%%%%%%%%%%% Starting IDIM-OLS %%%%%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_OLS Options:
|
|
optionsIDIM_LS.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsIDIM_LS.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsIDIM_LS.solver ='backslash'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsIDIM_LS.alg = 'OLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsIDIM_LS.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsIDIM_LS.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsIDIM_LS.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run IDIM_OLS Identification:
|
|
results_OLS = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsIDIM_LS, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_OLS', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%% Finished IDIM-OLS %%%%%%%%%%%%%%%%")
|
|
|
|
end % end IDIM_OLS
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% IDIM and Ordinary Least Square (IDIM-OLS) with Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % IDIM_OLS with 2-pass-Butterworth filter
|
|
|
|
disp("%%%%%%%%%%%%% Starting IDIM-OLS_f %%%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_OLS_f Options:
|
|
optionsIDIM_LS_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsIDIM_LS_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsIDIM_LS_f.solver = 'backslash'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsIDIM_LS_f.alg = 'OLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsIDIM_LS_f.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsIDIM_LS_f.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsIDIM_LS_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run IDIM_OLS_f Identification:
|
|
results_OLS_f = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsIDIM_LS_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_OLS_f', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%% Finished IDIM-OLS_f %%%%%%%%%%%%%%%%")
|
|
|
|
end % end IDIM_OLS_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Inverse Dynamic Identification Model and Weighted Least Square (IDIM-WLS):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % IDIM_WLS
|
|
|
|
disp("%%%%%%%%%%%%%%%% Starting IDIM-WLS %%%%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_WLS Options:
|
|
optionsIDIM_WLS.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsIDIM_WLS.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsIDIM_WLS.solver ='backslash'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsIDIM_WLS.alg = 'WLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsIDIM_WLS.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsIDIM_WLS.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsIDIM_WLS.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run IDIM_WLS Identification:
|
|
results_WLS = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsIDIM_WLS, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_WLS', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%% Finished IDIM-WLS %%%%%%%%%%%%%%%%")
|
|
|
|
end % end IDIM_WLS
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% IDIM and Weighted Least Square (IDIM-WLS) with Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % IDIM_WLS with 2-pass-Butterworth filter
|
|
|
|
disp("%%%%%%%%%%%%%% Starting IDIM-WLS_f %%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_WLS_f Options:
|
|
optionsIDIM_WLS_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsIDIM_WLS_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsIDIM_WLS_f.solver = 'backslash'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsIDIM_WLS_f.alg = 'WLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsIDIM_WLS_f.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsIDIM_WLS_f.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsIDIM_WLS_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run IDIM_WLS_f Identification:
|
|
results_WLS_f = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsIDIM_WLS_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_WLS_f', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%% Finished IDIM-WLS_f %%%%%%%%%%%%%%%%")
|
|
|
|
end % end IDIM_WLS_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Inverse Dynamic Identification Model and Total Least Square (IDIM-TLS):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % IDIM_TLS
|
|
|
|
disp("%%%%%%%%%%%%%%%% Starting IDIM-TLS %%%%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_TLS Options:
|
|
optionsIDIM_TLS.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsIDIM_TLS.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsIDIM_TLS.solver = 'svd_ridge'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [svd]: compute TLS with svd, [recursive]: compute TLS using Gauss-Newton recursion.
|
|
optionsIDIM_TLS.alg = 'TLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsIDIM_TLS.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Set IDIM_TLS Stop criteria:
|
|
optionsIDIM_TLS.stopCrit.tol_1 = functionTolerance;
|
|
optionsIDIM_TLS.stopCrit.Max_it = iterationMax;
|
|
|
|
% Run IDIM_TLS Identification:
|
|
results_TLS = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsIDIM_TLS, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_TLS', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%% Finished IDIM-TLS %%%%%%%%%%%%%%%")
|
|
|
|
end % end IDIM_TLS
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Total Least Square (IDIM-TLS_f) with Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % IDIM_TLS_f
|
|
|
|
disp("%%%%%%%%%%%%%%%% Starting IDIM-TLS_f %%%%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_TLS Options:
|
|
optionsIDIM_TLS_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsIDIM_TLS_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsIDIM_TLS_f.solver = 'svd_ridge'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [svd]: compute TLS with svd, [recursive]: compute TLS using Gauss-Newton recursion.
|
|
optionsIDIM_TLS_f.alg = 'TLS'; % [LS]: classic Least Squares, [WLS]: Weighted Least Squares, [TLS]: Total Least Squares, [RR]: Ridge Regression, [WRR]: Weighted Ridge Regression
|
|
optionsIDIM_TLS_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Set IDIM_TLS Stop criteria:
|
|
optionsIDIM_TLS_f.stopCrit.tol_1 = functionTolerance;
|
|
optionsIDIM_TLS_f.stopCrit.Max_it = iterationMax;
|
|
|
|
% Run IDIM_TLS Identification:
|
|
results_TLS_f = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsIDIM_TLS_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_TLS_f', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%% Finished IDIM-TLS_f %%%%%%%%%%%%%%%")
|
|
|
|
end % end IDIM_TLS_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Inverse Dynamic Identification Model and Iteratively Reweighted Least Square (IDIM-IRLS):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % IDIM_IRLS
|
|
|
|
disp("%%%%%%%%%%%%%%%% Starting IDIM-IRLS %%%%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_WLS Options:
|
|
optionsIDIM_IRLS.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsIDIM_IRLS.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsIDIM_IRLS.solver ='backslash'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function
|
|
optionsIDIM_IRLS.alg = 'IRLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsIDIM_IRLS.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsIDIM_IRLS.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsIDIM_IRLS.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run IDIM_WLS Identification:
|
|
results_IRLS = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsIDIM_IRLS, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_IRLS', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%% Finished IDIM-IRLS %%%%%%%%%%%%%%%%")
|
|
|
|
end % end IDIM_WLS
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% IDIM and Iteratively Reweighted Least Square (IDIM-IRLS) with Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % IDIM_IRLS with 2-pass-Butterworth filter
|
|
|
|
disp("%%%%%%%%%%%%%% Starting IDIM-IRLS_f %%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_WLS_f Options:
|
|
optionsIDIM_IRLS_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsIDIM_IRLS_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsIDIM_IRLS_f.solver = 'backslash'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function
|
|
optionsIDIM_IRLS_f.alg = 'IRLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsIDIM_IRLS_f.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsIDIM_IRLS_f.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsIDIM_IRLS_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run IDIM_WLS_f Identification:
|
|
results_IRLS_f = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsIDIM_IRLS_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_IRLS_f', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%% Finished IDIM-IRLS_f %%%%%%%%%%%%%%%%")
|
|
|
|
end % end IDIM_IRLS_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Maximum Likelihood estimator:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % ML estimator
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting ML %%%%%%%%%%%%%%%%%")
|
|
|
|
% Set ML Options:
|
|
optionsML.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsML.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsML.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
optionsML.optimizer = 'lsqnonlin'; % [simplex], [lsqnonlin], [fmin], [pso] or [ga]
|
|
|
|
% Set ML Stop criteria:
|
|
optionsML.stopCrit.tol_1 = functionTolerance/1000;
|
|
optionsML.stopCrit.tol_2 = stepTolerance/1000;
|
|
optionsML.stopCrit.Max_it = iterationMax;
|
|
|
|
% Run ML Identification:
|
|
results_ML = run_ML(robot, benchmarkSettings, experimentDataStruct, optionsML, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_ML', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%%%% Finished ML %%%%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end ML
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Maximum Likelihood estimator with Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % ML_f estimator
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting ML_f %%%%%%%%%%%%%%%%%")
|
|
|
|
% Set ML Options:
|
|
optionsML_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsML_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsML_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
optionsML_f.optimizer = 'lsqnonlin'; % [simplex], [lsqnonlin], [fmin], [pso] or [ga]
|
|
|
|
% Set ML Stop criteria:
|
|
optionsML_f.stopCrit.tol_1 = functionTolerance/1000;
|
|
optionsML_f.stopCrit.tol_2 = stepTolerance/1000;
|
|
optionsML_f.stopCrit.Max_it = iterationMax;
|
|
|
|
% Run ML Identification:
|
|
results_ML_f = run_ML(robot, benchmarkSettings, experimentDataStruct, optionsML_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_ML_f', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%%%% Finished ML_f %%%%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end ML_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Inverse Dynamic Identification Model and Instrumental Variables (IDIM-IV):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % IDIM-IV
|
|
|
|
disp("%%%%%%%%%%%%%% Starting IDIM-IV %%%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_IV Options:
|
|
optionsIDIM_IV.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsIDIM_IV.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsIDIM_IV.solver = 'backslash'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsIDIM_IV.alg = 'WLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsIDIM_IV.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsIDIM_IV.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsIDIM_IV.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Set IDIM_IV Stop criteria:
|
|
optionsIDIM_IV.stopCrit.tol_1 = functionTolerance;
|
|
optionsIDIM_IV.stopCrit.tol_2 = stepTolerance;
|
|
optionsIDIM_IV.stopCrit.Max_it = iterationMax;
|
|
|
|
% Run IDIM_IV Identification:
|
|
results_IV = run_IDIM_IV(robot, benchmarkSettings, experimentDataStruct, optionsIDIM_IV, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_IV', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%% Finished IDIM-IV %%%%%%%%%%%%%%%%")
|
|
|
|
end % end IDIM_IV
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Direct and Inverse Dynamic Identification Model (DIDIM):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % DIDIM
|
|
|
|
disp("%%%%%%%%%%%%%%%%% Starting DIDIM %%%%%%%%%%%%%%%%%")
|
|
|
|
% Set DIDIM Options:
|
|
optionsDIDIM.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsDIDIM.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsDIDIM.solver = 'backslash'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsDIDIM.alg = 'WLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsDIDIM.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsDIDIM.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsDIDIM.filter = 'no'; % [no]: no filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Set DIDIM Stop criteria:
|
|
optionsDIDIM.stopCrit.tol_1 = functionTolerance;
|
|
optionsDIDIM.stopCrit.tol_2 = stepTolerance;
|
|
optionsDIDIM.stopCrit.Max_it = iterationMax;
|
|
|
|
% Run DIDIM Identification:
|
|
results_DIDIM = run_DIDIM(robot, benchmarkSettings, experimentDataStruct, optionsDIDIM, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_DIDIM', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%% Finished DIDIM %%%%%%%%%%%%%%%%%")
|
|
|
|
end % end DIDIM
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Closed-Loop Input-Error (CLIE) with joint torque input:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % CLIE
|
|
|
|
disp("%%%%%%%%%%%%%%%% Starting CLIE %%%%%%%%%%%%%%%%%")
|
|
|
|
% Set CLIE Options:
|
|
optionsCLIE.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsCLIE.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsCLIE.isParamWise = false; % If true, run the parameterwise identification, else identify all the parameters at the same time
|
|
optionsCLIE.paramwiseOptimizer = 'lsqnonlin'; % [simplex] or [lsqnonlin]
|
|
optionsCLIE.globalOptimizer = 'lsqnonlin'; % [simplex], [lsqnonlin], [pso] or [ga]
|
|
optionsCLIE.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Set CLIE Stop criteria:
|
|
optionsCLIE.stopCrit.tol_1 = functionTolerance/1000;
|
|
optionsCLIE.stopCrit.tol_2 = stepTolerance/1000;
|
|
optionsCLIE.stopCrit.Max_it = iterationMax;
|
|
|
|
% Run CLIE Identification:
|
|
results_CLIE = run_CLIE(robot, benchmarkSettings, experimentDataStruct, optionsCLIE, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_CLIE', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%% Finished CLIE %%%%%%%%%%%%%%%%")
|
|
|
|
end % end CLIE
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Closed-Loop Output-Error (CLOE) with joint position input:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % CLOE
|
|
|
|
disp("%%%%%%%%%%%%%%%% Starting CLOE %%%%%%%%%%%%%%%%%")
|
|
|
|
% Set CLOE Options:
|
|
optionsCLOE.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsCLOE.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsCLOE.isParamWise = false; % If true, run the parameterwise identification, else identify all the parameters at the same time
|
|
optionsCLOE.paramwiseOptimizer = 'lsqnonlin'; % [simplex] or [lsqnonlin]
|
|
optionsCLOE.globalOptimizer = 'lsqnonlin'; % [simplex], [lsqnonlin], [pso] or [ga]
|
|
optionsCLOE.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
optionsCLOE.errorFunction = 'Q'; % [Q] for joint position error function, [Qp] for joint velocity error function and [Qpp] for joint acceleration error function
|
|
|
|
% Set CLOE Stop criteria:
|
|
optionsCLOE.stopCrit.tol_1 = functionTolerance/1000;
|
|
optionsCLOE.stopCrit.tol_2 = stepTolerance/1000;
|
|
optionsCLOE.stopCrit.Max_it = iterationMax;
|
|
|
|
% Run CLOE Identification:
|
|
results_CLOE = run_CLOE(robot, benchmarkSettings, experimentDataStruct, optionsCLOE, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_CLOE', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%% Finished CLOE %%%%%%%%%%%%%%%%")
|
|
|
|
end % end CLOE
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Extended Kalman Filter (EKF):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % EKF
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting EKF %%%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set EKF Options:
|
|
optionsEKF.useComputedTorque = false; % If true, run the kalman filter in CLOE mode
|
|
optionsEKF.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsEKF.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsEKF.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsEKF.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsEKF.samplingFactor = 100;
|
|
optionsEKF.type = 'ekf';
|
|
|
|
% Run EKF Identification:
|
|
results_EKF = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsEKF, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_EKF', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished EKF %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end EKF
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Square Root Extended Kalman Filter (SREKF):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % SREKF
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting SREKF %%%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set SREKF Options:
|
|
optionsSREKF.useComputedTorque = false; % If true, run the kalman filter in CLOE mode
|
|
optionsSREKF.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsSREKF.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsSREKF.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsSREKF.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsSREKF.samplingFactor = 100;
|
|
optionsSREKF.type = 'srekf';
|
|
|
|
% Run SREKF Identification:
|
|
results_SREKF = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsSREKF, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_SREKF', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished SREKF %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end SREKF
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Unscented Kalman Filter (UKF):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % UKF
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting UKF %%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set UKF Options:
|
|
optionsUKF.useComputedTorque = false; % If true, run the kalman filter in CLOE mode
|
|
optionsUKF.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsUKF.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsUKF.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsUKF.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsUKF.samplingFactor = 100;
|
|
optionsUKF.type = 'ukf';
|
|
optionsUKF.sigmaCompute = 'svd'; % [svd]: compute matrix square root using svd, [chol]: compute matrix square root using Cholesky factorization.
|
|
|
|
% Run UKF Identification:
|
|
results_UKF = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsUKF, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_UKF', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished UKF %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end UKF
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Square Root Unscented Kalman Filter (SRUKF):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % SRUKF
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting SRUKF %%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set SRUKF Options:
|
|
optionsSRUKF.useComputedTorque = false; % If true, run the kalman filter in CLOE mode
|
|
optionsSRUKF.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsSRUKF.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsSRUKF.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsSRUKF.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsSRUKF.samplingFactor = 100;
|
|
optionsSRUKF.type = 'srukf';
|
|
|
|
% Run SRUKF Identification:
|
|
results_SRUKF = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsSRUKF, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_SRUKF', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished SRUKF %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end SRUKF
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Central Difference Kalman Filter (CDKF):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % CDKF
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting CDKF %%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set CDKF Options:
|
|
optionsCDKF.useComputedTorque = false; % If true, run the kalman filter in CLOE mode
|
|
optionsCDKF.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsCDKF.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsCDKF.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsCDKF.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsCDKF.samplingFactor = 100;
|
|
optionsCDKF.type = 'cdkf';
|
|
optionsCDKF.sigmaCompute = 'svd'; % [svd]: compute matrix square root using svd, [chol]: compute matrix square root using Cholesky factorization.
|
|
|
|
% Run CDKF Identification:
|
|
results_CDKF = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsCDKF, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_CDKF', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished CDKF %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end CDKF
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Square Root Central Difference Kalman Filter (SRCDKF):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % SRCDKF
|
|
disp("%%%%%%%%%%%%%%%%%% Starting SRCDKF %%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set SRCDKF Options:
|
|
optionsSRCDKF.useComputedTorque = false; % If true, run the kalman filter in CLOE mode
|
|
optionsSRCDKF.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsSRCDKF.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsSRCDKF.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsSRCDKF.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsSRCDKF.samplingFactor = 100;
|
|
optionsSRCDKF.type = 'srcdkf';
|
|
|
|
% Run SRCDKF Identification:
|
|
results_SRCDKF = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsSRCDKF, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_SRCDKF', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished SRCDKF %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end SRCDKF
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Particle Filter (PF):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PF
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting PF %%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set PF Options:
|
|
optionsPF.useComputedTorque = false; % If true, run the paticle filter in CLOE mode
|
|
optionsPF.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPF.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsPF.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsPF.samplingFactor = 100; % How often is the debug information displayed
|
|
optionsPF.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsPF.nbParticules = 25*robot.paramVectorSize;
|
|
optionsPF.initialBias = 5e-2;
|
|
optionsPF.type = 'pf';
|
|
optionsPF.resampleThreshold = 1e-2; % Resampling occurs each resampleThreshold iterations
|
|
|
|
% Run PF Identification:
|
|
results_PF = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsPF, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PF', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished PF %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end PF
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Extended Kalman Filter with computed torque (EKF_ct):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % EKF_ct
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting EKF_ct %%%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set EKF_ct Options:
|
|
optionsEKF_ct.useComputedTorque = true; % If true, run the kalman filter in CLOE mode
|
|
optionsEKF_ct.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsEKF_ct.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsEKF_ct.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsEKF_ct.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsEKF_ct.samplingFactor = 100;
|
|
optionsEKF_ct.type = 'ekf';
|
|
|
|
% Run EKF_ct Identification:
|
|
results_EKF_ct = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsEKF_ct, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_EKF_ct', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished EKF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end EKF_ct
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Square Root Extended Kalman Filter with computed torque (SREKF_ct):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % SREKF_ct
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting SREKF_ct %%%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set SREKF_ct Options:
|
|
optionsSREKF_ct.useComputedTorque = true; % If true, run the kalman filter in CLOE mode
|
|
optionsSREKF_ct.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsSREKF_ct.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsSREKF_ct.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsSREKF_ct.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsSREKF_ct.samplingFactor = 100;
|
|
optionsSREKF_ct.type = 'srekf';
|
|
|
|
% Run SREKF_ct Identification:
|
|
results_SREKF_ct = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsSREKF_ct, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_SREKF_ct', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished SREKF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end SREKF_ct
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Unscented Kalman Filter with computed torque (UKF_ct):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % UKF_ct
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting UKF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set UKF_ct Options:
|
|
optionsUKF_ct.useComputedTorque = true; % If true, run the kalman filter in CLOE mode
|
|
optionsUKF_ct.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsUKF_ct.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsUKF_ct.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsUKF_ct.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsUKF_ct.samplingFactor = 100;
|
|
optionsUKF_ct.type = 'ukf';
|
|
optionsUKF_ct.sigmaCompute = 'svd'; % [svd]: compute matrix square root using svd, [chol]: compute matrix square root using Cholesky factorization.
|
|
|
|
% Run UKF_ct Identification:
|
|
results_UKF_ct = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsUKF_ct, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_UKF_ct', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished UKF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end UKF_ct
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Square Root Unscented Kalman Filter with computed torque (SRUKF_ct):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % SRUKF_ct
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting SRUKF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set SRUKF_ct Options:
|
|
optionsSRUKF_ct.useComputedTorque = true; % If true, run the kalman filter in CLOE mode
|
|
optionsSRUKF_ct.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsSRUKF_ct.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsSRUKF_ct.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsSRUKF_ct.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsSRUKF_ct.samplingFactor = 100;
|
|
optionsSRUKF_ct.type = 'srukf';
|
|
|
|
% Run SRUKF_ct Identification:
|
|
results_SRUKF_ct = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsSRUKF_ct, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_SRUKF_ct', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished SRUKF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end SRUKF_ct
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Central Difference Kalman Filter with computed torque (CDKF_ct):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % CDKF_ct
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting CDKF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set CDKF_ct Options:
|
|
optionsCDKF_ct.useComputedTorque = true; % If true, run the kalman filter in CLOE mode
|
|
optionsCDKF_ct.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsCDKF_ct.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsCDKF_ct.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsCDKF_ct.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsCDKF_ct.samplingFactor = 100;
|
|
optionsCDKF_ct.type = 'cdkf';
|
|
optionsCDKF_ct.sigmaCompute = 'svd'; % [svd]: compute matrix square root using svd, [chol]: compute matrix square root using Cholesky factorization.
|
|
|
|
% Run CDKF_ct Identification:
|
|
results_CDKF_ct = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsCDKF_ct, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_CDKF_ct', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished CDKF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end CDKF_ct
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% SR Central Difference Kalman Filter with computed torque (SRCDKF_ct):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % SRCDKF_ct
|
|
disp("%%%%%%%%%%%%%%%%%% Starting SRCDKF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set SRCDKF_ct Options:
|
|
optionsSRCDKF_ct.useComputedTorque = true; % If true, run the kalman filter in CLOE mode
|
|
optionsSRCDKF_ct.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsSRCDKF_ct.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsSRCDKF_ct.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsSRCDKF_ct.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsSRCDKF_ct.samplingFactor = 100;
|
|
optionsSRCDKF_ct.type = 'srcdkf';
|
|
|
|
% Run SRCDKF_ct Identification:
|
|
results_SRCDKF_ct = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsSRCDKF_ct, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_SRCDKF_ct', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished SRCDKF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end SRCDKF
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Particle Filter with computed torque (PF_ct):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PF_ct
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Starting PF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
% Set PF_ct Options:
|
|
optionsPF_ct.useComputedTorque = true; % If true, run the paticle filter in CLOE mode
|
|
optionsPF_ct.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPF_ct.debug = debugFlag; % If true, the solver plots the parameter evolution.
|
|
optionsPF_ct.getIterationData = false; % If true, saves the augmented state estimate and its covariance matrix each samplingFactor iteration. Produces MASSIVE data files !
|
|
optionsPF_ct.samplingFactor = 100; % How often is the debug information displayed
|
|
optionsPF_ct.anneal = true; % If true, anneals the process noise covariance of the parameters with time.
|
|
optionsPF_ct.nbParticules = 25*robot.paramVectorSize;
|
|
optionsPF_ct.initialBias = 5e-2;
|
|
optionsPF_ct.type = 'pf';
|
|
optionsPF_ct.resampleThreshold = 1e-2; % Resampling occurs each resampleThreshold iterations
|
|
|
|
% Run PF_ct Identification:
|
|
results_PF_ct = run_Kalman(robot, benchmarkSettings, experimentDataStruct, optionsPF_ct, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PF_ct', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%%%%% Finished PF_ct %%%%%%%%%%%%%%%%%%")
|
|
|
|
end % end PF_ct
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Inverse Dynamic Identification Model and Adaline Neural Network:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % ANN
|
|
|
|
disp("%%%%%%%%% Starting Adaline Neural Network %%%%%%%%%")
|
|
|
|
% Set ANN Options:
|
|
optionsANN.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsANN.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsANN.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
optionsANN.neuronet = 'stochastic_grad'; % [toolbox]: Use matlab neural network toolbox, [grad]: Use custom gradient descent, [stochastic_grad]: Use custom stochastic gradient descent.
|
|
optionsANN.learningRate = 1;
|
|
|
|
% Set ANN Stop criteria:
|
|
optionsANN.stopCrit.tol = 1e-5;
|
|
optionsANN.stopCrit.Max_training_epochs = 1e4;
|
|
|
|
% Run ANN Identification:
|
|
results_ANN = run_ANN(robot, benchmarkSettings, experimentDataStruct, optionsANN, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_ANN', '-v7.3')
|
|
|
|
disp("%%%%%%%%% Finished Adaline Neural Network %%%%%%%%%")
|
|
|
|
end % end ANN
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Inverse Dynamic Identification Model and Filtered Adaline Neural Network:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % ANN_f
|
|
|
|
disp("%%%%%%%%% Starting Adaline Neural Network %%%%%%%%%")
|
|
|
|
% Set ANN Options:
|
|
optionsANN_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsANN_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsANN_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
optionsANN_f.neuronet = 'stochastic_grad'; % [toolbox]: Use matlab neural network toolbox, [grad]: Use custom gradient descent, [stochastic_grad]: Use custom stochastic gradient descent.
|
|
optionsANN_f.learningRate = 1;
|
|
|
|
% Set ANN Stop criteria:
|
|
optionsANN_f.stopCrit.tol = 1e-5;
|
|
optionsANN_f.stopCrit.Max_training_epochs = 1e4;
|
|
|
|
% Run ANN Identification:
|
|
results_ANN_f = run_ANN(robot, benchmarkSettings, experimentDataStruct, optionsANN_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_ANN_f', '-v7.3')
|
|
|
|
disp("%%%%%%%%% Finished Adaline Neural Network %%%%%%%%%")
|
|
|
|
end % end ANN_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Inverse Dynamic Identification Model and Hopfield Neural Network:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % HTRNN
|
|
|
|
disp("%%%%%%%% Starting Hopfield Neural Network %%%%%%%%")
|
|
|
|
% Set HTRNN Options:
|
|
optionsHTRNN.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsHTRNN.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsHTRNN.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
if strcmp(robotName,'RV2SQ')
|
|
optionsHTRNN.learningRate = 1e-6; % real TX40: 3e-8; % 1e-7 for TX40 1e-6 for RV2SQ;
|
|
elseif strcmp(robotName,'TX40')
|
|
optionsHTRNN.learningRate = 3e-8;
|
|
else
|
|
optionsHTRNN.learningRate = 1e-7;
|
|
end
|
|
|
|
% Set HTRNN Stop criteria:
|
|
optionsHTRNN.stopCrit.tol = 1e-5;
|
|
optionsHTRNN.stopCrit.Max_training_epochs = 1e4;
|
|
|
|
% Run HTRNN Identification:
|
|
results_HTRNN = run_HTRNN(robot, benchmarkSettings, experimentDataStruct, optionsHTRNN, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_HTRNN', '-v7.3')
|
|
|
|
disp("%%%%%%%% Finished Hopfield Neural Network %%%%%%%%")
|
|
|
|
end % end HTRNN
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Inverse Dynamic Identification Model and Filtered Hopfield Neural Network:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % HTRNN_f
|
|
|
|
disp("%%%%%%%% Starting Hopfield Neural Network %%%%%%%%")
|
|
|
|
% Set HTRNN Options:
|
|
optionsHTRNN_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsHTRNN_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsHTRNN_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
if strcmp(robotName,'RV2SQ')
|
|
optionsHTRNN_f.learningRate = 1e-6; % real TX40: 3e-8; % 1e-7 for TX40 1e-6 for RV2SQ;
|
|
elseif strcmp(robotName,'TX40')
|
|
optionsHTRNN_f.learningRate = 3e-8;
|
|
else
|
|
optionsHTRNN_f.learningRate = 1e-7;
|
|
end
|
|
|
|
% Set HTRNN Stop criteria:
|
|
optionsHTRNN_f.stopCrit.tol = 1e-5;
|
|
optionsHTRNN_f.stopCrit.Max_training_epochs = 1e4;
|
|
|
|
% Run HTRNN Identification:
|
|
results_HTRNN_f = run_HTRNN(robot, benchmarkSettings, experimentDataStruct, optionsHTRNN_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_HTRNN_f', '-v7.3')
|
|
|
|
disp("%%%%%%%% Finished Hopfield Neural Network %%%%%%%%")
|
|
|
|
end % end HTRNN
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-OLS (PC-IDIM-OLS):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_OLS
|
|
|
|
disp("%%%%%%%%%%%%% Starting PC-IDIM-OLS %%%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_OLS Options:
|
|
optionsPC_IDIM_OLS.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_OLS.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_OLS.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_OLS.alg = 'PC-OLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_OLS.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_OLS.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_OLS.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run IDIM_OLS Identification:
|
|
results_PC_OLS = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_OLS, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_OLS', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%% Finished PC-IDIM-OLS %%%%%%%%%%%%%%")
|
|
|
|
end % end PC_IDIM_OLS
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-OLS (PC-IDIM-OLS) with Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_OLS with 2-pass-Butterworth filter
|
|
|
|
disp("%%%%%%%%%%% Starting PC-IDIM-OLS-f %%%%%%%%%%%%")
|
|
|
|
% Set PC_IDIM_OLS_f Options:
|
|
optionsPC_IDIM_OLS_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_OLS_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_OLS_f.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_OLS_f.alg = 'PC-OLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_OLS_f.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_OLS_f.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_OLS_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run PC_ IDIM_OLS_f Identification:
|
|
results_PC_OLS_f = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_OLS_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_OLS_f', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%% Finished PC-IDIM-OLS-f %%%%%%%%%%%%%%")
|
|
|
|
end % end PC_IDIM_OLS_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-WLS (PC-IDIM-WLS):
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_WLS
|
|
|
|
disp("%%%%%%%%%%%%% Starting PC-IDIM-WLS %%%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_WLS Options:
|
|
optionsPC_IDIM_WLS.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_WLS.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_WLS.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_WLS.alg = 'PC-WLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_WLS.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_WLS.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_WLS.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run IDIM_WLS Identification:
|
|
results_PC_WLS = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_WLS, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_WLS', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%% Finished PC-IDIM-WLS %%%%%%%%%%%%%%")
|
|
|
|
end % end PC_IDIM_WLS
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-WLS (PC-IDIM-WLS) with Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_WLS with 2-pass-Butterworth filter
|
|
|
|
disp("%%%%%%%%%%% Starting PC-IDIM-WLS-f %%%%%%%%%%%%")
|
|
|
|
% Set PC_IDIM_WLS_f Options:
|
|
optionsPC_IDIM_WLS_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_WLS_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_WLS_f.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_WLS_f.alg = 'PC-WLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_WLS_f.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_WLS_f.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_WLS_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run PC_IDIM_WLS_f Identification:
|
|
results_PC_WLS_f = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_WLS_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_WLS_f', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%% Finished PC-IDIM-WLS-f %%%%%%%%%%%%%%")
|
|
|
|
end % end PC_IDIM_WLS_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-IRLS (PC-IDIM-IRLS)
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_IRLS
|
|
|
|
disp("%%%%%%%%%%%%% Starting PC-IDIM-IRLS %%%%%%%%%%%%%%")
|
|
|
|
% Set IDIM_IRLS Options:
|
|
optionsPC_IDIM_IRLS.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_IRLS.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_IRLS.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_IRLS.alg = 'PC-IRLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_IRLS.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_IRLS.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_IRLS.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run PC_IDIM_IRLS Identification:
|
|
results_PC_IRLS = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_IRLS, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_IRLS', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%% Finished PC-IDIM-IRLS %%%%%%%%%%%%%%")
|
|
|
|
end % end PC_IDIM_IRLS
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-IRLS (PC-IDIM-IRLS) with Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_IRLS with 2-pass-Butterworth filter
|
|
|
|
disp("%%%%%%%%%%% Starting PC-IDIM-IRLS-f %%%%%%%%%%%%")
|
|
|
|
% Set PC_IDIM_IRLS_f Options:
|
|
optionsPC_IDIM_IRLS_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_IRLS_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_IRLS_f.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_IRLS_f.alg = 'PC-IRLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_IRLS_f.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_IRLS_f.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_IRLS_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run PC_IDIM_IRLS_f Identification:
|
|
results_PC_IRLS_f = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_IRLS_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_IRLS_f', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%% Finished PC-IDIM-IRLS-f %%%%%%%%%%%%%%")
|
|
|
|
end % end PC_IDIM_IRLS_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-OLS with Euclidean Regularization:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_OLS_Euclidean
|
|
|
|
disp("%%%%%%% Starting PC-IDIM-OLS-Euclidean %%%%%%%")
|
|
|
|
% Set PC_IDIM_OLS_Euclidean Options:
|
|
optionsPC_IDIM_OLS_Euclidean.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_OLS_Euclidean.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_OLS_Euclidean.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_OLS_Euclidean.alg = 'PC-OLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_OLS_Euclidean.regularizerType = 'Euclidean'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_OLS_Euclidean.gammaReg = gammaReg; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_OLS_Euclidean.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run PC_IDIM_OLS_Euclidean Identification:
|
|
results_PC_OLS_Euclidean = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_OLS_Euclidean, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_OLS_Euclidean', '-v7.3')
|
|
|
|
disp("%%%%%%% Finished PC-IDIM-OLS-Euclidean %%%%%%%")
|
|
|
|
end % end PC_IDIM_OLS_Euclidean
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-OLS with Euclidean Regularization and Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 %PC_IDIM_OLS_Euclidean with 2-pass-Butterworth filter
|
|
|
|
disp("%%%%% Starting PC-IDIM-OLS-Euclidean-f %%%%%")
|
|
|
|
% Set PC_IDIM_OLS_Euclidean_f Options:
|
|
optionsPC_IDIM_OLS_Euclidean_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_OLS_Euclidean_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_OLS_Euclidean_f.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_OLS_Euclidean_f.alg = 'PC-OLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_OLS_Euclidean_f.regularizerType = 'Euclidean'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_OLS_Euclidean_f.gammaReg = gammaReg; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_OLS_Euclidean_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run PC_IDIM_OLS_Euclidean_f Identification:
|
|
results_PC_OLS_Euclidean_f = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_OLS_Euclidean_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_OLS_Euclidean_f', '-v7.3')
|
|
|
|
disp("%%%%%% Finished PC-IDIM-OLS-Euclidean-f %%%%%%%")
|
|
|
|
end % end PC_IDIM_OLS_Euclidean_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-OLS with Entropic Regularization:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_OLS_Entropic
|
|
|
|
disp("%%%%%%%%% Starting PC-IDIM-OLS-Entropic %%%%%%%%%")
|
|
|
|
% Set PC_IDIM_OLS_Entropic Options:
|
|
optionsPC_IDIM_OLS_Entropic.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_OLS_Entropic.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_OLS_Entropic.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_OLS_Entropic.alg = 'PC-OLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_OLS_Entropic.regularizerType = 'Entropic'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_OLS_Entropic.gammaReg = gammaReg; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_OLS_Entropic.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run PC_IDIM_OLS_Entropic Identification:
|
|
results_PC_OLS_Entropic = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_OLS_Entropic, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_OLS_Entropic', '-v7.3')
|
|
|
|
disp("%%%%%%%%% Finished PC-IDIM-OLS-Entropic %%%%%%%%%")
|
|
|
|
end % end PC_IDIM_OLS_Entropic
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-OLS with Entropic Regularization and Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_OLS_Entropic with 2-pass-Butterworth filter
|
|
|
|
disp("%%%%%%% Starting PC-IDIM-OLS-Entropic-f %%%%%%%")
|
|
|
|
% Set PC_IDIM_OLS_Entropic_f Options:
|
|
optionsPC_IDIM_OLS_Entropic_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_OLS_Entropic_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_OLS_Entropic_f.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_OLS_Entropic_f.alg = 'PC-OLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_OLS_Entropic_f.regularizerType = 'Entropic'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_OLS_Entropic_f.gammaReg = gammaReg; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_OLS_Entropic_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run PC_IDIM_OLS_Entropic_f Identification:
|
|
results_PC_OLS_Entropic_f = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_OLS_Entropic_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_OLS_Entropic_f', '-v7.3')
|
|
|
|
disp("%%%%%%%% Finished IDIM-PC-OLS-Entropic-f %%%%%%%%%")
|
|
|
|
end % end PC_IDIM_OLS_Entropic_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-OLS with ConstPullback Regularization:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_OLS_ConstPullback
|
|
|
|
disp("%%%%%%% Starting PC-IDIM-OLS-ConstPullback %%%%%%%")
|
|
|
|
% Set PC_IDIM_OLS_ConstPullback Options:
|
|
optionsPC_IDIM_OLS_ConstPullback.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_OLS_ConstPullback.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_OLS_ConstPullback.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_OLS_ConstPullback.alg = 'PC-OLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_OLS_ConstPullback.regularizerType = 'ConstPullback'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_OLS_ConstPullback.gammaReg = gammaReg; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_OLS_ConstPullback.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run PC_IDIM_OLS_ConstPullback Identification:
|
|
results_PC_OLS_ConstPullback = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_OLS_ConstPullback, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_OLS_ConstPullback', '-v7.3')
|
|
|
|
disp("%%%%%%% Finished PC-IDIM-OLS-ConstPullback %%%%%%%")
|
|
|
|
end % end PC_IDIM_OLS_ConstPullback
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Physically Consistent IDIM-OLS with ConstPullback Regularization and Zero-shift Butterworth filter:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM_OLS_ConstPullback with 2-pass-Butterworth filter
|
|
|
|
disp("%%%%% Starting PC-IDIM-OLS-ConstPullback-f %%%%%")
|
|
|
|
% Set PC_IDIM_OLS_ConstPullback_f Options:
|
|
optionsPC_IDIM_OLS_ConstPullback_f.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_OLS_ConstPullback_f.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_OLS_ConstPullback_f.solver ='mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_OLS_ConstPullback_f.alg = 'PC-OLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_OLS_ConstPullback_f.regularizerType = 'ConstPullback'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_OLS_ConstPullback_f.gammaReg = gammaReg; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_OLS_ConstPullback_f.filter = 'butterworth'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Run PC_IDIM_OLS_ConstPullback_f Identification:
|
|
results_PC_OLS_ConstPullback_f = run_IDIM_LS(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_OLS_ConstPullback_f, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_OLS_ConstPullback_f', '-v7.3')
|
|
|
|
disp("%%%%%% Finished PC-IDIM-OLS-ConstPullback-f %%%%%%%")
|
|
|
|
end % end PC_IDIM_OLS_ConstPullback_f
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Inverse Dynamic Identification Model and Instrumental Variables (IDIM-IV) + LMI:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_IDIM-IV
|
|
|
|
disp("%%%%%%%%%%%%%% Starting PC-IDIM-IV %%%%%%%%%%%%%%")
|
|
|
|
% Set PC_IDIM_IV Options:
|
|
optionsPC_IDIM_IV.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_IDIM_IV.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_IDIM_IV.solver = 'mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_IDIM_IV.alg = 'PC-WLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_IDIM_IV.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_IDIM_IV.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_IDIM_IV.filter = 'no'; % [no]: no filter, [lowpass]: low pass filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Set PC_IDIM_IV Stop criteria:
|
|
optionsPC_IDIM_IV.stopCrit.tol_1 = functionTolerance;
|
|
optionsPC_IDIM_IV.stopCrit.tol_2 = stepTolerance;
|
|
optionsPC_IDIM_IV.stopCrit.Max_it = iterationMax;
|
|
|
|
% Run PC_IDIM_IV Identification:
|
|
results_PC_IV = run_IDIM_IV(robot, benchmarkSettings, experimentDataStruct, optionsPC_IDIM_IV, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_IV', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%% Finished PC-IDIM-IV %%%%%%%%%%%%%%")
|
|
|
|
end % end PC_IDIM_IV
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%% Direct and Inverse Dynamic Identification Model (DIDIM) + LMI:
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
if identificationMethods.isActivated(methodIndex)==1 % PC_DIDIM
|
|
|
|
disp("%%%%%%%%%%%%%%% Starting PC-DIDIM %%%%%%%%%%%%%%%")
|
|
|
|
% Set PC_DIDIM Options:
|
|
optionsPC_DIDIM.verbose = verboseFlag; % If true, the solver communicates the identification error at each step.
|
|
optionsPC_DIDIM.debug = debugFlag; % If true, the solver plots the robot trajectory using the computed parameters.
|
|
optionsPC_DIDIM.solver = 'mosek'; % [backslash]: use the matlab optimized function (x=A\b), [pinv]: use the matlab pseudoinverse function, [cvx]: use the cvx solver, [mosek]: use the cvx+mosek solver
|
|
optionsPC_DIDIM.alg = 'PC-WLS'; % [OLS]: Ordinary LS, [WLS]: Weighted LS, [TLS]: Total LS, [IRLS]: Iteratively Reweighted LS, [PC-OLS]: Physically Consistent OLS, [PC-WLS]: Physically Consistent WLS, [PC-IRLS]: Physically Consistent IRLS
|
|
optionsPC_DIDIM.regularizerType = 'no'; % [no]: no regularizer, [Euclidean]: Euclidian regularization, [Entropic]: Entropic divergence regularization, [ConstPullback]: Constant Pullback distance regularization
|
|
optionsPC_DIDIM.gammaReg = 0; % Regularization factor, to be multiplied to the regularizer.
|
|
optionsPC_DIDIM.filter = 'no'; % [no]: no filter, [butterworth]: zero-shift butterworth filter
|
|
|
|
% Set PC_DIDIM Stop criteria:
|
|
optionsPC_DIDIM.stopCrit.tol_1 = functionTolerance;
|
|
optionsPC_DIDIM.stopCrit.tol_2 = stepTolerance;
|
|
optionsPC_DIDIM.stopCrit.Max_it = iterationMax;
|
|
|
|
% Run PC_DIDIM Identification:
|
|
results_PC_DIDIM = run_DIDIM(robot, benchmarkSettings, experimentDataStruct, optionsPC_DIDIM, progressBar);
|
|
|
|
% Save the Results:
|
|
if ~exist(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate), 'dir')
|
|
mkdir(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d', robot.name,noiseLevel,benchmarkSettings.decimRate));
|
|
end
|
|
save(sprintf('Benchmark/Robot_Identification_Results/%s_%s/decim_%d/results_%s_%s.mat',robot.name,noiseLevel,benchmarkSettings.decimRate,identificationMethods.algName{methodIndex},robot.name), 'results_PC_DIDIM', '-v7.3')
|
|
|
|
disp("%%%%%%%%%%%%%%% Finished PC-DIDIM %%%%%%%%%%%%%%%")
|
|
|
|
end % end PC_DIDIM
|
|
|
|
methodIndex=methodIndex+1;
|
|
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
|
|
if benchmarkSettings.displayProgression == true
|
|
close(progressBar); % Close progress bar object
|
|
end
|
|
|
|
disp("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
|
|
disp("%%%%%%%%%%%% Starting Post Processing %%%%%%%%%%%%")
|
|
disp("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
|
|
|
|
if runPostProcessing == true
|
|
postProcessing(robot, benchmarkSettings, experimentDataStruct);
|
|
end
|
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
|
|
else
|
|
disp("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
|
|
disp("%%%%%% No identification method selected... %%%%%%")
|
|
disp("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
|
|
end
|
|
|
|
disp("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
|
|
disp("%%%%%%%%%%%%% Computations Completed %%%%%%%%%%%%%")
|
|
disp("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")
|