fix bug for regressor identification

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);
+robot = estimate_dyn(robot,opt);

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;

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

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');
 
 % -----------------------------------------------------------------------

untitled3.m

@@ -63,3 +63,4 @@ 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;
+vpa(simplify(pi_lgr_base),2)