Robotics
/
BIRDy
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
BIRDy/run_identification_benchmark.m

1802 lines
125 KiB
Mathematica
Raw Normal View History

"Added code"
2021-04-29 09:42:38 +00:00
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% 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 = false; % If true, recomputes the robot symbolic model.
regenerateTrajectory = false; % If true, launch the generation routine for new excitation trajectory.
regenerateData = false; % 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 = false; % If true, triggers visualization of interpolated trajectories with the different available methods.
% MEX files compilation:
recompileMexFiles = false; % 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 1];
% 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 = false; % If true, display progression GUI for each identification algorithm.
filter.visualizeFilter = false; % Filter tuning
displayTrajectory = false; % If true, display the robot joint trajectory.
displayControlPerformance = false; % If true, display the robot tracking performance on the generated trajectory.
getKinematicSymbolicExpressions = false; % 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
1,... % OLS_f
1,... % WLS
1,... % WLS_f
1,... % TLS
1,... % TLS_f
1,... % IRLS
1,... % IRLS_f
1,... % ML
1,... % ML_f
1,... % IV
1,... % DIDIM
1,... % CLIE
1,... % CLOE
1,... % EKF
1,... % SREKF
1,... % UKF
1,... % SRUKF
1,... % CDKF
1,... % SRCDKF
1,... % PF
1,... % ANN
1,... % ANN_f
1,... % HTRNN
1,... % HTRNN_f
1,... % PC_OLS
1,... % PC_OLS_f
1,... % PC_WLS
1,... % PC_WLS_f
1,... % PC_IRLS
1,... % PC_IRLS_f
1,... % PC_OLS_Euclidian
1,... % PC_OLS_Euclidian_f
1,... % PC_OLS_Entropic
1,... % PC_OLS_Entropic_f
1,... % PC_OLS_ConstPullback
1,... % PC_OLS_ConstPullback_f
1,... % PC_IV
1]; % PC_DIDIM
identificationMethods.showResults = ...
[1,...% OLS
1,... % OLS_f
1,... % WLS
1,... % WLS_f
1,... % TLS
1,... % TLS_f
1,... % IRLS
1,... % IRLS_f
1,... % ML
1,... % ML_f
1,... % IV
1,... % DIDIM
1,... % CLIE
1,... % CLOE
1,... % EKF
1,... % SREKF
1,... % UKF
1,... % SRUKF
1,... % CDKF
1,... % SRCDKF
1,... % PF
1,... % ANN
1,... % ANN_f
1,... % HTRNN
1,... % HTRNN_f
1,... % PC_OLS
1,... % PC_OLS_f
1,... % PC_WLS
1,... % PC_WLS_f
1,... % PC_IRLS
1,... % PC_IRLS_f
1,... % PC_OLS_Euclidian
1,... % PC_OLS_Euclidian_f
1,... % PC_OLS_Entropic
1,... % PC_OLS_Entropic_f
1,... % PC_OLS_ConstPullback
1,... % PC_OLS_ConstPullback_f
1,... % PC_IV
1]; % 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("%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%")