SysIDAlgFMParamOnly.m

%% IRLS Algorithm for only motor, friction and offset parameters

% Inputs:

% n = number of joints, double
% na = number of actuated joints, double
% nu = number of unactuated joints, double
% na/nu_idx = vector of indices of actuated/unactuated joints, na/nu vector
% m = number of data points, double
% W_ip = Observation matrix for inertial parameters, n*m x 10*n matrix 
% T = torque vector over m data points, n*m vector
% data = matrix containing pos., vel. and accel. for all joints over m data points, n x 3*m matrix
% J = constraint jacobian function, nc x n matrix (nc = number of constraints)
% lb = lower bound on paramaters, p_full vector
% ub = upper bound on parameters p_full vector
% lba = lower bound on friction exponents, n vector
% uba = upper bound on friction exponents, n vector
% X_ip = inertial parameters, 10*n vector
% X0_1 = initial condition for parameters, p_full vector

% Algorithm options:

% k = error filter value for weights, double
% tol = stopping criterion for while loops, double << 1
% MS1 = number of multi searchs for first optimization, double
% MS2 = number of multi searchs for second optimization, double
% SearchAlpha = include optimization over friction exponents, bool
% includeOffset = include offset as parameter, bool
% includeConstraints = systen has constraints, bool
% constraintVariant = choose method of constraint projection matrix, double (1 or 2)

% Output:

% X = vector of all parameters from optimization, p_full vector 
% Wfull = observation matrix for inertial, motor, friction and offset parameters, n*m x p_full matrix
% alphanew = new friction exponents from optimization, n vector

% Author: Daniel Haugk, 2024, University of Michigan

function [X, Wfull, alphanew] = SysIDAlgFMParamOnly(variables,AlgOptions,na_idx,nu_idx,J,lb,ub,lba,uba,X_ip,T,data,dataFull,W_ip,X0_1,options)

% initialize variables
n = variables{1};
m = variables{2};

% initalize algorithm options
k = AlgOptions{1};
tol = AlgOptions{2};
MS1 = AlgOptions{3};
MS2 = AlgOptions{4};
SearchAlpha = AlgOptions{8};
includeOffset = AlgOptions{9};
includeConstraints = AlgOptions{11};
constraintVariant = AlgOptions{12};

% check if constraints are included
if includeConstraints

    % get number of actuated links
    na = length(na_idx);

    if constraintVariant == 1
    % general case for all kinds of constraints
    
        % get joint dimension of regressor matrix
        ncv = n;

        for i = 1:m

            % compute constraint projection matrix
            J_temp = J(dataFull(:,i));
            K = eye(n) - J_temp(:,sort([na_idx nu_idx])).'*pinv(J_temp(:,sort([na_idx nu_idx])).');

            % compute constrained observation matrix and joint torques
            T((i-1)*n+1:i*n) = K*T((i-1)*n+1:i*n);
            W_ip((i-1)*n+1:i*n,:) = K*W_ip((i-1)*n+1:i*n,:);
        end

    elseif constraintVariant == 2
    % special case for fully actuated systems

        % get joint dimension of regressor matrix
        ncv = na;

        W_ip_temp = [];

        for i = 1:m

            % compute constraint projection matrix
            J_temp = J(dataFull(:,i));
            P = [eye(na); -J_temp(:,nu_idx)\J_temp(:,na_idx)];

            % compute constrained observation matrix on actuated joint level
            W_ip_ = P.'*W_ip((i-1)*n+1:i*n,:);
            W_ip_temp = [W_ip_temp;W_ip_];
        end

        % assign temporary observation matrix variable to observation matrix
        W_ip = W_ip_temp;

    end

else
    
    % set variables for constrained system to empty
    na = [];
    na_idx = [];
    nu_idx = [];
    J = [];

    % set number of joints in regressor matrix to all joints
    ncv = n;

end

% compute T_hat , i.e. torque vector - (inertial parameter observation matrix * inertial parameters)
T_hat = T-W_ip*X_ip;

% compute number of all parameters with (b) and without (p) regrouping
if includeOffset
    p_fm = 4*n;
else
    p_fm = 3*n;
end

% get bounds for first optimization problem
lowerB1 = lb;
upperB1 = ub;

