110 lines
4.2 KiB
Mathematica
110 lines
4.2 KiB
Mathematica
|
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 = 9:-1:1
|
||
|
|
Wb = observationMatrix(i).Wb;
|
||
|
|
Tau = observationMatrix(i).Tau;
|
||
|
|
% [nRow,nCol] = size(Wb);
|
||
|
|
if i == 9
|
||
|
|
pib_OLS=pinv(Wb(:,1:observationMatrix(i).rank))*Tau;
|
||
|
|
pib_MLS = [];
|
||
|
|
elseif i > 1
|
||
|
|
pib_OLS=pinv(Wb(:,observationMatrix(i+1).rank+1:observationMatrix(i).rank))*...
|
||
|
|
(-Wb(:,1:observationMatrix(i+1).rank)*pib_MLS+Tau);
|
||
|
|
else
|
||
|
|
break;
|
||
|
|
end
|
||
|
|
pifrctn_OLS = 0;
|
||
|
|
pib_MLS = [pib_MLS;pib_OLS];
|
||
|
|
end
|
||
|
|
a=1
|
||
|
|
% 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
|