# PIDA: Smooth and Stable Flight Using Stochastic Dual Simplex Algorithm and
Genetic Filter

Seid Miad Zandavi, Vera Chung, Ali Anaissi Seid M. Zandavi, Vera Chung and Ali
Anaissi are with the School of Computer Science, The University of Sydney,
Sydney NSW 2006 Australia e-mails: {miad.zandavi, vera.chung,
<EMAIL_ADDRESS>
###### Abstract

This paper presents a new Proportional-Integral-Derivative-Accelerated (PIDA)
control with a derivative filter to improve quadcopter flight stability in a
noisy environment. The mathematical model is derived from having an accurate
model with a high level of fidelity by addressing the problems of non-
linearity, uncertainties, and coupling. These uncertainties and measurement
noises cause instability in flight and automatic hovering. The proposed
controller associated with a heuristic Genetic Filter (GF) addresses these
challenges. The tuning of the proposed PIDA controller associated with the
objective of controlling is performed by Stochastic Dual Simplex Algorithm
(SDSA). GF is applied to the PIDA control to estimate the observed states and
parameters of quadcopters in both attitude and altitude. The simulation
results show that the proposed control associated with GF has a strong ability
to track the desired point in the presence of disturbances.

###### Index Terms:

Drone, Control, PIDA, SDSA, Genetic Filter.

##  I Introduction

Control is exciting areas of research in self-driving/autonomous system, as
many engineering applications required this factor. Recently, unmanned aerial
vehicles (UAVs) have gained the attention of many researchers working in
different applications, such as search and rescue, delivery, and crowdsourcing
[1, 2]. UAVs or drones have been developed in many areas, including robotics,
control, path planning, and communication [3, 4, 5, 6]. The current attention
to increasing the usability of drones in many commercial and civil
applications inspires researchers to make this dynamic system more
controllable. In particular, quadcopters are popular drones due to their
performance in terms of vertical take-off, landing, their simple and stable
structures. However, their instability, unstable dynamics, non-linearity, and
cross-coupling make this system an interesting underactuated system [7].
Generally, a quadcopter has six degrees of freedom, although four rotors
should control all directions. This causes cross-coupling between rotation and
translational motions [8]. Therefore, the nonlinear dynamics needs to be
managed by the controller.

In the past, classical control techniques were applied to address autonomous
formulations. The main issue is to formulate an accurate model describing a
dynamic system. This means that any changes and modifications in the system,
such as uncertainties in both model and environment, affect the performance of
the controller, making it necessary to update the controller parameters. Thus,
when the system’s dynamics and its operation through the environment change,
its control parameters and features require re-tuning [9, 10, 11].

Various control algorithms have been developed to manage the non-linearity of
the quadrotor. For example, command-filtered Proportional-Derivative (PD) [12]
or Proportional-Integral-Derivative (PID) control [13], integral predictive
control [14], and optimal control [15, 16] have been applied. The Sliding Mode
Control (SMC) is another common control algorithm that is used to improve
performance in terms of stability due to the influence of modeling errors and
external disturbances [5, 17, 18, 19]. Note that the chattering effect in the
SMC arises in the steady state, where it simulates unmodeled frequencies of
the system dynamics.

Of these controllers, PID is preferred due to its simplicity and ability to
adapt to unknown changes. These fast and simple features make the PID control
strategy efficient and versatile in robotics, although it causes wide
overshoot and large settling time [20]. Initially, the parameters of a PID
controller, called gains, were set by the expertise in obeying certain rules,
such as investigating its step responses, Bode plots and Nyquist diagrams.
However, when the complex environment changes and affects the dynamics of the
quadcopter, the PID parameters must to be re-tuned and it is essential to
consider uncertainties in formulating the model. In this regard, uncertainties
and stochastic processes in the dynamic system can be modeled as color noise
and white noise. Thus, the derivative term is able to cope the effect of
disturbances.

To manage unstable systems, derivative plays a significant role in improved
control loop performance. Mathematically, the derivative terms in PID
controller open an avenue for more actions when the error (i.e., following the
desired response) is fluctuating wildly. Thus, an additional derivative term
(i.e., zero) can decrease the size of overshooting [21]. This can improve
controllability. Additionally, this derivative term supports a better response
in terms of speed and smoothness, where limiting overshoot and settling time
in an acceptable bound are considered.

In addition to the control, integrated estimation of states and parameters
plays an important role in improving the performance of the quadcopter in the
presence of uncertainties and measurement noise [22]. Two different categories
of classical and heuristic [23] filters have been used to address the state
estimation problem. For example, Kalman Filter (KF) [24], Extended Kalman
Filter (EKF) [25], Unscented Kalman Filter (UKF) [26] are classical filters,
and Particle Filter (PF) [27], Simplex Filter (SF) [28] and Genetic Filter
(GF) [23] are introduced as heuristic filters [23].

Heuristic filters work based on point mass (or particle) representation of the
probability densities [29]. Unlike UKF, PF represents the required posterior
Probability Density Function (PDF) by a set of random samples instead of
deterministic ones. Also, it uses a resampling process to reduce the
degeneracy of particles. The standard resampling process copies the important
particles and discards insignificant ones based on their fitness. This
strategy suffers from the gradual loss of diversity among the particles, known
as sample impoverishment. Researchers have proposed different resampling
strategies such as Binary Search [30], systematic resampling [31] and residual
resampling [29]. Some heuristic optimization algorithms have also been
inserted to PF to improve its performance. For example, SF [28] utilizes
Nelder-Mead simplex approach for state estimation. GF [23] was utilized a
genetic algorithm scheme, and its operators to estimate the state of dynamic
systems.

In this paper, the new accelerated PID controller with derivative filter
associated with GF is proposed to make an unstable quadcopter track the
desired reference with the proper stability. GF is utilized to estimate the
height and vertical velocity of the modeled dynamic system (i.e., the
quadcopter) while hovering. Consequently, the mathematical model of the
dynamic system is provided and considers non-linearity, instability, cross-
coupling among different modes (i.e., pitch, roll, and yaw), and the uncertain
environment. The controller parameters are tuned using the Stochastic Dual
Simplex Algorithm (SDSA) optimization algorithm [32], which improves the
trade-off between exploration and exploitation to achieve better optimal
parameters for the proposed controller.

This paper is organized as follows. Section II describes the mathematical
model of the dynamic system. The proposed controller is introduced in Section
III. Stability analysis is presented in Section IV. Optimization and heuristic
filter are explored in Section V. Numerical results and discussion are given
in Section VI. Finally, the paper ends with the conclusion in Section VII.

##  II Dynamic Model

The mathematical model of a system can be used as the first step to study its
performance. In this regard, the quadcopter studied in this paper is modeled
in Fig. 1, considering earth-centered inertia (ECI) and body frame. Thus,
$X_{E}=[x_{E},y_{E},z_{E}]^{T}$ and $X_{B}=[x_{B},y_{B},z_{B}]^{T}$ are
defined as transformational motions from inertia frame to body frame due to
having an accurate dynamic model.

Figure 1: Earth Fixed and Body Fixed coordinate systems

The attitude of the quadcopter is formulated based on the Eular angles roll,
pitch, and yaw, which are rotated from the x-axis, y-axis and z-axis,
respectively. Thus, the Eular angles are $\Theta=[\phi,\theta,\psi]^{T}$, and
the angular velocity in the body frame is
$\dot{\Theta}=[\dot{\phi},\dot{\theta},\dot{\psi}]^{T}$. In this sense, the
angular velocity in inertia ($\omega=[p,q,r]^{T}$) is formulated as follows:

$\omega=\left[{\begin{array}[]{ccc}1&0&-\sin(\theta)\\\
0&\cos(\phi)&\cos(\theta)\sin(\phi)\\\
0&-\sin(\phi)&\cos(\theta)\cos(\phi)\end{array}}\right]\cdot\dot{\Theta}$ (1)

Total torques are caused by three segments: thrust forces ($\tau$), body
gyroscopic torque (${\tau}_{b}$) and aerodynamic friction (${\tau}_{a}$). In
addition, each component of the torque vector
($\tau=[{\tau}_{\phi},{\tau}_{\theta},{\tau}_{\psi}]^{T}$), corresponding to a
rotation in the roll, pitch, and yaw axis, can be determined by Eqs (2)–(4):

${\tau}_{\phi}=l(F_{2}-F_{4})$ (2)

${\tau}_{\theta}=l(F_{3}-F_{1})$ (3)

${\tau}_{\psi}=c(F_{2}-F_{1}+F_{4}-F_{3})$ (4)

where l is the distance between the center of motor and the center of mass,
and c is the force to torque coefficient. As assumed, the quadcopter is a
rigid body and symmetrical dynamics apply, from which the torque can be
calculated by following equation:

${\tau}=I\dot{\omega}+\Omega(I\omega)$ (5)

where l is the distance between the center of motor to center of mass, and c
is the force to torque coefficient. As assumed, the quadcopter is a rigid body
and symmetrical dynamics, from which the following equation can calculate the
torque:

${\Omega}=\left[{\begin{array}[]{ccc}0&-r&q\\\ r&0&-p\\\
-q&p&0\end{array}}\right]$ (6)

In this system, the main control inputs are correlated to the torque
($\tau=[{\tau}_{\phi},{\tau}_{\theta},{\tau}_{\psi}]^{T}$) caused by thrust
forces, body gyroscopic effects, propeller gyroscopic effects and aerodynamic
friction. Gyroscopic effects and aerodynamic friction are considered external
disturbances for the control. Thus, control inputs are determined as Eq (7).

$\left[{\begin{array}[]{c}u_{\phi}\\\ u_{\theta}\\\ u_{\psi}\\\
u_{T}\end{array}}\right]=\left[{\begin{array}[]{c}\tau_{\phi}\\\
\tau_{\theta}\\\ \tau_{\psi}\\\
\tau_{T}\end{array}}\right]=\left[{\begin{array}[]{cccc}0&l&0&-l\\\
-l&0&l&0\\\ -c&c&-c&c\\\
1&1&1&1\end{array}}\right]\left[{\begin{array}[]{c}F_{1}\\\ F_{2}\\\ F_{3}\\\
F_{4}\end{array}}\right]$ (7)

where $\tau_{T}$ is the lift force and $u_{T}$ corresponds to the total thrust
acting on the four propellers, where $u_{\phi}$, $u_{\theta}$ and $u_{\psi}$
represent the roll, pitch, and yaw, respectively. The drone’s altitude can be
controlled by lift force ($u_{T}$), which is equal to quadcopter weight. The
dynamic equations of the quadcopter are formulated based on the Newton-Euler
method [33]. The six degree of freedom (6-DOF) motion equations are stated by
Eqs (8)–(13).

$\dot{u}=rv-qw-g\sin(\theta)$ (8)

$\dot{v}=pw-ru+g\sin(\phi)\cos(\theta)$ (9)

$\dot{w}=qu-pv+g\cos(\theta)\cos(\phi)-\frac{1}{m}u_{T}$ (10)

$\dot{p}=\frac{1}{I_{xx}}\left[(I_{yy}-I_{zz})qr+u_{\phi}+d_{\phi}\right]$
(11)

$\dot{q}=\frac{1}{I_{yy}}\left[(I_{zz}-I_{xx})pr+u_{\theta}+d_{\theta}\right]$
(12)

$\dot{r}=\frac{1}{I_{zz}}\left[(I_{xx}-I_{yy})pq+u_{\psi}+d_{\psi}\right]$
(13)

where $d=[d_{\phi},d_{\theta},d_{\psi}]^{T}$ is the angular acceleration
disturbance corresponded to propeller angular speed, and these acceleration
disturbances are modeled by Eq (14).

$d=\left[{\begin{array}[]{c}+qI_{m}\Omega_{r}\\\ -pI_{m}\Omega_{r}\\\
0\end{array}}\right]$ (14)

where $\Omega_{r}=\sum_{i=1}^{4}(-1)^{i}\Omega_{i}$ is the overall residual
propeller angular speed, and $\Omega_{i}$ is the angular velocity of each
rotor. $I_{m}$ is the rotor moment of inertia around the axis of rotation.
Hence, the dynamics equations of the system can be summarized as follows:

$\displaystyle\begin{split}\dot{x}(t)=A(x)+B(x)u(t)+d\\\
y(t)=C(x)+D(x)u(t)\end{split}$ (15)

where $x=[\phi,\theta,\psi,p,q,r,w]^{T}$ and $y=[y_{1},y_{2},y_{3},y_{4}]^{T}$
are the states and measurable outputs, respectively.
$u=[u_{1},u_{2},u_{3},u_{4}]^{T}$ is the control and $d$ is the disturbance.
$A$, $B$, $C$, and $D$ are the nonlinear functions regarding dynamic equations
of the system.

The control design is considered to minimize the error for tracking the
desired command (see Eq (16)).

$\lim_{t\to\infty}{\|{e(t)}\|}=\varepsilon$ (16)

where $e(t)=r(t)-y(t)$ is the difference between reference inputs and the
system’s measurable outputs and $\varepsilon$ is the small positive value.

##  III Proposed PIDA Controller

The PID control is applied to many engineering applications because of its
simplicity. Note that PID cannot function effectively when wide overshoot and
considerable settling time occur in the system. A modified PID controller can
address this issue by adding a zero known as PID acceleration (PIDA). It is
employed to achieve a faster and smoother response for a higher-order system
and retains both overshoots and settling time within an acceptable limit. The
proposed linear control can also control the nonlinear system. In this
approach, the dynamic airframe is linearized about the equilibrium point. The
linearization of the model is given by Eq (17).

$\Delta\dot{X}=J_{X}\Delta X+J_{U}\Delta U$ (17)

where $J_{X}$ and $J_{U}$ are the Jacobian transformation of the nonlinear
model about the equilibrium point
($X_{eq}=[\phi_{0},\theta_{0},\psi_{0},p_{0},q_{0},r_{0},w_{0}]^{T}$). Note
that the equilibrium point can be calculated by solving $\dot{X}=AX=0$. Any
solution can be the equilibrium point because of the null space if $det(A)$ is
equal to zero.

In this regard, a multi-input, multi-output (MIMO) control system design
follows the desire command in altitude and attitude channels. A MIMO tracking
controller can not only stabilize the system, but also make it follow a
reference input. Thus, the linear system is given as follows:

${\begin{array}[]{c}\dot{X}=AX+BU+D_{d}\\\ Y=CX\end{array}}$ (18)

where $Y$ is the outputs that follow the reference inputs and
$D_{d}=[0,0,0,d^{T},0]^{T}$ is the angular disturbance. In this approach, the
integral state is defined as follows:

$\dot{X}_{N}=R-Y=R-CX$ (19)

According to Eq (19), the new state space of the system is formulated in Eq
(20). The system can follow the reference inputs if the designed controller
proves the stability of the system.

${\begin{array}[]{c}\left[{\begin{array}[]{c}\dot{X}\\\
\dot{X}_{N}\end{array}}\right]=\left[{\begin{array}[]{cc}A&0\\\
-C&0\end{array}}\right]\left[{\begin{array}[]{c}{X}\\\
{X}_{N}\end{array}}\right]+\left[{\begin{array}[]{c}B\\\
\Phi\end{array}}\right]U+\left[{\begin{array}[]{c}\Phi\\\
I\end{array}}\right]R\\\ +\left[{\begin{array}[]{c}I\\\
\Phi\end{array}}\right]D_{d}\\\
Y=\left[{\begin{array}[]{cc}C&0\end{array}}\right]\left[{\begin{array}[]{c}X\\\
X_{N}\end{array}}\right]\end{array}}$ (20)

where $\Phi$ is a zero matrix.

Regarding the acceleration disturbance in the system, the general form of the
proposed controller in the time series is given in Eq (21).

$u(t)=k_{p}e(t)+k_{i}\int{e(t)dt}+k_{d}\dot{e}(t)+k_{a}\ddot{e}(t)$ (21)

where $kp$, $k_{i}$, $kd$ and $k_{a}$ are the gain of proposed controller.
Then, the MIMO controller is generated by

$U(s)=\left[k_{p}+\frac{k_{i}}{s}+k_{d}s+k_{a}s^{2}\right]E(s)$ (22)

As seen in Eq (22), the derivative term is inefficient in the high-frequency
domain and can affect the performance of the whole system in a noisy
environment. The addition of a derivative filter is proposed to address this
issue. Thus, the proposed control is modeled as follows:

$U(s)=\left[k_{p}+\frac{k_{i}}{s}+k_{d}\times sL(s)+k_{a}\times sL(s)\times
sL(s)\right]E(s)$ (23)

where $L(s)$ is the optimal derivative filter which is formulated as follows:

$L(s)=\frac{N/T}{(N/T)\frac{1}{s}+1}$ (24)

where $N$ and $T$ are the order of the filter and time constant, respectively.
Based on Eq (24), the transfer function of the optimal derivative filter can
be simplified as follows:

$L(s)=\frac{1}{1+T_{f}s}$ (25)

where $T_{f}=T/N$ is the time constant of the optimal derivative filter.
Hence, the controller and filter’s parameters can be found by SDSA to minimize
the objective function given by Eq (26).

$f_{obj}=(M_{os}-M_{s})^{2}-(t_{s}-t_{s})^{2}$ (26)

where $M_{os}$ is the desired maximum overshoot, which is set to $5$ percent;
$t_{s}$, the desired settling time for the system, is $2~{}sec$. $M_{s}$ and
$t_{s}$ are the overshoot and settling time for each set of the designed
controller. The stability analysis of the system (Eq (15)) is introduced
before the simulation results are presented.

##  IV Stability Analysis of the Proposed PIDA

In this section, the stability of the system, considering the proposed
controller is investigated. The following definitions are needed.

######  Definition 1.

”Asymptotically stable” is a system around its equilibrium point if it meets
the following conditions:

  1. 1.

Given any $\epsilon>0$, $\exists\delta_{1}>0$ such that if
$\|x(t_{0})\|<\delta_{1}$, then $\|x(t)\|<\epsilon$, $\forall t>t_{0}$

  2. 2.

$\exists\delta_{2}>0$ such that if $\|x(t_{0})\|<\delta_{2}$, then $x(t)\to 0$
as $t\to\infty$

######  Theorem 1.

$[V(x)=x^{T}Px,\quad x\in\mathbb{R}^{n}]$ is a positive definite function if
and only if all the eigenvalues of $P$ are positive.

Proof. Since $P$ is symmetric, it can be diagonalized by an orthogonal matrix
so $P=U^{T}DU$ with $U^{T}U=I$ and $D$ diagonal. Then, if $y=Ux$,

$\displaystyle\begin{split}V(x)&=x^{T}PX\\\ &=x^{T}U^{T}DUx\\\ &=y^{T}Dy\\\
&=\sum{\lambda}_{i}|{y_{i}}|^{2}\end{split}$ (27)

Thus,

$V(x)>0\quad\forall x\neq 0\iff\lambda_{i}>0,\quad\forall i$ (28)

######  Definition 2.

A matrix $P$ is a positive definite if it satisfies $x^{T}Px>0\quad\forall
x\neq 0$.

Therefore, any positive definite matrix follows the inequality in Eq (29).

$\lambda_{min}P\|x\|^{2}\leq V(x)\leq\lambda_{max}P\|x\|^{2}$ (29)

######  Definition 3.

($V$) is a positive definite function as a candidate Lyapunov function if
($\dot{V}$) has derivative, and it is negative semi-definite function.

######  Theorem 2.

If the candidate Lyapunov function (i.e., $V(x)=x^{T}Px,\quad P>0$) exists for
the dynamic system, there is a stable equilibrium point.

According to Theorem 2 and the dynamic system defined in Eq (15), the system
in the form of Lyapunov function is as follows:

$\displaystyle\begin{split}\dot{V}(x)&=\dot{x}^{T}Px+x^{T}P\dot{x}\\\
&=x^{T}A^{T}Px+x^{T}PAx\\\ &=x^{T}(A^{T}P+PA)x\\\ &=-x^{T}Qx\end{split}$ (30)

where the new notation (see Eq (31)) is introduced to simplify the
calculation, it is noted that $Q$ is a symmetric matrix. According to
Definition 3, $V$ is a Lyapunov function if $Q$ is positive definite (i.e.,
$Q>0$). Thus, there is a stable equilibrium point which shows the stability of
the system around the equilibrium (see Theorem 2).

$A^{T}P+PA=-Q$ (31)

The relationship between $Q$ and $P$ shows that the solution of Eq (31),
called a Lyapunov equation, proves the stability of the system for picking
$Q>0$ if $P$ is a positive definite solution. Thus, there is a unique positive
definite solution if all the eigenvalues of $A$ are in the left half-plane. A
noisy environment causes the movement of eigenvalues to the right half-plane.
Therefore, the system dynamics can intensify instability. This issue raises
the cross-coupling among different modes such as roll, pitch, and yaw rate,
which are caused by the four rotors. Thus, the derivative term of the proposed
controller plays an essential role in maintaining stability. The numerical
results show that all eigenvalues of the quadcopter with considering the
proposed controller with uncertainties in the environment are in the left
half-plane, which it proves that the dynamic system is stable with
uncertainties.

##  V Optimization and Heuristic filter

In this section, Stochastic Dual Simplex Algorithm (SDSA) and Genetic Filter
(GF) are described. First, the optimizaiton algorithm (i.e., SDSA) general
setting out is presented. Then, GF, the state estimation module in the
proposed controller, is introduced.

###  V-A Stochastic Dual Simplex Algorithm

The heuristic optimization algorithm, named Stochastic Dual Simplex Algorithm
(SDSA), is carried out to find the best tuned parameters of the proposed
controller. SDSA is the new version of Nelder-Mead simplex algorithm [34],
executing three different operators such as reflection, expansion, and
contraction. These operators make dual simplex reshape and move toward the
maximum-likelihood regions of the promising area. Each simplex follows the
normal rules of simplex, from which the transformed vertices of the general
simplex approach are formulated as in Eq (32)-(34).

$\textbf{x}_{r}=(1+\alpha)\bar{\textbf{x}}_{0}-\alpha\textbf{x}_{h},\quad\alpha>0$
(32)

$\textbf{x}_{e}=\gamma\textbf{x}_{r}+(1-\gamma)\bar{\textbf{x}}_{0},\quad\gamma>1$
(33)

$\textbf{x}_{c}=\beta\textbf{x}_{h}+(1-\beta)\bar{\textbf{x}}_{0},\quad
0\leq\beta\leq 1$ (34)

where $\alpha$, $\gamma$ and $\beta$ are reflection, expansion and contraction
coefficients, respectively. During these transformations, the centroid of all
vertices excluding the worst point ($\textbf{x}_{h}$) is
$\bar{\textbf{x}}_{0}$.

In addition to the movement of dual simplex, a new definition of reflection
points is applied to improve the diversity and decrease the probability of
local minimum. Therefore, during the i-th iteration, the worst vertices of
simplexes in search space are replaced by normal distribution directions which
are modeled in Eq (35).

$\overset{*}{\textbf{x}}_{h_{s}}^{(i)}=\textbf{x}_{h_{s}}^{(i)}+g^{(i)}{\bar{\textbf{x}}_{0}}^{(i)}$
(35)

where $\overset{*}{\textbf{x}}_{h_{s}}^{(i)}$ is the new reflected point
computed by the worst point of each simplex (${\textbf{x}}_{h_{s}}^{(i)}$),
and $g^{(i)}$ is the normal distribution of the sampled solution in i-th
iteration and $s$-th simplex. The centroid of all simplexes and the
probability density function of the normal distributed simplexes are then
expressed in Eq (36) and Eq (37).

$\bar{\textbf{x}}_{0}^{(i)}=\sum_{s=1}^{n_{s}}{\bar{\textbf{x}}_{0_{s}}^{(i)}}$
(36)

$g(\textbf{x}_{h}|\Sigma)=\frac{1}{\sqrt{2\pi|\Sigma|}}.exp({-\frac{(\textbf{x}_{h}-\bar{\textbf{x}}_{0})^{T}{\Sigma}^{-1}(\textbf{x}_{h}-\bar{\textbf{x}}_{0})}{2}})$
(37)

where $n_{s}$ and $\Sigma$ are the number of simplexes and covariance matrix
of simplexes, respectively.

Reflection makes an action regarding to reflect the worst point, called high,
over the centroid $\bar{\textbf{x}}_{0}$. In this approach, simplex operators
utilize the expansion operation to expand the simplex in the reflection
direction if the reflected point is better than other points. Nevertheless the
reflection output is at least better than the worst, the algorithm carries out
the reflection operation with the new worst point again [32, 34]. The
contraction is another operation which contracts the simplex while the worst
point has the same value as the reflected point. The SDSA pseudocode is
presented in Algorithm 1, and the tuned parameters of SDSA, chosen based on
[32], are listed in Table I.

Algorithm 1 Stochastic Dual Simplex Algorithm (SDSA)

Initialization

$\quad{\textit{set}}\leftarrow$ [$\textit{a}_{max}$,$\alpha_{max}$,
$\gamma_{max}$, $\beta_{max}$, $i_{max}$ ]

$\quad{\textbf{x}_{0}}\leftarrow\textit{random}$

Generate initial $simplexes$

Repeat

Compute Objective Function $(F)$

$\quad{\textbf{x}_{h}}\leftarrow\textbf{x}_{worst}$

while ($\exists\;x_{i}$):

$reflection$

$expansion$

$contraction$

end

$\quad{\textbf{x}_{h}}\leftarrow\overset{*}{\textbf{x}_{h}}$

Update $simplexes$

Until Stop condition satisfied.

TABLE I: Tuned parameters of SDSA Parameters | Value
---|---
${a}_{max}$ | $10.5907$
${\alpha}_{max}$ | $9.7323$
${\gamma}_{max}$ | $9.9185$
${\beta}_{max}$ | $0.4679$
${\textit{i}}_{max}$ | $979$

###  V-B Genetic Filter

In the Genetic Filter (GF), the problem is to estimate the states of a
discrete nonlinear dynamic system in a continuous search space. The model is
as follows:

$\textbf{x}_{k}=\textbf{f}_{k}(\textbf{x}_{k-1},\textbf{w}_{k-1})$ (38)

Where $k$ is time step, $\textbf{f}_{k}$ is the system model,
$\textbf{x}_{k-1}$ is the state vector and $\textbf{w}_{k-1}$ is the process
noise corresponding to system uncertainties. Also, the measurement model is
considered as

$\textbf{z}_{k}=\textbf{h}_{k}(\textbf{x}_{k},\textbf{v}_{k})$ (39)

where $\textbf{h}_{k}$ is the measurement model and $\textbf{v}_{k}$ is the
measurements noise.

Therefore, GF is introduced as a tool for nonlinear systems state estimation
based on Genetic Algorithm (GA). GF has two loops. The outer loop generates an
initial population that belongs to the first generation every time a new
measurement is entered. The inner loop iterates to find the best estimation of
the current states, corresponding to the entered measurement. To do this, the
inner loop, first propagates the individuals. Then, for each individual, the
corresponding output is calculated based on the measurement model. The
calculated outputs are compared with the real measurement, and each individual
is assigned a cost. The inner loop uses genetic operation such as selection,
mutation, and crossover to select new parents and survive the fittest
individual and generate new population toward the maximum likelihood regions
of the state space and is terminated when the maximum number of iterations
(generations) ($\textit{i}_{max}$) is reached. Finally, the state estimation
is made using the average of individuals of the last generation (see Algorithm
2) . The average of the individuals is calculated and passed as the state
estimation. Algorithm 2 represents pseudo-code of GF.

Algorithm 2 Genetic Filter

Initialization

$\quad{Set}\leftarrow$ [ Max Generation ($\textit{i}_{max}$), Population Size,
Mutation Rate ]

Repeat

$\quad{\textbf{x}_{0}}\leftarrow randomize$

Generate initial population

$i=1$

Repeat

Propagate individuals

Measurement update for each individual

Compute cost function for each individual

Update population

Selection

Crossover

Mutation

$i=i+1$

$\textbf{Until}\leftarrow i\geq i_{max}$

$\textbf{Until}\leftarrow\text{measurement is stopped}$

##  VI Numerical Results

The numerical simulation is implemented to evaluate the performance of the
proposed controller. The model quadcopter was simulated in MATLAB R2016b in a
Simulink environment in Windows 10 with an Intel(R) Core(TM)i7-6700 CPU @ 3.4
Hz. The quadcopter parameters are listed in Table II.

TABLE II: Quadcopter Model Parameters Parameter | Description | value | Unit
---|---|---|---
$m$ | Mass | $0.8$ | $kg$
$l$ | Arm length | $0.2$ | $m$
$g$ | Gravity acceleration | $9.81$ | $m/s^{2}$
$c$ | Force to torque coefficient | $3.00e-5$ | $kg~{}m^{2}$
$I_{xx}$ | Body moment of inertia along x-axis | $2.28e-2$ | $kgm^{2}$
$I_{yy}$ | Body moment of inertia along y-axis | $3.10e-2$ | $kg~{}m^{2}$
$I_{zz}$ | Body moment of inertia along z-axis | $4.40e-2$ | $kg~{}m^{2}$
$I_{m}$ | Motor moment of inertia | $8.30e-5$ | $kg~{}m^{2}$

To begin the simulation and tune the parameters of the proposed controller,
the initial state of the quadcopter is at an altitude of $50~{}m$; and
attitude and velocity in different directions are equal to zero. A
disturbance, which is modeled as white noise (mean value ($\mu$) is zero and
standard deviation ($\sigma$) is one), at time $1~{}sec$ in the roll channel,
is applied to the quadcopter. This disturbance destabilizes the system and
locates the eigenvalues of $A$ in the right half-plane. Additionally, the
quadrotor is highly sensitive to the noisy environment because of instability
and cross-coupling. In this regard, PIDA with a derivative filter, which
obviates the noise from the measurement inputs, is designed to respond to this
issue and keep the flight stable.

According to the proposed PIDA with a derivative filter, an additional issue
is the tracking of desire inputs, which can be defined as a command to the
quadcopter, are another issue that can be addressed by a MIMO controller
(i.e., four inputs and four outputs). The proposed controller can be set by
four gains and the time constant for each mode/channel.

Figure 2 and Fig. 3 show the attitude of the modeled quadcopter in the initial
state without the noisy environment. It is obvious that the initial state is
stable at zero, which is expected to be.

Figure 2: Angular velocity at initial state Figure 3: Roll, Pitch and Yaw
angle for initial state

A disturbance, which is modeled as white noise (mean value ($\mu$) is zero,
and standard deviation ($\sigma$) is one), at time $1~{}sec$ in roll channel
is applied to the quadcopter. As seen in Figs. 4 and 5, this disturbance
renders the system unstable. Hence, the quadrotor is highly sensitive in the
noisy environment. In this regard, PIDA with a derivative filter, which
obviates the noise from the measurement inputs, is designed to respond to this
issue and keep the flight stable.

Figure 4: Angular velocity in noisy environment without controller Figure 5:
Roll, Pitch and Yaw angle for noisy environment without controller

To tune the parameters of PIDA, the complex commands that enable coupling
among different modes of the modeled quadcopter are used to evaluate the
performance of the designed controller. New command angles are provided by a
step function with $2~{}sec$ delay time in the simulation environment, where
$\phi=-5^{\circ}$, $\theta=10^{\circ}$, $\psi=30^{\circ}$ and with altitude
starting from $50~{}m$ and stabling at $20~{}m$. Note that noisy measurements
have been considered for this simulation, and are modeled as white noise.

The parameters of controllers are tuned using SDSA [32], and the convergence
graph is shown in Fig. 6. The SDSA is applied to the objective function
introduced in Eq (26). Table III represents the best fit set of parameters for
different modes/channels within the noisy environment.

Figure 6: Performance of SDSA versus iteration  TABLE III: Controller Parameters for Altitude and Attitude Controller Parameter | Channel
---|---
Roll | Pitch | Yaw | Altitude
$k_{i}$ | $0.1436$ | $3.6869$ | $0.0437$ | $1.00$
$k_{d}$ | $6.5097$ | $21.2743$ | $29.9872$ | $11.4676$
$k_{a}$ | $0.5772$ | $0.3429$ | $23.5238$ | $7.5114$
$T_{f}$ | $0.0437$ | $0.0331$ | $0.0117$ | $0.3752$

The delay time causes missing measurements in the noisy environment, so the
robust heuristic filters is required. GF, as robust filter for the dynamic
system, plays an important role to keep the flight stable by accurate
estimations and removing noise. Figures 7–9 show that the presented controller
with GF can adequately respond to and track the reference commands in the
noisy environment.

Figure 7: Controller response to the change of Roll, Pitch and Yaw angle for
PIDA and PIDA associated with GF Figure 8: Controller response to the change
of Angular velocity PIDA and PIDA associated with GF Figure 9: Controller
response to the change of Altitude PIDA and PIDA associated with GF

Having PIDA tuned under environmental uncertainties, a particular spiral
trajectory is introduced to evaluate the performance of the proposed control
in the noisy environment. The evaluated trajectory is modeled in Eq (40).

${\begin{array}[]{c}x=2sin(3\omega t)+2cos(\omega t)\\\ y=2sin(\omega
t)+2cos(3\omega t)\\\ z=0.3t\end{array}}$ (40)

where $x$, $y$, and $z$ are the reference trajectory. $\omega$ is the period
spiral trajectory, which is set to $\omega=1/2\pi$. $t$ is the flight time
between $0~{}sec$ and $60~{}sec$.

Figures 10 and 11 demonstrate the drone trajectory for both PIDA and PIDA-GF.
These figures show that PIDA associated with GF can boost the performance of
the proposed controller in the complex dynamics. Thus, the proposed controller
is able to have a great response in the spiral trajectory.

Figure 10: 2D trajectory of drone under uncertainties for PIDA and PIDA-GF
Figure 11: 3D trajectory of drone under uncertainties for PIDA and PIDA-GF

Accordingly, the drone movement in cooperation with PIDA-GF can guarantee a
smooth and stable flight while the drone is flying in the noisy environment.
Figure 12 shows the drone responses in the reference path. As seen in Fig. 12,
PIDA responses fluctuated around the points when the drone turns in the spiral
trajectory. This is due to the noise in the environment. Accordingly, GF is
able to reduce the effect of noise in the environment; thereby, the smooth
flight is performed by PIDA-GF.

Figure 12: Drone responses in the spiral trajectory

Consequently, these figures and the reference trajectory demonstrate that the
proposed controller associated with GF is powerful enough to control the
spiral path of drone.

As the simulation results demonstrate, not only is the quadcopter capable of
stable flight, but the proposed controller associated with GF also provides a
smooth flight. This smoothness and handling are acquired because of having the
robust filter, GF, in the dynamic movement. As shown, PIDA provides a fast and
stable flight, but applying GF for integrated estimation of the observed
states and parameters in both attitude and altitude boosts performance in the
quadcopter.

##  VII Conclusion

This paper has proposed a new Proportional-Integral-Derivative-Accelerated
(PIDA) controller with a derivative filter to improve flight stability for a
quadcopter and considers the noisy environment. The mathematical model
considering non-linearity, uncertainties, and coupling was derived from an
accurate model with a high level of fidelity. In the indoor environment and as
the critical features in the proposed controller, overshoot and settling time
were limited during the operation of the dynamic system (drone). The noisy
environment causes the movement of poles to the right half-plane. Thus, system
dynamics intensify instability. This issue raises the cross-coupling among
different modes, such as roll, pitch, and yaw, which were generated by the
four rotors. The proposed controller and heuristic Genetic Filter (GF)
addressed these challenges. Moreover, the derivative term of the proposed
controller assists the dynamic system to recover its stability. The controller
gains were optimized before performing the mission. In this regard, the tuning
of the proposed controller was performed by Stochastic Dual Simplex Algorithm
(SDSA). The simulation results show that the proposed PIDA controller
associated with GF was capable of supporting outstanding performance in
tracking the desired point despite disturbances.

## References

  * [1] S.-K. Kim and C. K. Ahn, “Adaptive nonlinear tracking control algorithm for quadcopter applications,” _IEEE Transactions on Aerospace and Electronic Systems_ , 2019.
  * [2] L. P. Koh and S. A. Wich, “Dawn of drone ecology: low-cost autonomous aerial vehicles for conservation,” _Tropical Conservation Science_ , vol. 5, no. 2, pp. 121–132, 2012.
  * [3] M. D. Phung, C. H. Quach, T. H. Dinh, and Q. Ha, “Enhanced discrete particle swarm optimization path planning for uav vision-based surface inspection,” _Automation in Construction_ , vol. 81, pp. 25–33, 2017.
  * [4] S. Rajappa, C. Masone, H. H. Bülthoff, and P. Stegagno, “Adaptive super twisting controller for a quadrotor uav,” in _2016 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2016, pp. 2971–2977.
  * [5] L. Derafa, A. Benallegue, and L. Fridman, “Super twisting control algorithm for the attitude tracking of a four rotors uav,” _Journal of the Franklin Institute_ , vol. 349, no. 2, pp. 685–699, 2012.
  * [6] A. C. Woods and H. M. La, “A novel potential field controller for use on aerial robots,” _IEEE Transactions on Systems, Man, and Cybernetics: Systems_ , vol. 49, no. 4, pp. 665–676, 2017.
  * [7] W. Meng, Q. Yang, S. Jagannathan, and Y. Sun, “Distributed control of high-order nonlinear input constrained multiagent systems using a backstepping-free method,” _IEEE transactions on cybernetics_ , vol. 49, no. 11, pp. 3923–3933, 2018.
  * [8] F. Santoso, M. A. Garratt, S. G. Anavatti, and I. Petersen, “Robust hybrid nonlinear control systems for the dynamics of a quadcopter drone,” _IEEE Transactions on Systems, Man, and Cybernetics: Systems_ , 2018.
  * [9] M. B. Milovanović, D. S. Antić, M. T. Milojković, and M. D. Spasić, “Adaptive control of nonlinear mimo system with orthogonal endocrine intelligent controller,” _IEEE Transactions on Cybernetics_ , 2020\.
  * [10] H. Gu, P. Liu, J. Lü, and Z. Lin, “Pid control for synchronization of complex dynamical networks with directed topologies,” _IEEE Transactions on Cybernetics_ , 2019.
  * [11] E. D. P. Puchta, H. V. Siqueira, and M. dos Santos Kaster, “Optimization tools based on metaheuristics for performance enhancement in a gaussian adaptive pid controller,” _IEEE Transactions on Cybernetics_ , vol. 50, no. 3, pp. 1185–1194, 2019.
  * [12] F. Santoso, M. A. Garratt, and S. G. Anavatti, “Hybrid pd-fuzzy and pd controllers for trajectory tracking of a quadrotor unmanned aerial vehicle: Autopilot designs and real-time flight tests,” _IEEE Transactions on Systems, Man, and Cybernetics: Systems_ , 2019.
  * [13] Z. Zuo, “Trajectory tracking control design with command-filtered compensation for a quadrotor,” _IET control theory & applications_, vol. 4, no. 11, pp. 2343–2355, 2010.
  * [14] G. V. Raffo, M. G. Ortega, and F. R. Rubio, “An integral predictive/nonlinear h∞ control structure for a quadrotor helicopter,” _Automatica_ , vol. 46, no. 1, pp. 29–39, 2010.
  * [15] R. Ritz, M. Hehn, S. Lupashin, and R. D’Andrea, “Quadrocopter performance benchmarking using optimal control,” in _2011 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2011, pp. 5179–5186.
  * [16] S. M. Zandavi and S. H. Pourtakdoust, “Multidisciplinary design of a guided flying vehicle using simplex nondominated sorting genetic algorithm ii,” _Structural and Multidisciplinary Optimization_ , vol. 57, no. 2, pp. 705–720, 2018.
  * [17] R. Xu and U. Ozguner, “Sliding mode control of a quadrotor helicopter,” in _Proceedings of the 45th IEEE Conference on Decision and Control_. IEEE, 2006, pp. 4957–4962.
  * [18] L. Besnard, Y. B. Shtessel, and B. Landrum, “Control of a quadrotor vehicle using sliding mode disturbance observer,” in _2007 American Control Conference_. IEEE, 2007, pp. 5230–5235.
  * [19] S. Wen, M. Z. Chen, Z. Zeng, X. Yu, and T. Huang, “Fuzzy control for uncertain vehicle active suspension systems via dynamic sliding-mode approach,” _IEEE Transactions on Systems, Man, and Cybernetics: Systems_ , vol. 47, no. 1, pp. 24–32, 2016.
  * [20] K. H. Ang, G. Chong, and Y. Li, “Pid control system analysis, design, and technology,” _IEEE transactions on control systems technology_ , vol. 13, no. 4, pp. 559–576, 2005.
  * [21] S. Jung and R. C. Dorf, “Analytic pida controller design technique for a third order system,” in _Proceedings of 35th IEEE Conference on Decision and Control_ , vol. 3. IEEE, 1996, pp. 2513–2518.
  * [22] Y. Yuan, Z. Wang, P. Zhang, and H. Dong, “Nonfragile near-optimal control of stochastic time-varying multiagent systems with control-and state-dependent noises,” _IEEE transactions on cybernetics_ , vol. 49, no. 7, pp. 2605–2617, 2018.
  * [23] S. M. Zandavi and V. Chung, “State estimation of nonlinear dynamic system using novel heuristic filter based on genetic algorithm,” _Soft Computing_ , vol. 23, no. 14, pp. 5559–5570, 2019.
  * [24] R. E. Kalman, “A new approach to linear filtering and prediction problems,” _Journal of basic Engineering_ , vol. 82, no. 1, pp. 35–45, 1960.
  * [25] A. H. Jazwinski, _Stochastic processes and filtering theory_. Courier Corporation, 2007.
  * [26] S. J. Julier and J. K. Uhlmann, “New extension of the kalman filter to nonlinear systems,” in _Signal processing, sensor fusion, and target recognition VI_ , vol. 3068. International Society for Optics and Photonics, 1997, pp. 182–193.
  * [27] J. Carpenter, P. Clifford, and P. Fearnhead, “Improved particle filter for nonlinear problems,” _IEE Proceedings-Radar, Sonar and Navigation_ , vol. 146, no. 1, pp. 2–7, 1999.
  * [28] H. Nobahari, S. M. Zandavi, and H. Mohammadkarimi, “Simplex filter: a novel heuristic filter for nonlinear systems state estimation,” _Applied Soft Computing_ , vol. 49, pp. 474–484, 2016.
  * [29] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for online nonlinear/non-gaussian bayesian tracking,” _IEEE Transactions on signal processing_ , vol. 50, no. 2, pp. 174–188, 2002\.
  * [30] N. J. Gordon, D. J. Salmond, and A. F. Smith, “Novel approach to nonlinear/non-gaussian bayesian state estimation,” in _IEE proceedings F (radar and signal processing)_ , vol. 140, no. 2. IET, 1993, pp. 107–113.
  * [31] A. Smith, _Sequential Monte Carlo methods in practice_. Springer Science & Business Media, 2013.
  * [32] S. M. Zandavi, V. Y. Y. Chung, and A. Anaissi, “Stochastic dual simplex algorithm: A novel heuristic optimization algorithm,” _IEEE Transactions on Cybernetics_ , pp. 1–10, 2019.
  * [33] P. H. Zipfel, _Modeling and simulation of aerospace vehicle dynamics_. American Institute of Aeronautics and Astronautics, 2007.
  * [34] S. S. Rao and S. S. Rao, _Engineering optimization: theory and practice_. John Wiley & Sons, 2009.