filter DVT1-2

Identification_main.m

@@ -20,15 +20,15 @@ robot = feval(get_robot_func,theta,dtheta,ddtheta,file,opt);
 get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
 robot = feval(get_Kinematics_func,robot,opt);
 
-Jaco=JacobianSpace(robot.slist(:,1:7),theta(1:7));
+% Jaco=JacobianSpace(robot.slist(:,1:7),theta(1:7));
-R1000_Dynamics_num;
+% R1000_Dynamics_num;
 % test_SixAxisFT;
 % R1000_Dynamics;
-% robot = get_velocity(robot, opt);
+robot = get_velocity(robot, opt);
-% robot = get_regressor(robot,opt);
+robot = get_regressor(robot,opt);
 % % symbol matched
 % % verify_regressor_R1000;
-% robot = get_baseParams(robot, opt);
+robot = get_baseParams(robot, opt);
-% % robot = estimate_dyn(robot,opt);
+% robot = estimate_dyn(robot,opt);
-% % robot = estimate_dyn_form_data(robot,opt);
+% robot = estimate_dyn_form_data(robot,opt);
-% robot = estimate_dyn_MLS(robot,opt);
+robot = estimate_dyn_MLS(robot,opt);

R1000_Dynamics_num.m

@@ -15,7 +15,7 @@ link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
-thetalist = [zero_;zero_;zero_;zero_;zero_;pi/3*ones_;zero_;zero_;zero_]';
+thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
 dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
 ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
 
@@ -138,12 +138,14 @@ Mlist_ED = cat(3, Mlist_ED, M);
 %TODO: Get Slist form DH table method
 % RRRRRRRRP
 Slist=robot.slist;
