111 lines
4.3 KiB
Matlab
111 lines
4.3 KiB
Matlab
function robot = estimate_dyn_MLS(robot,opt)
|
|
% -------------------------------------------------------------------
|
|
% Get datas
|
|
% ------------------------------------------------------------------------
|
|
get_GCTraj_R1000_EVT;
|
|
% -------------------------------------------------------------------
|
|
% Generate Regressors based on data
|
|
% ------------------------------------------------------------------------
|
|
drvGains = [];
|
|
baseQR = robot.baseQR;
|
|
|
|
for i = 1:1: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:1
|
|
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 |