From d5d85fbd990dc25c5388b4989067eed714849100 Mon Sep 17 00:00:00 2001
From: cosmic_power <cosmic_power@163.com>
Date: Fri, 25 Oct 2024 23:41:23 +0800
Subject: [PATCH] fix bug for regressor identification

---
 Identification_main.m              |  6 +++---
 codegen/base_regressor_R1000_DVT.m |  6 ++++--
 estimate_dyn.m                     |  4 ++--
 get_baseParams.m                   | 10 +++++-----
 untitled3.m                        |  3 ++-
 5 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/Identification_main.m b/Identification_main.m
index a349bf6..2c7e83d 100644
--- a/Identification_main.m
+++ b/Identification_main.m
@@ -9,9 +9,9 @@ opt.robotName = 'R1000_DVT';
 opt.reGenerate = false;
 opt.Isreal = true;
 
-theta = 1000*ones(9,1);
+theta = zeros(9,1);
 dtheta = zeros(9,1);
-ddtheta = 1000*ones(9,1);
+ddtheta = zeros(9,1);
 robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt);
 robot = get_Kinematics(robot, opt);
 
@@ -22,4 +22,4 @@ robot = get_regressor(robot,opt);
 % symbol matched
 % verify_regressor_R1000;
 robot = get_baseParams(robot, opt);
-% robot = estimate_dyn(robot,opt);
\ No newline at end of file
+robot = estimate_dyn(robot,opt);
\ No newline at end of file
diff --git a/codegen/base_regressor_R1000_DVT.m b/codegen/base_regressor_R1000_DVT.m
index 739881b..8f3134b 100644
--- a/codegen/base_regressor_R1000_DVT.m
+++ b/codegen/base_regressor_R1000_DVT.m
@@ -1,4 +1,4 @@
-function base_regrssor  = base_regressor_R1000_DVT(theta,dtheta,ddtheta)
+function base_regrssor  = base_regressor_R1000_DVT(theta,dtheta,ddtheta,baseQR)
 opt.robot_def = 'direct';
 opt.KM_method = 'SCREW';
 opt.Vel_method = 'Direct';
@@ -13,5 +13,7 @@ robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt);
 robot = get_Kinematics(robot, opt);
 robot = get_velocity(robot, opt);
 robot = get_regressor(robot, opt);
-robot = get_baseParams(robot, opt);
+% get base params
+robot.baseQR = baseQR;
+robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
 base_regrssor = robot.baseQR.regressor;
\ No newline at end of file
diff --git a/estimate_dyn.m b/estimate_dyn.m
index e49ee6e..babb132 100644
--- a/estimate_dyn.m
+++ b/estimate_dyn.m
@@ -2,7 +2,7 @@ function robot = estimate_dyn(robot,opt)
 % -------------------------------------------------------------------
 % Get datas
 % ------------------------------------------------------------------------
-time = 0:0.1:1;
+time = 0:0.01:1;
 f=1;
 q_J = sin(2*pi*f*time);
 qd_J = (2*pi*f)*cos(2*pi*f*time);
@@ -62,7 +62,7 @@ robot.sol = sol;
             %             Ybi = [Yi*E1, Yfrctni];
             base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
             Yb = feval(base_regressor_func, idntfcnTrjctry.q(:,i), ...
-                idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i));
+                idntfcnTrjctry.qd(:,i),idntfcnTrjctry.qdd(:,i),baseQR);
             Wb = vertcat(Wb, Yb);
             %             Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
             Tau = vertcat(Tau, idntfcnTrjctry.tau(:,i));
diff --git a/get_baseParams.m b/get_baseParams.m
index 2c8af6c..1bd148e 100644
--- a/get_baseParams.m
+++ b/get_baseParams.m
@@ -1,6 +1,6 @@
 function robot = get_baseParams(robot,opt)
 % Seed the random number generator based on the current time
-rng('shuffle');
+rng("default");
 ndof = robot.ndof;
 includeMotorDynamics = false;
 % ------------------------------------------------------------------------
@@ -15,7 +15,7 @@ q2d_max = 6*pi*ones(ndof,1);
 % -----------------------------------------------------------------------
 % Get observation matrix of identifiable paramters
 W = [];    
-for i = 1:250
+for i = 1:25
     q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
     qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
     q2d_rnd = -q2d_max + 2*q2d_max.*rand(ndof,1);
@@ -35,7 +35,7 @@ end
 %   R is upper triangular matrix
 %   Q is unitary matrix
 %   E is permutation matrix
-[Q, R, E] = qr(W);
+[~, R, E] = qr(W);
 
 % matrix W has rank qr_rank which is number number of base parameters 
 qr_rank = rank(W);
@@ -47,13 +47,13 @@ qr_rank = rank(W);
 R1 = R(1:qr_rank,1:qr_rank);
 R2 = R(1:qr_rank,qr_rank+1:end);
 beta = R1\R2; % the zero rows of K correspond to independent columns of WP
-beta(abs(beta)<sqrt(eps)) = 0; % get rid of numerical errors
+beta(abs(beta)<10^-5) = 0; % get rid of numerical errors
 % W2 = W1*beta
 
 % Make sure that the relation holds
 W1 = W*E(:,1:qr_rank);
 W2 = W*E(:,qr_rank+1:end);
-assert(norm(W2 - W1*beta) < 1e-6,... 
+assert(norm(W2 - W1*beta) < 1e-3,... 
         'Found realationship between W1 and W2 is not correct\n');
 
 % -----------------------------------------------------------------------
diff --git a/untitled3.m b/untitled3.m
index cd3933f..93e0e9f 100644
--- a/untitled3.m
+++ b/untitled3.m
@@ -62,4 +62,5 @@ pi1 = E(:,1:qr_rank)'*pi_lgr_sym; % independent paramters
 pi2 = E(:,qr_rank+1:end)'*pi_lgr_sym; % dependent paramteres
 beta = robot.baseQR.beta;
 % all of the expressions below are equivalent
-pi_lgr_base = pi1 + beta*pi2;
\ No newline at end of file
+pi_lgr_base = pi1 + beta*pi2;
+vpa(simplify(pi_lgr_base),2)
\ No newline at end of file