% get bounds for second optimization problem
lowerB2 = [lba;lb(n+1:end)];
upperB2 = [uba;ub(n+1:end)];

% check if optimization over friction exponent is included
if SearchAlpha
    [X, Wfm, alphanew] = AlgAlphaFMOnly(T_hat,tol,p_fm,n,ncv,na,na_idx,nu_idx,m,k,MS1,MS2,data,dataFull,J,lowerB1,upperB1,lowerB2,upperB2,X0_1,includeOffset,includeConstraints,constraintVariant,options);
else
    [X, Wfm] = AlgNoAlphaFMOnly(T_hat,tol,p_fm,n,ncv,na,na_idx,nu_idx,m,k,MS1,data,dataFull,J,lowerB1,upperB1,X0_1,includeOffset,includeConstraints,constraintVariant,options);
    alphanew = ones(n,1);
end

% put together full observation matrix
Wfull = [W_ip Wfm];

end


%% Algorithm functions

% Algorithm including optimization over friction exponent
function [X, Wfm, alphanew] = AlgAlphaFMOnly(T_hat,tol,p_fm,n,ncv,na,na_idx,nu_idx,m,k,MS1,MS2,data,dataFull,J,lowerB1,upperB1,lowerB2,upperB2,X0_1,includeOffset,includeConstraints,constraintVariant,options)

