-p-formation-control/BAB4/bab4.tex

\chapter{\babEmpat}

\section{Perangkat Percobaan}
Sub bab ini akan dibahas mengenai prangkat penunjang sebagai pembatu dalam menyelesaikan penelitian.
Sebagai langkah awal pengembangan, metode yang digunakan adalah \textit{Hardware-In Loop}.
\begin{figure}
\centering
  \includegraphics[scale=.5]{BAB3/img/hil_graph.png}
  \caption{Hardware-in-the-loop (\kutip{Jim1999}). }
  \label{fig:hil_graph}
\end{figure}

\textit{Hardware-in-the-loop} (HIL) adalah metode untuk pengembangan prangkat kendali dengan memanfaatkan model sebagai objek kendalinya. Seperti pada gambar~\ref{fig:hil_graph},
bahwa HIL terdiri dari dua prangkat, yaitu prangkat untuk menjalankan objek kendali atau dapat
disebut sebagai model/plant dan prangkat sistem kontrolnya, dalam kasus ini sistem kontrol menggunakan sistem tertanam (\textit{embedded system}).
Metode HIL, banyak digunakan oleh peneliti dalam proses pengembangan dengan pertimbangan efisiensi terhadap berbagai hal.
Seperti yang digunakan oleh~\kutip{Irwanto2018}, mengembangkan kendali UAV menggunakan HIL;
dan \kutip{QUESADA2019275}, mengembangkan prangkat pankreas buatan yang digunakan untuk mengendalikan kadar gula pada pengidap diabetes.


Pada penelitian ini akan digunakan \textit{microcontroller}(MCU) STM32F466 sebagai prangkat kendalinya.
MCU tersebut ber-arsitektur ARM Cortex-M4 dengan clock 180MHz, menampung ukuran program sampai 256K didalam memori Flash, serta fitur komunikasi standart MCU dengan lengkap.
\textit{Platform Library} yang digunakan dalam pembuatan aplikasi didalamnya adalah \textit{Mbed},
yang menyediakan berbagai banyak fungsi yang lengkap dan mudah untuk berinteraksi dengan fitur-fitur MCU. \textit{Mbed} juga menyediakan fungsi untuk mengaplikasikan RTOS (Real-time Operating System) dengan mudah dan terdokumentasi secara jelas didalam lamannya.
Pada prangkat PC akan dikembangkan program berbasis \textit{Python} yang akan
menjalankan simulasi model dan berkomunikasi dengan MCU secara \textit{real-time}.
Program \textit{Python} akan menjalankan model pada persamaan~\eqref{eq:ss1}-\eqref{eq:ss2}
dengan metode yang dijabarkan pada sub bab~\ref{bab:dua:solusi_ODE}.
Dalam penerapan multi-robot, digunakan 3 perangkat sistem tertanam untuk mempresentasikan kendali 3 robot (Gambar.~\ref{fig:hil_graph_1}). 
Setiap prangkat pengendali akan saling terhubung satu sama lain dan semua prangkat pengendali terhubung dengan prangkat PC.
Komunikasi antar prangkat pengendali akan digunakan untuk pertukaran informasi.
Sedangkan komunikasi dengan PC akan mempresentasikan aktuator dan sensor untuk setiap prangkat
kendali. PC akan merekam setiap keluaran dari model dan masukan dari setiap prangkat kendali
sebagai tampilan pergerakan robotnya.

\begin{figure}
\centering
  \input{BAB4/img/Diagram_hil_controller.tex}
  \caption{HIL Kendali Multi-Robot.}
  \label{fig:hil_graph_1}
\end{figure}

% \todo{
%    Tambahkan subsection mengenai
%    \begin{itemize}
%       %% \item pengembangan data/akuisisi data ?
%       % \item skenario pengujian/simulasi? (lebih ke teknis seperti lapangan environtment dll)
%       % \item skenario Analisa hasil
%       \item jadwal penelitian
%    \end{itemize}
% }

\section{Strategi Kendali Multi Robot}

Pembahasan strategi akan dibagi menjadi dua bagian, yaitu kendali tingkat bawah dan kendali tingkat atas.
Kendali tingkat bawah akan membahas mengenai kendali robot secara individu, sedangkan kendali tingkat atas akan dibahas mengenai penggabungan antara kendali tingkat bawah dan atas.

