\chapter{\babEmpat}

Metode penelitian akan menerangkan tentang bagaimana strategi untuk menemukan koordinat tetangga menggunakan variabel jarak saja.
Dapat diperhatikan pada persamaan~\eqref{eq:ss-formasi} bahwa \textit{state} yang dibutuhkan adalah kecepatan.
Maka kendali robot diharuskan dapat mencapai kecepatan tertentu.
Strategi pencarian koordinat mengharuskan juga robot untuk mencapai koordinat relatif tertentu.
Pada bab ini akan menjelaskan bagaimana robot menemukan koordinat, mencapai kecepatan dan koordinat tertentu,
dan membentuk formasi menggunakan model robot \textit{holonomic}.

\section{Strategi Penentuan Koordinat Tetangga}
\label{bab:empat:Strategi_koordinat_tetangga}

Tujuan dari strategi penentuan koordinat tetangga ini adalah menemukan koodinat menggunakan variabel jarak saja.
Mengadopsi dari pengembangan oleh Qiang (\kutip{qiang2018}) meletakkan dua sensor jarak pada salah satu robot,
menggunakan kaidah rumus segitiga untuk mendapatkan sudut diantara robot tetangganya dan mengubahnya menjadi koordinat polar.
Di penelitian ini menggunakan kaidah rumus segitiga tersebut akan tetapi robot hanya terpasang satu sensor jarak saja
dan mengharuskan melakukan langkah tertentu untuk mendapatkan sudut diantara robot tetangganya.

Penentuan koordinat tentangga dapat ditemukan dengang mengubah koordinat polar menjadi koordinat kartesian.
Koordinat polar membutuhkan panjang $d_a$, dan sudut $\alpha$.
Panjang $d_a$ adalah variable yang didapat dari sensor yang memberikan nilai jarak dari robot $A$ ke robot $B$,
akan tetapi untuk mendapatkan koordinat polar, pengukuran sudu $\alpha$ tidak tersedia.
Algoritama yang ditawarkan memanfaatkan hukum \textit{cosinus} pada segitiga untuk mendapatkan sudut tersebut.

\begin{figure}
  \centering
  \includegraphics[scale=.2]{BAB3/img/estimate_coordinate.png}
  \caption{Strategi Penentuan Koordinat Langkah Pertama}
  \label{fig:strategiPenentuanKoordinat_satu}
\end{figure}

\textbf{Langkah pertama}. Gambar~\ref{fig:strategiPenentuanKoordinat_satu} adalah Ilustrasi langkah pertama algoritma cosinus dimana
robot $A$ diharuskan berpindah sepanjang la atau ke koordinat $A' = (0, la )$ menggunakan
kendali mode dua di Persamaan~\eqref{eq:kendali_mode_dua} akan tetapi robot harus menyimpan jarak di $[k]$ terlebih
dahulu. Setelah berpindah robot $A$ mendapatkan jarak di $[k + 1]$ digunakan untuk menentukan
sudut $\alpha_i^\circ$ menggunakan rumus segitiga cosinus. Berikut adalah Persamaan $\alpha_i^\circ$.
\begin{align}
  \zeta_i^a      & = cos^{-1}\Bigg( \frac{l_a^2 + d_a[k+1]^2 -d_a[k]^2}{2d_a[k+1]l_a} \Bigg) \\
  \alpha_i^\circ & = 180^\circ\pm \zeta_i^a 
  \label{eq:algo_getAngle}
\end{align}
Variabel $\alpha_i^\circ$ dan $d_i[k+1]$ adalah nilai dari koordinat polar dari setiap robot tetangga A. Diubah
menjadi koordinat kartesian untuk dapat dimasukkan dalam state kendali formasi.
\begin{align}
  x_{B_i}^A = \begin{bmatrix}
    x_{B_i}= d_i[k]\cos \alpha_i^\circ \\
    y_{B_i}= d_i[k]\sin \alpha_i^\circ
  \end{bmatrix} \label{eq:algo_koordinat}
\end{align}

