76 lines
2.6 KiB
Matlab
76 lines
2.6 KiB
Matlab
function [Beta_HTRNN, it] = HTRNN_identification(robot, experimentDataStruct, expNb, benchmarkSettings, Beta_0, optionsHTRNN)
|
|
|
|
% Authors: Quentin Leboutet, Julien Roux, Alexandre Janot and Gordon Cheng
|
|
%
|
|
% Parameter identification using the Hopfield Neural Network method.
|
|
|
|
%% Data Decimation and Filtering:
|
|
|
|
[~, ~, Qpp, Qp, Q, ~, Tau_decim, ~, ~, ~, optionsHTRNN] = getFilteredData(robot, benchmarkSettings, experimentDataStruct, optionsHTRNN, expNb, 'HTRNN');
|
|
|
|
%% Populating the Observation Matrix W and Torque Vector:
|
|
|
|
if strcmp(benchmarkSettings.codeImplementation,'optim')
|
|
% Compute the sampled vector:
|
|
Y_tau = torqueVector_mex(Tau_decim);
|
|
% Compute the observation matrix:
|
|
W_ndec = observationMatrix_mex(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q, Qp, Qpp);
|
|
elseif strcmp(benchmarkSettings.codeImplementation,'classic')
|
|
% Compute the sampled vector:
|
|
Y_tau = torqueVector(Tau_decim);
|
|
% Compute the observation matrix:
|
|
W_ndec = observationMatrix(robot.name, robot.paramVectorSize, robot.numericalParameters.Geometry, robot.numericalParameters.Gravity, Q, Qp, Qpp);
|
|
else
|
|
error("HTRNN: unknown option");
|
|
end
|
|
|
|
% Decimation filter
|
|
W=zeros(robot.nbDOF*optionsHTRNN.nbSampleDecim, robot.paramVectorSize); % Observation matrix
|
|
for col=1:robot.paramVectorSize
|
|
for i = 1:robot.nbDOF
|
|
W(i:robot.nbDOF:end,col) = decimate(W_ndec(i:robot.nbDOF:end,col),benchmarkSettings.decimRate);
|
|
end
|
|
end
|
|
|
|
%% Defining the Hopfield Neural Network:
|
|
|
|
% Initialization:
|
|
|
|
Beta = Beta_0;
|
|
|
|
% Calculate weight and bias matrices:
|
|
alpha = max(abs(1.5*Beta_0));
|
|
beta = 5;
|
|
Betas = zeros(robot.paramVectorSize, optionsHTRNN.nbSampleDecim);
|
|
U = Beta_0;
|
|
Beta_dot = ones(robot.paramVectorSize,1);
|
|
it=1;
|
|
Weight = -W'*W;
|
|
Bias = - W'*Y_tau;
|
|
|
|
% Simulation of Hopfield-Tank neural dyanmics:
|
|
while norm(Beta_dot)>optionsHTRNN.stopCrit.tol && it < optionsHTRNN.stopCrit.Max_training_epochs
|
|
Beta_old = Beta;
|
|
[U, Beta] = HopfieldNeuronDynamics(U, Weight, Bias, alpha, beta, optionsHTRNN.learningRate);
|
|
Beta_dot = (Beta_old-Beta)/optionsHTRNN.learningRate;
|
|
Err = norm(Beta-benchmarkSettings.Beta_obj);
|
|
Betas(:,it)=Beta;
|
|
it = it +1;
|
|
end
|
|
|
|
Beta_HTRNN = Beta;
|
|
|
|
%% Debug plot:
|
|
|
|
debugPlot(robot, benchmarkSettings, experimentDataStruct, Beta_0, Beta_HTRNN, optionsHTRNN, expNb, 'HTRNN', Betas, it);
|
|
|
|
end
|
|
|
|
function [U, Beta] = HopfieldNeuronDynamics(U, Weight, Bias, alpha, beta, dt)
|
|
|
|
Beta = alpha*tanh(U/beta); % Compute the output of the neurons
|
|
Udot = Weight*Beta - Bias; % Compute the change of neuron input at next iteration
|
|
U = U + Udot*dt;
|
|
|
|
end
|