E1gen.m

@@ -0,0 +1,5 @@
+function E=E1gen(A,i,j)
+n=size(A); %求矩阵A的行数和列数
+m=min(n); %获取矩阵行数和列数中的最小值
+E=eye(m); %产生单位对角阵
+E(i,i)=0; E(j,j)=0; E(i,j)=1; E(j,i)=1;

Identification_main.m

@@ -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);

get_baseParams.m

@@ -66,7 +66,36 @@ W1 = W*E(:,1:qr_rank);
 W2 = W*E(:,qr_rank+1:end);
 assert(norm(W2 - W1*beta) < 1e-3,... 
         'Found realationship between W1 and W2 is not correct\n');
+% sort QR result
+[~,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);
+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);
+end
 
+[~,RRR]=qr(W(:,[A;index(16:end)]));
+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
+% 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');
+% get sort result : pi=pi1+beta_test*pi2;
 % -----------------------------------------------------------------------
 % Find base parmaters
 % -----------------------------------------------------------------------

untitled2.m

@@ -90,4 +90,9 @@ end
 yy=eye(4,4);
 for i =1:9
 yy = yy*Mlist_CG(:,:,i)
-end
+end
+%%
+A=pascal(6)
+AA = A;
+B=A*E1gen(A,6,1)*E1gen(A,5,2)
+% AA(:,[1,2,6]) = AA(:,[6,5,1])