\textbf{Langkah kedua}. Koordinat di Persamaan~\eqref{eq:algo_koordinat} akan menghasilkan bias dikarenakan
Persamaan~\eqref{eq:algo_getAngle} tidak mengetahui letak kuadran sudutnya. Menggunakan ilustrasi di Gambar~\ref{fig:strategiPenentuanKoordinat_satu},
langkah pertama menghasilkan dua kemungkinan koordinat robot $B_1$ dan $B_1'$ .
Apabila di Gambar~\ref{fig:strategiPenentuanKoordinat_satu}, Sudut $\zeta_i^a$ adalah sudut segitiga $\angle AA'B_1$ atau $\angle AA'B_2$ sehingga
dimungkinkan koordinat yang dihasilkan Persamaan~\eqref{eq:algo_getAngle} bisa berada pada kuadran 1 atau kuadran 4.
Oleh karena itu di Persamaan~\ref{eq:algo_getAngle} terdapat operasi $\pm$ dimana operasi tersebut
akan dilakukan berdasarkan letak kuadran $B_i$.

\begin{align}
  \alpha_i^\circ & = \begin{cases}
    180^\circ - \zeta_i^a & , Robot berada pada kuadran 1 dan 2 \\
    180^\circ + \zeta_i^a & , Robot berada pada kuadran 3 dan 4
  \end{cases}
  \label{eq:kejadian_langkah_dua}
\end{align}

Langkah kedua ini bertujuan untuk menentukan kejadian di Persamaan~\eqref{eq:kejadian_langkah_dua} dimana robot
harus berpindah ke koordinat $A'' = (x*_a , y*_a )$ (Gambar~\ref{fig:strategiPenentuanKoordinat_dua}).
Sebelum robot berpindah, kondisi robot telah mendapatkan koordinat dari langkah pertama. Koordinat tersebut akan diubah
menjadi jarak dan akan dibandingkan jarak tersebut dengan informasi jarak dari sensor setelah
berpindah ke $A''$ . Apabila terdapat perbedaan maka kejadian di Persamaan~\eqref{eq:algo_getAngle} diubah kejadian selanjutnya
dan mengkoreksi koordinat sebelumnya.

\begin{figure}
  \centering
  \includegraphics[scale=.2]{BAB3/img/estimate_coordinate2.png}
  \caption{Strategi Penentuan Koordinat Langakah Kedua}
  \label{fig:strategiPenentuanKoordinat_dua}
\end{figure}

\section{Analisa Algoritama Dengan Tetangga Statis}

Telah dijelaskan pada Bab \ref{bab:empat:Strategi_koordinat_tetangga} bahwa robot bergerak kearah yang random dengan jarak tertentu untuk mengetahui koordinat tetangga.
Analisa akan dilakukan dengan membandingkan berbagai jarak dari tinggat rendah, sedang, dan tinggi untuk mengetahui respon algoritma yang sesuai dan optimal untuk mendapatkan koordinat tetangga.

Dari hasil analisa ini akan menghasilkan jarak terbaik untuk algoritma menentukan koordinat tetangga. Pembuktian akan dilakukan secara menampilkan grafik respon.

\section{Kendali Robot \textit{Holonomic}}
Kendali robot \textit{holonomic} akan dibagi menjadi dua mode.
Perbedaan kedua mode tersebut adalah \textit{setpoint} kendalinya,
dimana mode satu akan memiliki setpoint kecepatan robot sedangkan
mode dua memiliki \textit{setpoint} koordinat kerangka robot.
Penjelasan lebih lengkap kegunaan dari kedua mode tersebut akan dijelaskan
di pembahasan metode strategi penentuan koordinat.


\textbf{Mode Satu}. Kendali robot mode satu bertujuan untuk robot mencapai kecepatan yang
diinginkan. Untuk mencapai tujuan tersebut akan menggunakan metode \textit{state-feedback}
seperti yang telah dijabarkan sebelumnya.
Mengadopsi persamaan \textit{state-space} dari robot.
\begin{align}
  \dot{x_c}(t) & = A_c x_c(t) + B_c u_c(t) + k_c sgn(x_c(t)), \label{eq:ss_kecepatan} \\
  y_c(t)       & = C_cx_c(t),