% Initialize friction exponents
alphanew = ones(n,1);
alphaold = zeros(n,1);

    % start while loop for friction exponents alpha
    while(norm(alphaold-alphanew)>tol)
        
        % initialize weights
        Dnew = ones(ncv*m,1);
        Dold = zeros(ncv*m,1);
        
        % build extended weight matrix
        for i = 1:p_fm
            De(:,i) = Dnew;
        end       

        % compute observation matrix for motor and friction dynamics 
        [Wfm,~,Wf] = Wextension(alphanew,n,na,na_idx,nu_idx,J,m,data,dataFull,includeOffset,includeConstraints,constraintVariant);

        % start while loop for weights
        while(norm(Dold-Dnew)>tol)

            % initialize covariance matrix as identity matrix
            Onew = eye(ncv);
            Oold = zeros(ncv);

            % initialize root of covariance matrix
            O_sqrt = real(sqrtm(Onew));

            % start while loop for covariance matrix
            while(norm(Oold-Onew)>tol)   

                % compute normalized observation matrix and torque vector
                for i = 1:m
                    Ts((i-1)*ncv+1:i*ncv,:) = O_sqrt\T_hat((i-1)*ncv+1:i*ncv);
                    Wfms((i-1)*ncv+1:i*ncv,:) = O_sqrt\Wfm((i-1)*ncv+1:i*ncv,:);
                    Wfs((i-1)*ncv+1:i*ncv,:) = O_sqrt\Wf((i-1)*ncv+1:i*ncv,:);
                end

                % compute weighted normalized observation matrix and torque vector
                Th = Dnew.*Ts;
                Wfmh = De.*Wfms;
                Wfh = De(:,1:p_fm-n).*Wfs;

                % initialize least squares function
                fun1 = @(X1) optimFun(X1, Wfmh, Th);

                % formulate least squares problem for all parameters
                problem1 = createOptimProblem('fmincon','x0',X0_1,'objective',fun1,'lb',lowerB1,'ub',upperB1,'options',options);
                
                % choose multi start option to increase the search space
                gs = MultiStart;

                % start first optimization
                X = run(gs,problem1,MS1);
                
                % set initial condition to identified inertial parameters
                X0_1 = X;

                % compute weighted normalized residuals
                Rh = Th - Wfmh*X;

                % get reshaped residual
                Eh = reshape(Rh,[ncv m]);

                % update old covariance matrix for while loop check
                Oold = Onew;
                
                % update new covariance matrix
                Onew = (Oold*(Eh*Eh.')*Oold)/(m-p_fm);

                % compute updated square root of new covariance matrix
                O_sqrt = real(sqrtm(Onew));

                % stop covariance iteration if the covariance matrix gets negative eigenvalues -> its square-root becomes complex               
                if ~isreal(sqrtm(Onew+9e-9))
                    break;
                end

            end
        
            % update old weights for while loop check
            Dold = Dnew;
            
            % update loop for new weights (this can be changed to another update rule for the weights)
            for i = 1:ncv*m
                % T class hard redecender function
                if Rh(i) < -k
                    Psi = 0;
                elseif Rh(i) > k
                    Psi = 0;
                else
                    Psi = 1;
                end
             
                % update new weights
                Dnew(i,1) = min(Dold(i,1),Psi);
            end

            % update extended weight matrix
            for i = 1:p_fm
                De(:,i) = Dnew;
            end

        end

    % check if offset is a parameter
    if includeOffset
        % compute estimated friction force/torque including offset
        Fest = (Th - Wfmh*[X(1:n)' zeros(1,3*n)]');
        % get intial conditions for friction and friction exponent optimization problem
        X0_2 = [alphanew; X(n+1:end)];
    else
        % compute estimated friction force/torque without offset
        Fest = (Th - Wfmh*[X(1:n)' zeros(1,2*n)]');
        % get intial conditions for friction and friction exponent optimization problem
        X0_2 = [alphanew; X(n+1:end)];
    end
    
    % update alpha for while loop check
    alphaold = alphanew;

    fun2 = @(X2) L2fric(Wfh,X2,n,na,na_idx,nu_idx,m,data,dataFull,J,Fest,includeOffset,includeConstraints,constraintVariant);
    
    % formulate least squares problem for friction exponents and parameters
    problem2 = createOptimProblem('fmincon','x0',X0_2,'objective',fun2,'lb',lowerB2,'ub',upperB2,'options',options);
   
    % choose multi start option to increase the search space
    gs = MultiStart;

    % start second optimization
    X2 = run(gs,problem2,MS2);

    % update new friction exponents
    alphanew = X2(1:n);

    % check if offset is a parameter
    if includeOffset
        % update friction and offset parameters
        X(end-3*n+1:end) = X2(n+1:end);
    else
        % update friction parameters
        X(end-2*n+1:end) = X2(n+1:end);
    end
    
    end

end


% ------------------------------------------------------------------------


% Algorithm without optimimzation over friction exponent
function [X, Wfm] = AlgNoAlphaFMOnly(T_hat,tol,p_fm,n,ncv,na,na_idx,nu_idx,m,k,MS1,data,dataFull,J,lowerB1,upperB1,X0_1,includeOffset,includeConstraints,constraintVariant,options)
        
    % initialize weights
    Dnew = ones(ncv*m,1);
    Dold = zeros(ncv*m,1);
        
    % build extended weight matrix
    for i = 1:p_fm
        De(:,i) = Dnew;
    end

    % compute observation matrix for motor and friction dynamics 
    [Wfm,~,~] = Wextension(ones(n,1),n,na,na_idx,nu_idx,J,m,data,dataFull,includeOffset,includeConstraints,constraintVariant);

    % start while loop for weights
    while(norm(Dold-Dnew)>tol)

        % initialize covariance matrix as identity matrix
        Onew = eye(ncv);
        Oold = zeros(ncv);

        % initialize root of covariance matrix
        O_sqrt = real(sqrtm(Onew));

        % start while loop for covariance matrix
        while(norm(Oold-Onew)>tol)   

            % compute normalized observation matrix and torque vector
            for i = 1:m
                Ts((i-1)*ncv+1:i*ncv,:) = pinv(O_sqrt)*T_hat((i-1)*ncv+1:i*ncv);
                Wfms((i-1)*ncv+1:i*ncv,:) = pinv(O_sqrt)*Wfm((i-1)*ncv+1:i*ncv,:);
            end

            % compute weighted normalized observation matrix and torque vector
            Th = Dnew.*Ts;
            Wfmh = De.*Wfms;

            % initialize least squares function
            fun1 = @(X1) optimFun(X1, Wfmh, Th);

            % formulate least squares problem for all parameters
            problem1 = createOptimProblem('fmincon','x0',X0_1,'objective',fun1,'lb',lowerB1,'ub',upperB1,'options',options);
               
            % choose multi start option to increase the search space
            gs = MultiStart;

            % start optimization
            X = run(gs,problem1,MS1);
                
            % set initial condition to identified inertial parameters
            X0_1 = X;

            % compute weighted normalized residuals
            Rh = Th - Wfmh*X;

            % get reshaped residual
            Eh = reshape(Rh,[ncv m]);

            % update old covariance matrix for while loop check
            Oold = Onew;

            % update new covariance matrix
            Onew = (Oold*(Eh*Eh.')*Oold)/(m-p_fm);

            % compute updated square root of new covariance matrix
            O_sqrt = real(sqrtm(Onew));
            
            % stop covariance iteration if the covariance matrix gets negative eigenvalues -> its square-root becomes complex
            if ~isreal(sqrtm(Onew+9e-9))
                break;
            end

        end
        
        % update old weights for while loop check
        Dold = Dnew;
            
        % update loop for new weights (this can be changed to another update rule for the weights)
        for i = 1:ncv*m
            % T class hard redecender function
            if Rh(i) < -k
                Psi = 0;
            elseif Rh(i) > k
                Psi = 0;
            else
                Psi = 1;
            end
             
            % update new weights
            Dnew(i,1) = min(Dold(i,1),Psi);
        end

        % build extended weight matrix
        for i = 1:p_fm
            De(:,i) = Dnew;
        end

    end

end


%% Least squares functions

% least squares function for all parameters excluding friction exponent
function [fmin, gradf] = optimFun(X, W, T)

    % least squares function
    fmin = norm(W*X - T)^2;
    
    % gradient of least squares function
    if nargout > 1
        gradf = 2*W.'*(W*X - T);
    end
 
end

% ------------------------------------------------------------------------

% least squares function for friction parameters and exponent estimation
function [fmin, gradf] = L2fric(Wf,X,nLinks,na,na_idx,nu_idx,mData,data,dataFull,J,Fest,includeOffset,includeConstraints,constraintVariant)
    
    % get friction exponents
    alpha = X(1:nLinks);
    
    % get friction and offset parameters
    phiF = X(nLinks+1:end);
    
    % least squares function
    fmin = norm(Wf*phiF - Fest)^2;
        
    % gradient of least squares function
    if nargout > 1
        gradf = [2*L2FricGradAlpha(alpha,nLinks,na,na_idx,nu_idx,mData,data,dataFull,J,phiF,includeOffset,includeConstraints,constraintVariant).'*(Wf*phiF - Fest);
                 2*Wf.'*(Wf*phiF - Fest)];
    end
    
    
end

% ------------------------------------------------------------------------

% gradient of friction expnonent part of friction observation matrix
function fricGradAlpha = L2FricGradAlpha(alpha,nLinks,na,na_idx,nu_idx,mData,data,dataFull,J,phiF,includeOffset,includeConstraints,constraintVariant)

    % preallocate gradient matrix
    fricGradAlpha = zeros(nLinks*mData,nLinks);
    fricGrad_ = zeros(nLinks,nLinks);

    % small value to avoid log(0)
    log_eps = 0.000001;

    % compute gradients over all data point
    for i = 1:mData

        % compute gradient over each joint
        for j = 1:nLinks

            % check if offset is included
            if includeOffset
                fricGrad_(j,j) = [0 log(log_eps+abs(data(j,i + mData)))*sign(data(j,i + mData))*abs(data(j,i + mData))^alpha(j) 0]*phiF(3*(j-1)+1:3*j);
            else
                fricGrad_(j,j) = [0 sign(data(j,i + mData))*log(log_eps+abs(data(j,i + mData)))*abs(data(j,i + mData))^alpha(j)]*phiF(2*(j-1)+1:2*j);
            end

        end

        % assign gradient of one datapoint and all joints in the gradient matrix
        fricGradAlpha((i-1)*nLinks+1:i*nLinks,:) = fricGrad_;

    end

    % check if constraints are included
    if includeConstraints
        
        if constraintVariant == 1
        % general case for all kinds of constraints
        
            for i = 1:mData
        
                % compute constraint projection matrix
                J_temp = J(dataFull(:,i));
                K = eye(nLinks) - J_temp(:,sort([na_idx nu_idx])).'*pinv(J_temp(:,sort([na_idx nu_idx])).');
        
                % compute gradient of constrained observation matrix for friction exponent part
                fricGradAlpha((i-1)*nLinks+1:i*nLinks,:) = K*fricGradAlpha((i-1)*nLinks+1:i*nLinks,:);
            end
        
        elseif constraintVariant == 2
        % special case for fully actuated systems
        
            fricGradAlpha_temp = [];
        
            for i = 1:mData
        
                % compute constraint projection matrix
                J_temp = J(dataFull(:,i));
                P = [eye(na); -J_temp(:,nu_idx)\J_temp(:,na_idx)];
        
                % compute gradient of constrained observation matrix for friction exponent part on actuated joint level
                fricGradAlpha_ = P.'*fricGradAlpha((i-1)*nLinks+1:i*nLinks,:);
                fricGradAlpha_temp = [fricGradAlpha_temp;fricGradAlpha_];
            end
        
            % assign temporary gradient variable to gradient
            fricGradAlpha = fricGradAlpha_temp;
        
        end
   end
    
end

%% Helper functions

% Computation of motor and friction observation matrix
% Wm: is the motor observation matrix
% Wf: is the friction and offset observtation matrix
% Wfm: is both combined
function [Wfm, Wm, Wf] = Wextension(alpha,nLinks,na,na_idx,nu_idx,J,mData,data,dataFull,includeOffset,includeConstraints,constraintVariant)

    % check if offset is included and preallocate friction regressor and observation matrix
    if includeOffset
        Wf = zeros(nLinks*mData,3*nLinks);
        Yf = zeros(nLinks,3*nLinks);
    else
        Wf = zeros(nLinks*mData,2*nLinks);
        Yf = zeros(nLinks,2*nLinks); 
    end

    % preallocate motor observation matrix
    Wm = zeros(nLinks*mData,nLinks);
    % preallocate motor regressor matrix
    Ym = zeros(nLinks,nLinks);
    
    % compute regressor matrices over all data points
    for i = 1:mData

        % compute regressor matrices
        for j = 1:nLinks

            % assign accelerations for each joint in motor regressor matrix : Fm = motorInertia*qdd -> dFm/dmotorInertia = qdd 
            Ym(j,j) = data(j,i + 2*mData);

            % check if offset is included and assign dFf/d(Fc,Fv,beta) for each joint, with Fc: coloumb friction coefficient, Fv: vicious friction coefficient, beta: offset
            if includeOffset
                Yf(j,3*(j-1)+1:3*j) = [sign(data(j,i + mData)) sign(data(j,i + mData))*abs(data(j,i + mData))^alpha(j) 1];
            else
                Yf(j,2*(j-1)+1:2*j) = [sign(data(j,i + mData)) sign(data(j,i + mData))*abs(data(j,i + mData))^alpha(j)];
            end
        end

        % assign regressor matrices to observation matrix
        Wm((i-1)*nLinks+1:i*nLinks,:) = Ym;
        Wf((i-1)*nLinks+1:i*nLinks,:) = Yf;

    end

    % check if constraints are included
    if includeConstraints
        
        if constraintVariant == 1
        % general case for all kinds of constraints
        
            for i = 1:mData
        
                % compute constraint projection matrix
                J_temp = J(dataFull(:,i));
                K = eye(nLinks) - J_temp(:,sort([na_idx nu_idx])).'*pinv(J_temp(:,sort([na_idx nu_idx])).');
        
                % compute constrained observation matrix for motor and friction dynamics
                Wm((i-1)*nLinks+1:i*nLinks,:) = K*Wm((i-1)*nLinks+1:i*nLinks,:);
                Wf((i-1)*nLinks+1:i*nLinks,:) = K*Wf((i-1)*nLinks+1:i*nLinks,:);
            end
        
        elseif constraintVariant == 2
        % special case for fully actuated systems
        
            Wf_temp = [];
            Wm_temp = [];
        
            for i = 1:mData
        
                % compute constraint projection matrix
                J_temp = J(dataFull(:,i));
                P = [eye(na); -J_temp(:,nu_idx)\J_temp(:,na_idx)];
        
                % compute constrained observation matrix for motor and friction dynamics on actuated joint level
                Wm_ = P.'*Wm((i-1)*nLinks+1:i*nLinks,:);
                Wf_ = P.'*Wf((i-1)*nLinks+1:i*nLinks,:);
                Wm_temp = [Wm_temp;Wm_];
                Wf_temp = [Wf_temp;Wf_];
            end
        
            % assign temporary observation matrix variable to observation matrix
            Wm = Wm_temp;
            Wf = Wf_temp;
        
        end
   end
        
   % get full observation matrix for motor and friction parameters
   Wfm = [Wm Wf];

end