MLSE GC
This commit is contained in:
cosmic_power
parent
1185d20dd1
commit
d7d355a51f
|
|
@ -32,4 +32,4 @@ robot = get_baseParams(robot, opt);
|
|||
% robot.currentData = currentData;
|
||||
% robot = estimate_dyn(robot,opt);
|
||||
% robot = estimate_dyn_form_data(robot,opt);
|
||||
% robot = estimate_dyn_MLS(robot,opt);
|
||||
robot = estimate_dyn_MLS(robot,opt);
|
||||
|
|
@ -17,15 +17,16 @@ toruqeSensorRange = [500;500;230;90;60;60;60;20;20];
|
|||
robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
|
||||
if opt.isJointTorqueSensor
|
||||
%hack
|
||||
% tau = zeros([robot.ndof,length(q_J)]);
|
||||
tau = zeros([1,length(q_J)]);
|
||||
tau = zeros([robot.ndof,length(q_J)]);
|
||||
% tau = zeros([1,length(q_J)]);
|
||||
for i = 1:length(q_J)
|
||||
% regressor = standard_regressor_Two_bar(q(:,i),qd(:,i),qdd(:,i));
|
||||
standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
|
||||
regressor = feval(standard_regressor_func,q(:,i),qd(:,i),qdd(:,i));
|
||||
tau_Full=regressor*robot_pi;
|
||||
joint_idex = robot.ndof-2;
|
||||
tau(:,i) = tau_Full((joint_idex-1)+1:(joint_idex));
|
||||
tau(:,i) = tau_Full;
|
||||
% joint_idex = robot.ndof-2;
|
||||
% tau(:,i) = tau_Full((joint_idex-1)+1:(joint_idex));
|
||||
% tau(:,i) = tau(:,i) + 5*10^-3*toruqeSensorRange.*rand(size(tau(:,i)));
|
||||
end
|
||||
elseif opt.isSixAxisFTSensor
|
||||
|
|
@ -82,8 +83,8 @@ robot.sol = sol;
|
|||
Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
|
||||
idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR);
|
||||
%hack
|
||||
joint_idex = robot.ndof-2;
|
||||
Yb = Yb((joint_idex-1)+1:(joint_idex),:);
|
||||
% joint_idex = robot.ndof-2;
|
||||
% Yb = Yb((joint_idex-1)+1:(joint_idex),:);
|
||||
Wb = vertcat(Wb, Yb);
|
||||
% Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
|
||||
Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
|
||||
|
|
|
|||
|
|
@ -0,0 +1,110 @@
|
|||
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(:,15-observationMatrix(i).rank+1"))*Tau;
|
||||
pib_MLS = [];
|
||||
elseif i > 1
|
||||
pib_OLS=pinv(Wb(:,15-observationMatrix(i).rank+1:15-observationMatrix(i+1).rank))*...
|
||||
(-Wb(:,15-observationMatrix(i+1).rank+1)*pib_MLS+Tau);
|
||||
else
|
||||
break;
|
||||
end
|
||||
pifrctn_OLS = 0;
|
||||
pib_MLS = [pib_OLS;pib_MLS];
|
||||
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
|
||||
|
|
@ -42,16 +42,16 @@ for i = 9:-1:1
|
|||
Tau = observationMatrix(i).Tau;
|
||||
% [nRow,nCol] = size(Wb);
|
||||
if i == 9
|
||||
pib_OLS=pinv(Wb(:,1:observationMatrix(i).rank))*Tau;
|
||||
pib_OLS=pinv(Wb(:,15-observationMatrix(i).rank+1:end))*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);
|
||||
pib_OLS=pinv(Wb(:,15-observationMatrix(i).rank+1:15-observationMatrix(i+1).rank))*...
|
||||
(-Wb(:,15-observationMatrix(i+1).rank+1:end)*pib_MLS+Tau);
|
||||
else
|
||||
break;
|
||||
end
|
||||
pifrctn_OLS = 0;
|
||||
pib_MLS = [pib_MLS;pib_OLS];
|
||||
pib_MLS = [pib_OLS;pib_MLS];
|
||||
end
|
||||
a=1
|
||||
% method = 'OLS';
|
||||
|
|
|
|||
|
|
@ -70,31 +70,25 @@ assert(norm(W2 - W1*beta) < 1e-3,...
|
|||
[~,RR]=qr(W);
|
||||
[~,index]=sort(abs(diag(RR)),'descend');
|
||||
A = sort(index(1:qr_rank));
|
||||
% this matrix is wrong, beacause A sort again, should be [(W*E1gen1)(1:qr_rank),
|
||||
% (W*E1gen2)(qr_rank+1:end)]? for example: 9 to 1, 1 to 17: 9->17;
|
||||
% No, only leaf E1gen(RR,index(i),i); still not correct
|
||||
P = eye(90);
|
||||
A = [A;index(qr_rank+1:end)];
|
||||
P = zeros(90);
|
||||
for i = 1:90
|
||||
if i <16
|
||||
temp(:,:,i) = E1gen(RR,i,A(i));
|
||||
else
|
||||
temp(:,:,i) = E1gen(RR,i,index(i));
|
||||
end
|
||||
P = P*temp(:,:,i);
|
||||
P(A(i),i) = 1;
|
||||
end
|
||||
|
||||
[~,RRR]=qr(W(:,[A;index(16:end)]));
|
||||
[~,RRR]=qr(W*P);
|
||||
RRR1 = RRR(1:qr_rank,1:qr_rank);
|
||||
RRR2 = RRR(1:qr_rank,qr_rank+1:end);
|
||||
beta_test = RRR1\RRR2; % the zero rows of K correspond to independent columns of WP
|
||||
beta_test(abs(beta_test)<10^-5) = 0; % get rid of numerical errors
|
||||
beta = RRR1\RRR2; % the zero rows of K correspond to independent columns of WP
|
||||
beta(abs(beta)<10^-5) = 0; % get rid of numerical errors
|
||||
% W2 = W1*beta
|
||||
|
||||
% Make sure that the relation holds
|
||||
% WW1 = W*P(:,1:qr_rank); % swap col
|
||||
% WW2 = W*P(:,qr_rank+1:end);
|
||||
% assert(norm(WW2 - WW1*beta_test) < 1e-3,...
|
||||
% 'Found realationship between W1 and W2 is not correct\n');
|
||||
WW1 = W*P(:,1:qr_rank); % swap col
|
||||
WW2 = W*P(:,qr_rank+1:end);
|
||||
assert(norm(WW2 - WW1*beta) < 1e-3,...
|
||||
'Found realationship between W1 and W2 is not correct\n');
|
||||
E = P;
|
||||
% get sort result : pi=pi1+beta_test*pi2;
|
||||
% -----------------------------------------------------------------------
|
||||
% Find base parmaters
|
||||
|
|
|
|||
Loading…
Reference in New Issue