\end{align}
Dimana $A_c,B_c,k_c,C_c \in \mathbb{R}^{3\times 3}$ adalah matriks parameter robot yang telah dijelaskan sebelumnya.
\begin{align*}
  A_c & = \begin{bmatrix}
    -\frac{3.l^2.K_t^2}{2.M.R_a.r^2}-\frac{B_{\dot{x}_r}}{M} & 0                                                        & 0                                                             \\
    0                                                        & -\frac{3.l^2.K_t^2}{2.M.R_a.r^2}-\frac{B_{\dot{y}_r}}{M} & 0                                                             \\
    0                                                        & 0                                                        & -\frac{3.l^2.K_t^2}{2.I.R_a.r^2}-\frac{B_{\dot{\theta}_r}}{I} \\
  \end{bmatrix}, \\
  B_c & = \begin{bmatrix}
    0                                & \frac{l.K_t}{R_a.r}. \frac{\cos(30^\circ)}{M} & -\frac{l.K_t}{R_a.r}.\frac{\cos(30^\circ)}{M} \\
    \frac{l.K_t}{R_a.r}.\frac{-1}{M} & \frac{l.K_t}{R_a.r}.\frac{\cos(60^\circ)}{M}  & \frac{l.K_t}{R_a.r}.\frac{\cos(60^\circ)}{M}  \\
    \frac{l.K_t}{R_a.r}.\frac{b}{I}  & \frac{l.K_t}{R_a.r}.\frac{b}{I}               & \frac{l.K_t}{R_a.r}.\frac{b}{I}               \\
  \end{bmatrix},
  k_c = \begin{bmatrix}
    -\frac{C_{\dot{x}_r}}{M} & 0                        & 0                        \\
    0                        & -\frac{C_{\dot{x}_r}}{M} & 0                        \\
    0                        & 0                        & -\frac{C_{\dot{x}_r}}{M} \\
  \end{bmatrix},   \\
  C_c & = \begin{bmatrix}
    1 & 0 & 0 & \\
    0 & 1 & 0 & \\
    0 & 0 & 1 & \\
  \end{bmatrix}
\end{align*}
Dimana $y_c(t) = x_c(t) = \begin{bmatrix} v & v_n & w \end{bmatrix}^T$ adalah vektor kecepatan.
Lalu ditentukan fungsi kendali $u_c(t)$ menggunakan \textit{state-feedback}.
\begin{align}
  u_c(t) & = -K_c x_c(t) + N_c r_c   \label{eq:kendali_mode_satu} \\
  N_c    & = -[C(A_c - B_c K_c)^{-1} B_c ]^{-1}
\end{align}
Dimana vektor $r_c = \begin{bmatrix} v* & v_n* & w* \end{bmatrix}$ adalah \textit{set-point} kendali mode satu dan
matriks $K_c$ diperoleh dari solusi persamaan \textit{Riccati}.


\textbf{Mode dua}. Kendali robot mode bertujuan untuk robot mencapai koordinat relatif yang
diinginkan. Untuk mencapai tujuan tersebut akan menggunakan metode \textit{state-feedback}
seperti yang telah dijabarkan sebelumnya.
Mengadopsi persamaan \textit{state-space} dari robot Persamaan\eqref{eq:ss1}-\eqref{eq:ss2}.
Lalu ditentukan fungsi kendali $u(t)$ menggunakan \textit{state-feedback}.
\begin{align}
  u(t) & = -K_r x_c(t) + N_r r_r             \label{eq:kendali_mode_dua} \\
  N_r  & = -[C(A_r - B_r K_r)^{-1} B_r ]^{-1}
\end{align}
Dimana vektor $r_c = \begin{bmatrix} \dot{x*}_r & \dot{y*}_r & \dot{\theta *}_r & v* & v_n* & w* \end{bmatrix}$
adalah \textit{set-point} kendali mode dua dan
matriks $K_r$ diperoleh dari solusi persamaan \textit{Riccati}.

\section{State Feedback}
\begin{figure}
  \centering
  \input{BAB4/img/statefeedback.tex}
  \caption{State-feedback Sistem}
  \label{fig:state-feedback}
