From 1185d20dd1cafe7d79039394070987280843eebc Mon Sep 17 00:00:00 2001
From: cosmic_power <cosmic_power@163.com>
Date: Wed, 13 Nov 2024 02:00:09 +0800
Subject: [PATCH] sort fail

---
 E1gen.m               |  5 +++++
 Identification_main.m |  2 +-
 get_baseParams.m      | 29 +++++++++++++++++++++++++++++
 untitled2.m           |  7 ++++++-
 4 files changed, 41 insertions(+), 2 deletions(-)
 create mode 100644 E1gen.m

diff --git a/E1gen.m b/E1gen.m
new file mode 100644
index 0000000..6b762fd
--- /dev/null
+++ b/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;
\ No newline at end of file
diff --git a/Identification_main.m b/Identification_main.m
index eeab42f..532d5be 100644
--- a/Identification_main.m
+++ b/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);
\ No newline at end of file
+% robot = estimate_dyn_MLS(robot,opt);
\ No newline at end of file
diff --git a/get_baseParams.m b/get_baseParams.m
index 7af9334..0f1e322 100644
--- a/get_baseParams.m
+++ b/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
 % -----------------------------------------------------------------------
diff --git a/untitled2.m b/untitled2.m
index cef6b9d..d393baf 100644
--- a/untitled2.m
+++ b/untitled2.m
@@ -90,4 +90,9 @@ end
 yy=eye(4,4);
 for i =1:9
 yy = yy*Mlist_CG(:,:,i)
-end
\ No newline at end of file
+end
+%%
+A=pascal(6)
+AA = A;
+B=A*E1gen(A,6,1)*E1gen(A,5,2)
+% AA(:,[1,2,6]) = AA(:,[6,5,1])
\ No newline at end of file