second order, need add on error analysis

SOURCE/RP_2015_Rozenheck.tex

@@ -69,8 +69,10 @@ refrensi.
     $e = \begin{bmatrix} e_1^T \\ \vdots \\ e_m^T \end{bmatrix} \in \mathbb{R}^{2m}$ & Edge Vector \\
 
     $E \in R^{n\times m}$ & Incidence matrix dimana isinya adalah $\{0, \pm 1\}$ \\
-     & barisnya menandakan vertices dan kolom nya menandakan edge \\ \hline
+    & barisnya menandakan vertices dan kolom nya menandakan edge \\ \hline
-    \hline
+    $A_1, \dots, A_i \in \mathbb{R}^{p \times q}$ & \\
+    $diag(A_i) \triangleq blkdiag\{A_1, \dots, A_n\} \in \mathbb{R}^{np \times nq}$ & https://www.mathworks.com/help/matlab/ref/blkdiag.html \\
+   \hline
   \end{tabular}
 %% \end{table}
 
@@ -182,7 +184,7 @@ Dengan menerapkan kendali PI
 Menghasilkan state baru dan Ditulis ulang
 \begin{align}
     \dot{x}_1 =& x_{2}(t)\\
-    \dot{x}_2 =& -k_{p1}x_2(t) -R(x_1)^T k_{p2}(R(x_1)x_1(t) - d )) - \xi_1 - R(x_1) \xi_2 \\
+    \dot{x}_2 =& -k_{p1}x_2(t) -R(x_1)^T k_{p2}(R(x_1)x_1(t) - d )) - \xi_1 - R(x_1)^T \xi_2 \\
   \dot{\xi}_1 =& k_{i1} x_2(t)\\
   \dot{\xi}_2 =& k_{i2} (R(x_1)x_1(t) - d)
 \end{align}
@@ -193,7 +195,7 @@ Dalam bentuk matrix
   \end{bmatrix} =
   \begin{bmatrix}
     0 & 1 & 0 & 0\\
-    -k_{p2}R(x_1)^T R(x_1) & -k_{p1} & -1 & -R(x_1) \\
+    -k_{p2}R(x_1)^T R(x_1) & -k_{p1} & -1 & -R(x_1)^T \\
     0 & k_{i1} & 0 & 0 \\
     k_{i2}R(x_1) & 0 & 0 & 0 \\
   \end{bmatrix}

SOURCE/SM_2015_Rozenheck.m

@@ -1,85 +1,48 @@
 clear -all
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
 conRobot = [1 2 1;
-            %% 2 1 -1;
+            3 2 1;
-            1 5 1;
+            3 1 1;
-            %% 5 1 -1;
-            1 6 1;
-            %% 6 1 -1;
-            2 3 1;
-            %% 3 2 -1;
-            2 4 1;
-            %% 4 2 -1;
-            3 4 1;
-            %% 4 3 -1;
-            4 5 1;
-            %% 5 4 -1;
-            5 6 1;
-            %% 6 5 -1;
-            1 4 1;
-            %% 4 1 -1;
-            5 2 1;
-            %% 2 5 -1
-            %% 6 3 1;
-            %% 6 4 1;
-            %% 6 2 1;
-            %% 3 1 1;
-            %% 3 5 1;
            ];
-length_d = 3;
+length_d = .5;
 dRobot = [ 1;
            1;
            1;
-           1;
-           1;
-           1;
-           1;
-           1;
-           1.41;
-           1.41;
-           %% 2;
-           %% 3.014;
-           %% 3.014;
-           %% 3.014;
-           %% 3.014;
 ];
 corRobot = [1.5; 1.7;
             2; 2.5;
             2.5; 2.8;
-            2.5; 2;
+           ]; % xy xy xy
-            2.2; 1;
-            1.5; 0.2;
-           ]*10; % xy xy xy
 B = [
      1 0; 0 1;
      0 0; 0 0;
      0 0; 0 0;
-     0 0; 0 0;
-     0 0; 0 0;
-     0 0; 0 0;
     ];
-kp = 30;
+kp = 80;
-ki = 3;
+ki = 1;
-%% fvref = fncSpeedRef('ysin',100,2);
+% fvref = fncSpeedRef('ysin',50,2);
-%% fvref = fncSpeedRef('xsin',50,2);
+% fvref = fncSpeedRef('xsin',50,2);
-%% fvref = fncSpeedRef('cw',-100,1);
+% fvref = fncSpeedRef('cw',-300,1);
-fvref = fncSpeedRef('s',0,0);
+fvref = fncSpeedRef('s',50,50);
 fvrefans = fncSpeedRef('s',0,0);
-tspan = 1:0.1:5;
+tspan = 1:0.1:10;
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur
 
 [R,K,d] = rigidityMatrixFnc(conRobot);
 _zero = zeros(size(R(corRobot,K),1),1);
 sInit = [corRobot; _zero;];
 sAnsInit =[_zero; _zero;];