\end{figure}
Pada persamaan~\eqref{eq:ss1} diketahui bahwa state memiliki dimensi $6 \times 1$. Dimensi
tersebut tidak menunjukan sistem memiliki orde 6. Apabila diperhatikan orde
dari sistem adalah orde 2. Dengan membaginya kedalam 3 persamaan state-space
akan lebih mudah dalam analisis parameter kendalinya. Berikut adalah persamaannya.

\begin{align}
  \begin{bmatrix}\dot{x}_p \\ \ddot{x}_r \end{bmatrix}  & =
  \begin{bmatrix}0 & A_{14} \\ 0 & A_{44}  \end{bmatrix}
  \begin{bmatrix}{x}_p \\ \dot{x}_r\end{bmatrix} +
  \begin{bmatrix}0 & 0& 0 \\ B_{11} & B_{12} & B_{13} \end{bmatrix}
  \begin{bmatrix}u_1 \\ u_2 \\ u_3 \end{bmatrix} + k_{44}sgn(\dot{x}_r) \label{eq:ssx} \\
  \begin{bmatrix}\dot{y}_p \\ \ddot{y}_r \end{bmatrix}  & =
  \begin{bmatrix}0 & A_{25} \\ 0 & A_{55}  \end{bmatrix}
  \begin{bmatrix}{y}_p \\ \dot{y}_r\end{bmatrix} +
  \begin{bmatrix}0 & 0& 0 \\ B_{21} & B_{22} & B_{23} \end{bmatrix}
  \begin{bmatrix}u_1 \\ u_2 \\ u_3 \end{bmatrix} + k_{55}sgn(\dot{y}_r)\label{eq:ssy} \\
  \begin{bmatrix}\dot{\theta}_p \\ \ddot{\theta}_r\end{bmatrix} & =
  \begin{bmatrix}0 & A_{34} \\ 0 & A_{66}  \end{bmatrix}
  \begin{bmatrix}{\theta}_p \\ \dot{\theta}_r\end{bmatrix} +
  \begin{bmatrix}0 & 0& 0 \\ B_{31} & B_{32} & B_{33} \end{bmatrix}
  \begin{bmatrix}u_1 \\ u_2 \\ u_3 \end{bmatrix} + k_{66}sgn(\dot{\theta}_r) \label{eq:ssthe}
\end{align}

State feedback membutuhkan kembalian nilai state dari sistem dan mengka-
likanya dengan besaran tertentu agar nilai karakteristik sistem tetap dalam keadaan
stabil atau sesuai ketentuan. Secara umum, state tidak dapat diperoleh langsung dari
sistem. Kemampuan untuk memperoleh state dari sistem langsung disebut dengan
kemampuan Observablity. Apabila sebuh sistem tidak Observable, maka dalam
kendalinya dibutuhkan Observer. Dimana tugasnya adalah mengestimasi state pada
sistem dengan membandingkan keluaran dan masukan. Syarat untuk dapat diterap-
kan state feedback, sistem harus observable dan controlable. Berikut adalah rumus
untuk menguji apakah sistem bersifat controlable atau tidak (Dorf, dkk (2010)).
\begin{align*} P_c = \begin{bmatrix} B & AB & A^2B & \dots & A^{n-1}B \end{bmatrix}\end{align*}
\begin{align} rank[P_c] = n \end{align}
Apabila hasil dari $rank(P_c ) \neq n$ maka sistem tidak \textit{fully controlable}. Sedangkan untuk
menguji observabilitas dapat menggunakan rumus berikut.
\begin{align*} P_o = \begin{bmatrix} C \\ CA \\ CA^2 \\ \vdots \\ CA^{n-1} \end{bmatrix}\end{align*}
\begin{align} rank[P_o] = n \end{align}
Apabila sistem observable, rank dari matriks Observablity $P_o$ sama dengan besar orde sistem.
Menggunakan parameter robot pada Tabel~\ref{tab:param_model} untuk diimplementasi pada persamaan~\eqref{eq:ss1}-\eqref{eq:ss2} akan menghasilkan $rank[P_c]=6$ dan $rank[P o] = 6$ .
Maka dari itu dapat disimpulkan sistem robot adalah controlable dan observable.
Karena pengukuran pada setiap \textit{state} dapat dilakukan,
maka \textit{observer} tidak dibutuhkan dalam desain kendali robot.