\subsection{Kendali Robot}
Pada kendali robot akan dibahas mengenai analisis kendali robot menggunakan
state-space feedback. Kendali robot ini adalah kendali tingkat bawah dari kendali ke-
seluruhan. Dapat diperhatikan pada persamaan~\eqref{eq:ss-formasi}, sebagai kendali tahap awal,
bahwa state yang digunakan adalah koordinat. Maka koordinat tersebut akan men-
jadi set point bagi robot. Variable yang dikendalikan pada kendali robot adalah
koordinat robot dari kondisi inisial. Koordinat disini adalah koordinat state pada
persamaan~\eqref{eq:ss1}. Pada sub bab ini akan didefinisi mengenai kriteria pencapaian
set point dan membahas parameter kendali state-space feedback agar mencapai kri-
teria yang diinginkan.

\subsubsection{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 persamaan

\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}
\subsubsection{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}

\subsection{Kendali Formasi Multi Robot}
Pada sub bab~\ref{subbab:KendaliFormasi} dijabarkan bagaimana kendali formasi menggunakan
kendali-PI 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.

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

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=.5]{BAB3/img/estimate_coordinate.png}
   \caption{Strategi Penentuan Koordinat}
   \label{fig:strategiPenentuanKoordinat}
\end{figure}

Dapat diperhatikan pada gambar~\ref{fig:strategiPenentuanKoordinat} untuk gambaran strateginya.
Robot $B \in \tetangga_A$, adalah tetangga dari robot $A$.
Pertama-tama, sebelum robot $A$ bergerak, disimpan terlebih dahulu nilai $d_a$,
atau dinotasikan dengan $d_a[k]$ sebagai jarak sebelum bergerak.
Lalu robot $A$ berjalan secara random kesegala arah dengan jarak $l_a$.
Disimpan kembali nilai jara $d_a$, atau dinotasikan dengan $d_a[k+1]$.
Setalah itu dapat ditentukan sudut $\alpha[k+1]$
\begin{align}
   \alpha[k+1] = cos^{-1}\Bigg[ \frac{l_a^2 + d_a[k+1]^2 -d_a[k]^2}{2d_a[k+1]l_a} \Bigg]
   \label{eq:algo_getAngle}
\end{align}
Sebelum $\alpha[k+1]$ digunakan, jarak $d_a[k+1]$ dan $d_a[k]$ berpengaruh dalam penentuan koordinat.
Sehingga diperlukan sedikit algoritma
\begin{align}
   \alpha_i=
   \begin{cases}
      \alpha[k+1]     & ,d_a[k+1] > d_a[k] \\
      180-\alpha[k+1] & ,d_a[k+1] < d_a[k]
   \end{cases}.\label{eq:init_relatif_koordinat}
\end{align}

Strategi pada gambar~\ref{fig:strategiPenentuanKoordinat} hanya berlaku apabila target ukur berhenti. Apabila dinotasikan koordinat $(x_B^A, y_B^A)$ adalah koordinat relatif robot $B$ terhadap $A$,
maka $(\dot{x}_B^A, \dot{y}_B^A)$ adalah notasi kecepatan koordinat dari robot B.
Dengan menggunakan persamaan~\eqref{eq:kinematika_robot} untuk menyelesaikan koordinat dalam
keadaan robot $B$ bergerak, yaitu mengirimkan informasi kecepatan koordinatnya
ke robot $A$. Lalu robot $A$ dapat mengkalkulasi koordinat relatif dengan persamaan berikut
\begin{align}
   \alpha[k+1] & = \alpha[k]+tan^{-1} \Big[ \frac{\dot{x}_B^A}{\dot{y}_B^A} \Big]
\end{align}
dimana kondisi inisial adalah $\alpha[k] = \alpha_i$ diperoleh dari hasil strategi pada persamaan~\eqref{eq:init_relatif_koordinat}.
Dengan memanfaatkan kedua strategi tersebut dapat digunakan untuk
mengkalkulasi koordinat robot $B$ relatif terhadap robot $A$
\begin{align}
   x_B^A = \begin{bmatrix}
      x_B = d_a[k]\cos \alpha[k] \\
      y_B = d_a[k]\sin \alpha[k]
   \end{bmatrix}