+s2ndInit = [corRobot;     %x1
+            zeros(length(corRobot),1); %x2
+            zeros(length(corRobot),1); %xi1
+            _zero]; %xi2
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start solving
 printf("Mulai memecakan masalah \n\n");
 startExe = tic;
 
-dydt = @(t, y) systm_robot(t, y,(dRobot*length_d), R,K, kp, ki, B, fvref(t));
+dydt = @(t, y) systm_2nd_order_robot(t, y,(dRobot*length_d), R,K, kp,kp, ki,ki, B, fvref(t));
-[t,y] = ode45(dydt, tspan, sInit);
+[t,y] = ode45(dydt, tspan, s2ndInit);
-
 cntr = 1;
 bypassCntr = @(c) cntr;
 plusPlusCntr = @(c) cntr++;
@@ -133,7 +96,7 @@ function plot_con (pltRb, yOut, conIn,xm,ym, time)
 endfunction
 
 plot_con(plot_rb, y', conRobot, xrb, yrb, 1);
-plot_con(plot_rb, y', conRobot, xrb, yrb, round(length(tspan)/2));
+% plot_con(plot_rb, y', conRobot, xrb, yrb, round(length(tspan)/2));
 plot_con(plot_rb, y', conRobot, xrb, yrb, length(tspan));
 
 
@@ -171,6 +134,7 @@ str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
 eval(str_tmp)
 title("Norm error setiap edge robot")
 
-save DataOutMotion.data y
+csvwrite("DataOutMotion.csv", [t' y'])
-save DataErrorEdge.data yans
+% save DataOutMotion.data y
+% save DataErrorEdge.data yans
 

SOURCE/rigidityMatrixFnc.m

@@ -22,7 +22,7 @@ function [R,K, d] = rigidityMatrixFnc (edgeL)
   endfor
   str = strcat(str,sprintf("errVec(%i:%i)' ));",i+2,i+3));
                                 % return sebagai fungsi R
-  eval(str);
+  eval(str);  %% replicate from https://www.mathworks.com/help/matlab/ref/blkdiag.html
   R = @(x,k) errBlockDiagonal(edgeVector(x,k))*k;
 
 endfunction

SOURCE/systm_2nd_order_robot.m

@@ -0,0 +1,32 @@
+
+%% systm_2nd_robot
+%% param :
+%% t -> time
+%% x -> state
+%% d -> distance antara robot nya
+%% R, K -> return dari fungsi rigidityMatrixFnc()
+%% kp -> konstanta proporsional
+%% ki -> konstanta integral
+%% B -> matrix selctor vref
+%% vref -> kecepatan refrensi
+function dxdt = systm_2nd_order_robot(t,x,d,R,K,kp1,kp2,ki1,ki2,B,vref)
+  n = length(K(1,:))/2;
+  l = length(R(ones(n*2,1),K));
+  m = length(K(:,1))/2;
+  dxdt = zeros(length(x),1);
+  x1 = x(1:n*2);
+  len = length(x1);
+  x2 = x(len+1:len+length(x1));
+  len = len + length(x2);
+  x3 = x(len+1:len+length(x2));
+  len = len + length(x3);
+  x4 = x(len+1:length(x));
+  
+  dxdt(1:n*2)                = x2;
+  len = length(x1);
+  dxdt(len+1:len+length(x1)) = -kp1*x2 -kp2*R(x1,K)'*R(x1,K)*x1 +kp2*R(x1,K)'*d -x3 -R(x1,K)'*x4 + (B*vref);
+  len = len + length(x2);
+  dxdt(len+1:len+length(x2)) = ki1*x2;
+  len = len + length(x3);
+  dxdt(len+1:length(x))      = ki2*R(x1,K)*x1 -ki2*d;
+endfunction

SOURCE/systm_robot.m

@@ -1,3 +1,13 @@
+%% systm_robot
+%% param :
+%% t -> time
+%% x -> state
+%% d -> distance antara robot nya
+%% R, K -> return dari fungsi rigidityMatrixFnc()
+%% kp -> konstanta proporsional
+%% ki -> konstanta integral
+%% B -> matrix selctor vref
+%% vref -> kecepatan refrensi
 function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
   n = length(K(1,:))/2;
   l = length(R(ones(n*2,1),K));
@@ -5,14 +15,6 @@ function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
   dxdt = zeros(l,1);
   x1 = x(1:n*2);
   x2 = x(length(x1)+1:length(x));
-  %% if (t > 5) && (t < 15)
-    %% vref = [5; 10];
-  %% else
-    %% if (t > 15) && (t > 20)
-      %% vref = [5; -10];
-    %% endif
-  %% endif
-  %% dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref);
   dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref);
   dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*d);
 endfunction