\begin{table}
  \caption{Parameter model oleh~\kutip{CORREIA20127}}
  \label{table:parameter_model}
  \begin{center}
    \begin{tabular}{| c | c | c |}
      \hline
      Simbol                & Deskripsi                                        & Nilai      \\ \hline
      $B_v (N/m/s)$         & viscous friction coefficient related to v        & 0.94       \\
      $B_{vn} (N/m/s)$      & viscous friction coefficient related to vn       & 0.96       \\
      $B_\omega (N/rad/s)$  & viscous friction coefficient related to $\omega$ & 0.01       \\
      $C_v (N )$            & coulomb friction coefficient related to v        & 2.2        \\
      $C_{vn} (N )$         & coulomb friction coefficient related to vn       & 1.5        \\
      $C_\omega (N.m)$      & coulomb friction coefficient related to $\omega$ & 0.099      \\
      $b(m)$                & radius of the robot                              & 0.1        \\
      $M (kg)$              & mass of the robot                                & 1.5        \\
      $In(kg.m^2 )$         & inertia moment of the robot                      & 0.025      \\
      $\delta$              & angle                                            & $30^\circ$ \\
      $r_1 , r_2 , r_3 (m)$ & radius of the wheels                             & 0.035      \\
      $l_1, l_2, l_3$       & reduction of the motors                          & 19:1       \\
      $L_{a1...3} (H)$      & motor’s armature inductance                      & 0.00011    \\
      $R_{a1...3} (\Omega)$ & motor’s armature resistance                      & 1.69       \\
      $K_v (V olts/rad/$s)  & motor’s emf constant                             & 0.0059     \\
      $K_{t1...3}$ (N.m/A)  & motor’s torque constant                          & 0.0059     \\
      \hline
    \end{tabular}
  \end{center}
  \label{tab:param_model}
\end{table}
\section{Desain Kendali}

Berdasarkan \kutip{Richard2010}, bahwa kendali optimal berdasarkan indeks kinerja sistem. Indeks tersebut adalah hasil dari meminimalisasi pada integral kuadrat error atau \textit{integration square error} (ISE).
Kendali optimal dilakukan oleh komputer untuk mengkalkulasi minimal indeks tersebut.
Apabila sebuah sistem \textit{state space}
\begin{align*}
  \dot{x} & = Ax + Bu \\
  u       & = -K_sx
\end{align*}
maka indeks kinerja
\begin{align}
  J & = \int_0^{\infty} (x^T Q x + u^TRu ) dt
\end{align}
dimana $Q$ adalah matriks diagonal $n \times n$, $R$ adalah matriks diagonal $m \times m$ dan keduanya adalah matriks pembobot terhadap state sistem dan input.
Ketika indeks terminimalisasi, maka
\begin{align}
  K_s = R^{-1}B^TP
\end{align}
dengan matriks P $n \times n$ ditentukan dari solusi persamaan \textit{Riccati}.
\begin{align}
  A^TP+PA-PBR^{-1}B^TP+Q=0
\end{align}
% here is it KST from octave
% 0.00000  -21.08185    3.33333    0.00000   -3.74993    0.99738
% 18.25742   10.54093    3.33333    3.25168    1.87496    0.99738
% -18.25742   10.54093    3.33333   -3.25168    1.87496    0.99738
Kalkulasi konstanta $K_s$ akan dikalkulasi menggunakan persamaan-\eqref{eq:ssx}~\eqref{eq:ssthe}.
Sehingga konstanta $K_s$ akan terbagi dalam sub matriks $K_{sx}, K_{sy},$ dan $K_{s\theta}$.
Berikut adalah hasil kalkulasi.
\begin{align*}
  K_{s}^{x}      & = \begin{bmatrix}
    0.00000   & 0.00000  \\
    18.25742  & 3.25168  \\
    -18.25742 & -3.25168 \\
  \end{bmatrix} = \begin{bmatrix} K_{s}^{x1} & K_{s}^{x2} \end{bmatrix} \\
  K_{s}^{y}      & = \begin{bmatrix}
    -21.08185 & -3.74993 \\
    10.54093  & 1.87496  \\
    10.54093  & 1.87496  \\
  \end{bmatrix} = \begin{bmatrix} K_{s}^{y1} & K_{s}^{y2} \end{bmatrix} \\
  K_{s}^{\theta} & = \begin{bmatrix}
    3.33333 & 0.99738 \\
    3.33333 & 0.99738 \\
    3.33333 & 0.99738 \\
  \end{bmatrix} = \begin{bmatrix} K_{s}^{\theta 1} & K_{s}^{\theta 2} \end{bmatrix}