\end{align}
Dalam strategi ini akan terjadi ketidak akuratan dalam pengukuran apabila target ukur
berada pada sudut $90^\circ$.
Akan tetapi, \kutip{Cao2007} sudah menjelaskan mengenai kriteria posisi agent ketika dalam kondisi inisial.
Yaitu semua agent tidak berada pada kondisi sejajar secara koordinat global.

\begin{algorithm}
  \DontPrintSemicolon
  \KwInput{
  Integer $l_a>0$,
  $\tetangga_i=getConnectionRobot()$, }
  \KwOutput{$x_i^j$}

  \tcc{inisialisasi}
  \tcc{getRandomDirection() anak mengembalikan sudur random antara 0 - 360}
  $dir = getRandomDirection()$\;
  $d_{before} = getDistanceFromSensor(\tetangga_i)$\;
  $r = \begin{bmatrix}
      l_a \cos(dir) \\
      l_a \sin(dir)
   \end{bmatrix}$\;

  \tcc{Menjalankan robot hingga mencapai setpoint}
  \While{isSetpointReached()}{
    $runRobotToSetpoint(r)$\;
  }

  \tcc{Mengambil jarak setelah robot mencapai setpoint}
  $d_{after} = getDistanceFromSensor(\tetangga_i)$\;

  \tcc{Mengkalkulasi sudut}
  $ang = cos^{-1}\Bigg[ \frac{l_a^2 + d_{after}^2 -d_{before}^2}{2d_{before}l_a} \Bigg]$\;
  \If{$d_{before}<d_{after}$}
  {
    $ang = 180-ang$\;
  }
  \tcc{Menjadikan koordinat kartesian}
  $x_i^j = \begin{bmatrix}
      d_{after} \cos(ang) \\
      d_{after} \sin(ang)
   \end{bmatrix}$\;
  \caption{\textit{Algoritma inisialisasi}}
  \label{algo:solution_initialitation}
\end{algorithm}
\subsubsection{Implementasi Kendali Formasi Dengan Kendali Robot}

Implementasi kendali formasi pada kendali robot akan menggabungkan persamaan state space pada~\eqref{eq:ss-formasi} dengan persamaan~\eqref{eq:ss-control-robot}.
Implementasi ini akan menjadikan persamaan~\eqref{eq:ss-formasi} sebagai kendali utama sedangkan pada persamaan~\eqref{eq:ss-control-robot} adalah kendali tingkat bawah.
Kendali utama akan diberikan input berupa jarak, $d$, sebagai tujuan pengendali. Sedangkan keluaran dari kendali tersebut adalah koordinat yang perlu dicapai oleh kendali tingkat bawah.
Pembahasan akan diambil dari sudut pandang robot secara individual.
Dengan menulis ulang persamaan~\eqref{eq:ss-control-robot} dengan notasi baru akan lebih mudah dalam penjelasaan lebih lanjut.
\begin{align}
  \begin{cases}
    \dot{x_r} & = (A_r-B_rK_s)x_r + B_rN_rr \\
    y_r       & = C_rx_r
  \end{cases}
  \label{eq:ss-control-robot-implement}
\end{align}
Apabila diasumsikan state pada persamaan~\eqref{eq:ss-formasi} sebagai berikut
\begin{align*}
  x(t) &= \begin{bmatrix}
    y_r^T & y_{rj1}^T &  y_{rj2}^T & \dots & y_{rjn}^T
    \end{bmatrix}^T , \quad (i,jn) \in \sisi
\end{align*}
maka $y_{rjn}$ adalah koordinat yang didapat dari algoritma atau hasil pengiriman data dari robot tetangga.
Sedangkan $r$ pada persamaan~\eqref{eq:ss-control-robot-implement} adalah koordinat refrensi yang dihasilkan dari persamaan~\eqref{eq:ss-formasi}, $r = C_fx(t)$. Untuk lebih mudah dalam memahami dapat diperhatikan diagram pada gambar~\ref{fig:all-control}