-exf=[0;0;0;0;0;0];
+exf=[0;0;0;0;1;0];
 
 for i = 1:length(thetalist)
     [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
         = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
-        [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
+        [0;0;0], exf, Mlist_CG, Glist, Slist);
+    %         [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
+
     G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
     T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
     %Want to get the result from TC_delta, which means F at CG represent under frame at the last origin  

get_GCTraj_R1000_DVT.m

@@ -1,12 +1,12 @@
 %temp get DVT traj
 gearRatio = [100,100,120,100,100,80,50,100,1/(0.012/(2*pi))];
-motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
+% motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
 % DVT1-2
-% motorConstant = [0.21,0.21,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
+motorConstant = [0.21,0.21,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
 sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
 % load("C:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat");
 % load("D:\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat")
-load("D:\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat");
+load("D:\1833128421\123同步文件夹\R1000-GC-Data\DVT1-2\DVT1-2-GC-6axisok\DVT1-2-GC-6axisok.mat");
 posDir = [1,1,1,1,1,1,1,-1,1];
 isCurrentSensor = true;
 % J9 traj
@@ -78,6 +78,17 @@ for i=robot.ndof:-1:2
     idntfcnTrjctry(i).qdd = idntfcnTrjctry(i).qdd(:,mod(1:dataLength, 400) == 0);
 end
 
+% isoutlier
+for i=robot.ndof:-1:2
+    detect  = isoutlier(idntfcnTrjctry(i).tau);
+    for j=1:length(detect)
+        if(detect(j)==1)
+           %tricky
+           idntfcnTrjctry(i).tau(j) = idntfcnTrjctry(i).tau(j-10);
+        end
+    end
+end
+
 %hack
 % for i=robot.ndof:-1:2
 %     idntfcnTrjctry(i).q(8,:) = - idntfcnTrjctry(i).q(8,:); 

plotData_current.m

@@ -1,13 +1,30 @@
 gearRatio = [100,100,120,100,100,80,50,100,1/(0.012/(2*pi))];
-motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
+% motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
 % DVT1-2
-% motorConstant = [0.21,0.21,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
+motorConstant = [0.21,0.21,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
 sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
 % load("C:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat");
 posDir = [1,1,1,1,1,1,1,-1,1];
 
 % J9 traj
-for i=8
+% hack
+% Filter outliers using moving median filter
+windowSize = 21; % Must be odd
+data_filtered = zeros([length(data(:,(6*(2+1))+11*3)),robot.ndof]);
+for i= robot.ndof:-1:2
+data_filtered(:,i) = data(:,(6*(i+1))+11*3);
+% Apply moving median filter
+medianFiltered = movmedian(data_filtered(:,i), windowSize);
+% Find outliers using median absolute deviation
+medianVal = median(data_filtered(:,i));
+madVal = median(abs(data_filtered(:,i) - medianVal));
+threshold = 3; % Number of MAD deviations to consider as outlier
+outlierIdx = abs(data_filtered(:,i) - medianVal) > threshold * madVal;
+% Replace outliers with median filtered values
+data_filtered(outlierIdx,i) = medianFiltered(outlierIdx);
+end
+
+for i=6
     fileData=eval(strcat('fileData', num2str(i)));
     data = fileData.data;
     dataLength = length(data);
@@ -18,11 +35,11 @@ for i=8
     if i==2
         plot3(data(1:d1Length,i+1+11),data(1:d1Length,6+1+11),data(1:d1Length,i+1+11*2),'r');    hold on;
         plot3(data(d1Length:end,i+1+11),data(d1Length:end,6+1+11),data(d1Length:end,i+1+11*2),'b');    hold on;
-%         plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m');    hold on;
+        plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m');    hold on;
     else
         plot3(data(1:d1Length,i+1+11),data(1:d1Length,i+11),data(1:d1Length,i+1+11*2),'r');    hold on;
         plot3(data(d1Length:end,i+1+11),data(d1Length:end,i+11),data(d1Length:end,i+1+11*2),'b');    hold on;
-%         plot3(data(:,i+1+11),data(:,i+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m');    hold on;
+        plot3(data(:,i+1+11),data(:,i+11),data_filtered*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m');    hold on;
     end
     hold off;
     title(['J' num2str(i) ' Gravity Model']);
@@ -32,7 +49,7 @@ end
 % should run identifcation program firstly
 resolution = 20;
 tau_estimate=[];
-for k=8
+for k=6
     if k ==2
         qx = idntfcnTrjctry(k).q(k,:);
         qy = idntfcnTrjctry(k).q(6,:);

plotData_test.m

@@ -1,11 +1,31 @@
 gearRatio = [100,100,120,100,100,80,50,100,1/(0.012/(2*pi))];
 motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
-sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
+% DVT1-1
+% sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
+% DVT1-2
+sensorDir = [-1,1,-1,-1,1,-1,-1,1,1];
 % load("D:\1833128421\123同步文件夹\R1000-GC-Data\lab12.mat");
 posDir = [1,1,1,1,1,1,1,-1,1];
 
 % J9 traj
-for i=6
+% hack
+% Filter outliers using moving median filter
+windowSize = 21; % Must be odd
+data_filtered = zeros([length(data(:,(6*(2+1))+11*3)),robot.ndof]);
+for i= robot.ndof:-1:2
+data_filtered(:,i) = data(:,(6*(i+1))+11*3);
+% Apply moving median filter
+medianFiltered = movmedian(data_filtered(:,i), windowSize);
+% Find outliers using median absolute deviation
+medianVal = median(data_filtered(:,i));
+madVal = median(abs(data_filtered(:,i) - medianVal));
+threshold = 3; % Number of MAD deviations to consider as outlier
+outlierIdx = abs(data_filtered(:,i) - medianVal) > threshold * madVal;
+% Replace outliers with median filtered values
+data_filtered(outlierIdx,i) = medianFiltered(outlierIdx);
+end
+
+for i=5
     fileData=eval(strcat('fileData', num2str(i)));
     data = fileData.data;
     dataLength = length(data);
@@ -16,11 +36,12 @@ for i=6
     if i==2
         plot3(data(1:d1Length,i+1+11),data(1:d1Length,6+1+11),data(1:d1Length,i+1+11*2)*gearRatio(i)*motorConstant(i),'r');    hold on;
         plot3(data(d1Length:end,i+1+11),data(d1Length:end,6+1+11),data(d1Length:end,i+1+11*2)*gearRatio(i)*motorConstant(i),'b');    hold on;
-%         plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i),'m');    hold on;
+        plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i),'m');    hold on;
     else
         plot3(data(1:d1Length,i+1+11),data(1:d1Length,i+11),data(1:d1Length,i+1+11*2)*gearRatio(i)*motorConstant(i),'r');    hold on;
         plot3(data(d1Length:end,i+1+11),data(d1Length:end,i+11),data(d1Length:end,i+1+11*2)*gearRatio(i)*motorConstant(i),'b');    hold on;
 %         plot3(data(:,i+1+11),data(:,i+11),data(:,(6*(i+1))+11*3)*sensorDir(i),'m');    hold on;
+        plot3(data(:,i+1+11),data(:,i+11),data_filtered(:,i)*sensorDir(i),'m');    hold on;
     end
     hold off;
     title(['J' num2str(i) ' Gravity Model']);
@@ -30,7 +51,7 @@ end
 % should run identifcation program firstly
 resolution = 20;
 tau_estimate=[];
-for k=6
+for k=5
     if k ==2
         qx = idntfcnTrjctry(k).q(k,:);
         qy = idntfcnTrjctry(k).q(6,:);