\end{align*}
Apabila diintegrasi terhadap persamaan~\eqref{eq:ss1} terhadap diagram~\ref{fig:state-feedback}
\begin{align}
  K_s = \begin{bmatrix}
    K_{s}^{x1} & K_{s}^{y1} & K_{s}^{\theta 1} &
    K_{s}^{x2} & K_{s}^{y2} & K_{s}^{\theta 2}
  \end{bmatrix}
\end{align}

Setelah mendapatkan konstanta $K_s$, sistem sudah dalam keadaan stabil.
Akan tetapi sistem tidak mencapai set poin yang diinginkan.
Maka permasalahan tersebut dapat diselesaikan dengan \textit{input refrence}.
Dimana refrence tersebut akan dikalikan dengan konstanta $N$.
Berikut adalah persamaan \textit{input refrence} sebagai penambah dari \textit{state feedback}.
\begin{align}
  u(t) = -Kx(t)+Nr
\end{align}
Sehingga persamaan \textit{state space} menjadi berikut.
\begin{align}
  \begin{cases}
    \dot{x} & = (A-BK_s)x + BNr \\
    y       & = Cx
  \end{cases}
  \label{eq:ss-control-robot}
\end{align}
Untuk mendapatkan nilai $N$ maka dapat diasumsikan bahwa sistem dalam keadaan \textit{steady state}, yaitu $\dot{x} = 0$, sehingga persamaan state space menjadi berikut.
\begin{align}
  x & = -(A-BK_s)^{-1}BNr
\end{align}
Dalam keadaan \textit{steady state}, harapannya adalah nilai refrence sama dengan nilai keluaran, $y=r$.
Sehingga dapat diperoleh persamaan $N$.
\begin{align}
  N & = -[C(A-BK_s)^{-1}B]^{-1}
\end{align}
Berikut adalah hasil kalkulasi dari rumus $N$ menggunakan matriks pada persamaan~\eqref{eq:ssx},\eqref{eq:ssy}, dan \eqref{eq:ssthe}.

% 0.00000  -21.08185    3.33333   0.00000   0.00000   0.00000
% 18.25742   10.54093    3.33333   0.00000   0.00000   0.00000
% -18.25742   10.54093    3.33333   0.00000   0.00000   0.00000
\begin{align*}
  N^x      & = \begin{bmatrix}
    0.00000   & 0.00000 \\
    18.25742  & 0.00000 \\
    -18.25742 & 0.00000
  \end{bmatrix} = \begin{bmatrix}N^{x1} & N^{x2}\end{bmatrix} \\
  N^y      & = \begin{bmatrix}
    -21.08185 & 0.00000 \\
    10.54093  & 0.00000 \\
    10.54093  & 0.00000
  \end{bmatrix} = \begin{bmatrix}N^{y1} & N^{y2}\end{bmatrix} \\
  N^\theta & = \begin{bmatrix}
    3.33333 & 0.00000 \\
    3.33333 & 0.00000 \\
    3.33333 & 0.00000
  \end{bmatrix} = \begin{bmatrix}N^{\theta 1} & N^{\theta 2}\end{bmatrix} \\
\end{align*}
Apabila diintegrasi terhadap persamaan~\eqref{eq:ss1} terhadap diagram~\ref{fig:state-feedback}
\begin{align}
  N = \begin{bmatrix}
    N^{x1} & N^{y1} & N^{\theta 1} &
    N^{x2} & N^{y2} & N^{\theta 2}
  \end{bmatrix}