\section{Strategi Uji Coba}
Strategi ujicoba akan diawali dengan menghitung kesetabilan menggunakan teori Euler pada Bab \ref{bab:dua:solusi_ODE}. 
Langkah selanjutnya adalah percobaan terhadap Algoritma~\ref{algo:solution_initialitation} dengan kondisi robot tetangga dalam keadaan statis. 
Percobaan tersebut bermaksut untuk menguji apakah algoritma berjalan dengan benar.
Langkah terakhir adalah percobaan keseluruhan robot menggunakan Algoritma yang dikembangkan
dengan skenario percobaan yang sama dengan penelitian sebelumnya oleh \kutip{Rozenheck2015}. 

\begin{figure}
   \centering
   \input{BAB4/img/implement-control.tex}
   \caption{Kendali keseluruhan}
   \label{fig:all-control}
\end{figure}

\subsection{Analisa Kesetabilan Model}
Perhitungan kesetabilan akan menggunakan parameter model dari penelitian sebelumnya, dapat diperhatikan di Tabel \ref{tab:param_model}.
Parameter tersebut akan diimplementasi di persamaan~\eqref{eq:ss1}-\eqref{eq:ss2}.
Lalu ditentukan waktu sample dan diubah menjadi persamaan diskrit meggunakan persamaan~\eqref{eq:disstab}. 
Setelah itu akan diuji menggunakan diagram Gambar.\ref{fig:explicit_euler} untuk mengetahui kesetabilan model.

Dari hasil analisa ini akan menghasilkan parameter waktu sampling ($h$) untuk menjalankan Algoritma~\ref{algo:eEuler}.
Pembuktian secara grafik sangat dibutuhkan untuk mengetahui respon model.

\subsection{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.

\subsection{Analisa Percobaan Keseluruhan}

Percobaan keseluruhan akan menjalankan Algoritma \ref{algo:solution_initialitation} yang diterapkan pada seluruh robot. Lalu, seperti skenario penelitian sebelumnya salah satu robot diberikan kecepatan refrensi untuk bergerak ke arah tertentu.

Dari hasil percobaan ini akan menghasilkan grafik respon dari keseluruhan robot terhadap perubahan error yang terjadi. 
Dengan hipotesis, keseluruhan robot akan menjaga jarak formasi dengan baik.

\subsection{Jadwal Penelitian}
% \todo{
% buat draftnya dan pakek library ini 
% \url{
% http://www.martin-kumm.de/wiki/doku.php?id=05Misc:A_LaTeX_package_for_gantt_plots
% }
% }
\begin{enumerate}
  \item Analisa kestabilan model 
    \begin{enumerate}
      \item Pengembangan dan test Individu Model(1)
    \end{enumerate}
  \item Analisa algoritma dengan tetangga statis 
    \begin{enumerate}
      \item Pengembangan dan test Kendali (1)
      \item Pengembangan dan test Koneksi antara Individu Model dan Kendali (2)
      \item Pengembangan dan test Algoritma dengan tetangga yang statis (2)
    \end{enumerate}
  \item Analisa algoritma keseluruhan 
    \begin{enumerate}
      \item Pengembangan dan test Algoritma komunikasi antar kendali (3)
      \item Pengembangan dan test Algoritma keseluruhan (3)
    \end{enumerate}
\end{enumerate}
\begin{gantt}{7}{12}
  \begin{ganttitle}
  \numtitle{1}{1}{12}{1}
  \end{ganttitle}
  \ganttbar{1.a}{0}{1}
  \ganttbarcon{2.a}{1}{1}
  \ganttbarcon{2.b}{2}{2}
  \ganttbarcon{2.c}{4}{2}
  \ganttbarcon{3.a}{6}{3}
  \ganttbarcon{3.b}{9}{3}
  % \ganttbar{task 1}{0}{2}
  % \ganttbarcon{task 2}{2}{4}
  % \ganttbarcon{task 3}{8}{2}
  % \ganttmilestone[color=cyan]{task 4}{4}
  % \ganttbar{task 5}{2}{2}
  % \ganttbar[color=cyan]{task 6}{4}{4}
  % \ganttbar{task 7}{4}{2}
  % \ganttcon{4}{5}{4}{7}
  % \ganttmilestonecon{task 8}{7}
  % \ganttbarcon{task 9}{8}{2}
\end{gantt}


%% \subsection{