IRDYn/estimate_dyn_MLS.m

function robot = estimate_dyn_MLS(robot,opt)
% -------------------------------------------------------------------
% Get datas
% ------------------------------------------------------------------------
get_GCTraj_func = sprintf('get_GCTraj_%s',opt.robotName);
feval(get_GCTraj_func);
% -------------------------------------------------------------------
% Generate Regressors based on data
% ------------------------------------------------------------------------
drvGains = [];
baseQR = robot.baseQR;

for i = 2:1:robot.ndof
% for i = robot.ndof
    q = idntfcnTrjctry(i).q;
    qd = idntfcnTrjctry(i).qd;
    qdd = idntfcnTrjctry(i).qdd;
    [nRow,nCol] = size(idntfcnTrjctry(i).qd);
    Wb = []; Tau = [];
    for j = 1:nRow/robot.ndof
        for k = 1:nCol
            if opt.isJointTorqueSensor
                base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
                Yb = feval(base_regressor_func, q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
                    qd(robot.ndof*(j-1)+1:robot.ndof*j,k),...
                    qdd(robot.ndof*(j-1)+1:robot.ndof*j,k),baseQR);
                Yb = Yb(i,:);
                Wb = vertcat(Wb, Yb);
                Tau = vertcat(Tau, idntfcnTrjctry(i).tau(j,k));
            end
        end
    end
    observationMatrix(i).Wb = Wb;
    observationMatrix(i).Tau = Tau;
    observationMatrix(i).rank = robot.baseQR.rank(i);
end

% ---------------------------------------------------------------------
% Estimate parameters
% ---------------------------------------------------------------------
sol = struct;
for i = robot.ndof:-1:2
% for i = robot.ndof
    Wb = observationMatrix(i).Wb;
    Tau = observationMatrix(i).Tau;
    if i == robot.ndof
        pib_OLS=pinv(Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i).rank+1:end))*Tau;
        pib_MLS = [];
    elseif i > 1
        pib_OLS=pinv(Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i).rank+1:...
            baseQR.numberOfBaseParameters-observationMatrix(i+1).rank))*...
            (-Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i+1).rank+1:end)*pib_MLS+Tau);
    else
        break;
    end
    pifrctn_OLS = 0;
    pib_MLS = [pib_OLS;pib_MLS];
end
sol.pib_MLS = pib_MLS;
robot.sol = sol;
% method = 'OLS';
% if strcmp(method, 'OLS')
%     % Usual least squares
%     [sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(observationMatrix);
% elseif strcmp(method, 'PC-OLS')
%     % Physically consistent OLS using SDP optimization
%     [sol.pi_b, sol.pi_fr, sol.pi_s] = physicallyConsistentEstimation(Tau, Wb, baseQR);
% else
%     error("Chosen method for dynamic parameter estimation does not exist");
% end
% robot.sol = sol;
% % Local unctions
%     function observationMatrix = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains,opt)
%         for i = 1:1:robot.ndof
%             [nRow,nCol] = size(idntfcnTrjctry(i).qd);
%             Wb = []; Tau = [];
%             for j = 1:nRow/robot.ndof
%                 for k = 1:nCol
%                     if opt.isJointTorqueSensor
%                         base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
%                         Yb = feval(base_regressor_func, q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
%                             qd(robot.ndof*(j-1)+1:robot.ndof*j,k),...
%                             qdd(robot.ndof*(j-1)+1:robot.ndof*j,k),baseQR);
%                         Yb = Yb(i,:);
%                         Wb = vertcat(Wb, Yb);
%                         Tau = vertcat(Tau, idntfcnTrjctry(i).tau(j,k));
%                     end
%                 end
%             end
%             observationMatrix(i).Wb = Wb;
%             observationMatrix(i).Tau = Tau;
%         end
%     end
% 
% 
%     function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(observationMatrix)
%         % Function perfroms ordinary least squares estimation of parameters
% %         pi_OLS = (Wb'*Wb)\(Wb'*Tau);
% %         pib_OLS = pi_OLS(1:40); % variables for base paramters
% %         pifrctn_OLS = pi_OLS(41:end);
%         for i = 9:-1:1
%           Wb = observationMatrix(i).Wb;
%           Tau = observationMatrix(i).Tau;
%           pib_OLS(i)=pinv(Wb)*Tau;
%           pifrctn_OLS = 0;
%         end
%     end
%     function [pib_OLS, pifrctn_OLS] = MultiLeastSquareEstimation(idntfcnTrjctry, Tau, Wb)
%         % Function perfroms Multi step ordinary least squares estimation of parameters
%           
%         pib_OLS=pinv(Wb)*Tau;
%         pifrctn_OLS = 0;
%     end
end