\end{align}

\begin{figure}
  \begin{center}
    \begin{subfigure}[t]{.3\linewidth}
      \dataGraph{t}{x}{t}{x}{BAB4/data_response.csv}
    \end{subfigure}
    \begin{subfigure}[t]{.3\linewidth}
      \dataGraph{t}{y}{t}{y}{BAB4/data_response.csv}
    \end{subfigure}
    \begin{subfigure}[t]{.3\linewidth}
      \dataGraph{t}{theta}{t}{$\theta$}{BAB4/data_response.csv}
    \end{subfigure}
    \begin{subfigure}[t]{.3\linewidth}
      \dataGraph{t}{dx}{t}{$\dot{x}$}{BAB4/data_response.csv}
    \end{subfigure}
    \begin{subfigure}[t]{.3\linewidth}
      \dataGraph{t}{dy}{t}{$\dot{y}$}{BAB4/data_response.csv}
    \end{subfigure}
    \begin{subfigure}[t]{.3\linewidth}
      \dataGraph{t}{dtheta}{t}{$\dot{\theta}$}{BAB4/data_response.csv}
    \end{subfigure}
    \captionsetup{singlelinecheck=off}
    \caption[.]{\centering Grafik state terhadap t dengan state refrensi $ r = \begin{bmatrix}6 & -3 & -90 &0 &0 &0\end{bmatrix} $ }
  \end{center}
\end{figure}

\section{Kendali Formasi Multi Robot}
Pada sub bab~\ref{subbab:KendaliFormasi} dijabarkan bagaimana kendali formasi menggunakan
kendali-P dan menghasilkan persamaan~\eqref{eq:ss-formasi}.
Persamaan tersebut adalah persamaan \textit{state-space} kendali formasi.
Apabila diperhatikan \textit{state} yang digunakan adalah koordinat relatif dari robot.
Akan tetapi dalam batasanya, robot hanya bisa mengetahui nilai jarak dari robot lain.
Dengan kata lain, yang dibutuhkan dalam metode kendali formasi adalah jarak dalam bentuk koordinat,
$x \in \mathbb{R}^2$. Sedangkan dalam kenyataanya yang diketahui adalah jarak, $r \in \mathbb{R}$.
Apabila hanya variable jarak tersebut sebagai acuan kendali, maka robot tidak mengerti kearah mana
harusnya robot itu bergerak untuk meminimalisasi error jaraknya.

Di Persamaan~\eqref{eq:ss-formasi} adalah persamaan \textit{state-space} yang menggunakan model sederhana
di Persamaan~\eqref{eq:modelorde2}.
Apabila diperhatikan pada model tersebut masukan pada perseamaan tersebut berbentuk kecepatan.
Menggunakan Persamaan~\eqref{eq:ss_kecepatan} dan~\eqref{eq:kendali_mode_dua} akan diperoleh persamaan \textit{state-space} baru
\begin{align}
  \dot{x_c}(t) & = \Big(\big(A_c-B_cK_c + \big)\Big) x_c(t) K_{cf} sgn(x_c(t))+ (B_c N_c) r_c ,
\end{align}
Lalu diterapkan di Persamaan~\eqref{eq:ss-formasi} sebgai berikut.
\begin{align}
  \dot{p} & = A_{cf} p(t) + B_{cf} v(t) + K_{cf} sgn(p(t))         \\
  \dot{v} & = -k_{p1}v_i(t) -R(p(t))^T k_{p2}(R(p(t))x_1(t) - d )) \\
  A_{cf}  & = \begin{bmatrix}
    A_c-B_cK_c & 0          & 0          \\
    0          & A_c-B_cK_c & 0          \\
    0          & 0          & A_c-B_cK_c \\
  \end{bmatrix};
  B_{cf}  = \begin{bmatrix}
    (B_c N_c) \\ (B_c N_c) \\ (B_c N_c) \\
  \end{bmatrix}; \notag                     \\
  K_{cf}  & = \begin{bmatrix}
    k_c & 0   & 0   \\
    0   & k_c & 0   \\
    0   & 0   & k_c \\
  \end{bmatrix}
\end{align}