Modified Infinite-Time State-Dependent Riccati Equation Method for Nonlinear Affine Systems: Quadrotor Control

Abstract

This paper presents modeling and infinite-time suboptimal control of a quadcopter device using the state-dependent Riccati equation (SDRE) method. It establishes a solution to the control problem using SDRE and proposes a new procedure for solving the problem. As a new contribution, the paper proposes a modified SDRE-based suboptimal control technique for affine nonlinear systems. The method uses a pseudolinearization of the closed-loop system employing Moore–Penrose pseudoinverse. Then, the algebraic Riccati equation (ARE), related to the feedback compensator gain, is reduced to state-independent form, and the solution can be computed only once in the whole control process. The ARE equation is applied to the problem reported in this study that provides general formulation and stability analysis. The effectiveness of the proposed control technique is demonstrated through the use of simulation results for a quadrotor device.

1. Introduction

The state-dependent Riccati equation (SDRE) method has become more popular due to the possibility of designing more effective nonlinear controllers and providing flexibility through state-dependent weighting matrices, which is well presented in [1]. This method scheme originates from the LQR method [2,3] and is also similar to the MPC (or its nonlinear version, NMPC) [4,5,6]. The main differences between SDRE and those two methods are, for example, unlike the LQR method, the SDRE sets up its control laws at every time-step. The MPC method requires a convex optimization technique, while SDRE and LQR methods utilize optimal control laws for linear systems [7,8].

The SDRE control method reformulates the nonlinear dynamics using parameterization to a linear structure with state-dependent coefficient (SDC) matrices, which are essential to solve an algebraic Riccati equation (ARE) and obtain the suboptimal control law. The coefficients of the ARE equation differ in the state space. Thus, at a given point in state space, it is essential to solve an algebraic state-dependent Riccati equation. The nonuniqueness of the state-dependent parameterization (SDP) allows us to obtain extra degrees of freedom, which may help to enhance controller performance. Minimization of the nonlinear performance index gives a quadratic-like structure [9,10,11,12].

In the last decade, this method has been used in many different areas of research, for example, satellite and spacecraft control and estimation [13,14], control of servopneumatic drives [15], control of quadrotor aircraft [16,17,18,19,20], robotics [21,22], control of non-affine systems [23], and even in medical sciences for cancer treatment allowing optimization of drug dosing control [24].

The main disadvantage of this classic SDRE strategy is that Riccati equations are state dependent. Therefore, it is necessary to solve SDRE multiple times during the control process [25,26]. Nowadays, the solution can be successfully implemented in real control systems due to developments of digital controllers. However, the solution can be found faster, reducing computational effort and introducing linearization of closed-loop form. As a new contribution, the paper presents a possible solution for suboptimal control of nonlinear systems by solving the algebraic Riccati equation only once in the whole control process. Using linearization of the closed-loop system and introducing Moore–Penrose inversion [27], it is possible to modify SDRE into ARE. For the modified SDRE method, it is possible to obtain the same solution as for classic SDRE method. The advantage of the modified case is that there is no need to solve the state-dependent Riccati equation for each state of the control process. This allows us to save space for implementation in real-time control systems and simplifies computations. The specific differences between classical and modified methods are presented in the sections below.

2. SDRE Method

Let Equation (1) be the continuous time nonlinear system

$$\dot{\mathbf{x}}=\mathbf{F}\left(\mathbf{x}\right)+\mathbf{B}\mathbf{u},$$

(1)

where $\mathbf{x}\in {\mathbb{R}}^n$ and $\mathbf{u}\in {\mathbb{R}}^m$ are the state and input vectors, respectively. $\mathbf{F}\left(\mathbf{x}\right)$ is a continuous ($C^k$ class) nonlinear function of $\mathbf{x}$, and $\mathbf{B}\in {\mathbb{R}}^{n\times m}$ is the constant control matrix. The problem consists of finding an admissible control $\mathbf{u}\in {\mathbb{R}}^m$ for $t_0\le t$, which minimizes the performance index [28,29,30,31]

$$J\left(\mathbf{u}\right)={\displaystyle \frac{1}{2}}\underset{0}{\overset{\infty }{\int }}({\mathbf{x}}^T\mathbf{Q}\mathbf{x}+{\mathbf{u}}^T\mathbf{R}\mathbf{u})dt,$$

(2)

where $\mathbf{Q}\in {\mathbb{R}}^{n\times n}$ is a symmetric positive semidefinite, and $\mathbf{R}\in {\mathbb{R}}^{m\times m}$ is a symmetric positive-definite matrix. Rewriting the nonlinear Equation (1), the state-dependent coefficient form (SDC) follows

$$\dot{\mathbf{x}}=\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{B}\mathbf{u},$$

(3)

where $\mathbf{A}\left(\mathbf{x}\right)\in {\mathbb{R}}^{n\times n}$.

There are many methods for SDC parameterization. Each parameterization should be true for all $0\le \alpha \le 1$ [32]

$$\alpha {\mathbf{A}}_I\left(\mathbf{x}\right)\mathbf{x}+(1-\alpha ){\mathbf{A}}_{II}\left(\mathbf{x}\right)\mathbf{x}=\alpha \mathbf{F}\left(\mathbf{x}\right)+(1-\alpha )\mathbf{F}\left(\mathbf{x}\right)=\mathbf{F}\left(\mathbf{x}\right).$$

(4)

The control problem for the nonlinear continuous-time system Equation (1) can be formulated as follows. Given nonlinear functions $\mathbf{F}\left(\mathbf{x}\right)\in {\mathbb{R}}^n,\mathbf{B}\in {\mathbb{R}}^{n\times m},\mathbf{Q}\in {\mathbb{R}}^{n\times n},$ and $\mathbf{R}\in {\mathbb{R}}^{m\times m}$, we attempt to find a control vector $\mathbf{u}\in {\mathbb{R}}^m$ for $t\in [t_0,\infty ]$ that allows us to control the system state vector from ${\mathbf{x}}_0$ to ${\mathbf{x}}_{\infty }$ while the performance index is minimized (Equation (19)).

If the integral functions ${\mathbf{x}}^T\mathbf{Q}\mathbf{x}+{\mathbf{u}}^T\mathbf{R}\mathbf{u}$ and $\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{B}\mathbf{u}$ are continuously differentiable functions of each of their arguments, then we may imply that $\mathbf{u}\in C[t_0,\infty ]$ is a control that minimizes the functional $J\left(\mathbf{u}\right):C[t_0,\infty ]\to \mathbb{R}$. To solve the problem, consider the Hamiltonian

$$\mathbf{H}={\displaystyle \frac{1}{2}}({\mathbf{x}}^T\mathbf{Q}\mathbf{x}+{\mathbf{u}}^T\mathbf{R}\mathbf{u})+{\mathbf{p}}^T(\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{B}\mathbf{u}),$$

(5)

where $\mathbf{p}\in {\mathbb{R}}^n$ is the co-state vector.

To be consistent with the Pontryagin minimum principle, if $\mathbf{u}\in C[t_0,\infty ]$ is a control for the functional Equation (19), subject to state Equation (3), and if $\mathbf{x}$ denotes the corresponding state, then there exists a $\mathbf{p}\in C[t_0,\infty ]$ such that

$${\displaystyle \frac{\partial H}{\partial \mathbf{u}}}(\mathbf{p},\mathbf{x},\mathbf{u},t)=\mathbf{0}fort\in [t_0,\infty ]$$

(6)

and

$$\dot{\mathbf{p}}=-{\displaystyle \frac{\partial H}{\partial \mathbf{x}}}(\mathbf{p},\mathbf{x},\mathbf{u},t)fort\in [t_0,\infty ]and\mathbf{p}(\infty )=\mathbf{0}.$$

(7)

Equations (6) and (7) are conditions allowing us to obtain suboptimal control while minimizing Equation (19).

It follows that any optimal input $\mathbf{u}\in {\mathbb{R}}^m$ and the corresponding state $\mathbf{x}\in {\mathbb{R}}^n$ satisfies Equation (6); that is,

$${\displaystyle \frac{\partial H}{\partial \mathbf{u}}}=\mathbf{R}\mathbf{u}+{\mathbf{B}}^T\mathbf{p}=\mathbf{0}.$$

(8)

From Equation (8), the optimal control vector is

$$\mathbf{u}=-{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{p},$$

(9)

and subsequently, the adjoint differential equation is

$$\dot{\mathbf{p}}={\displaystyle \frac{\partial H}{\partial \mathbf{x}}}=-{\left[{\displaystyle \frac{\partial \left(\mathbf{A}\right(\mathbf{x}\left)\right)\mathbf{x}}{\partial \mathbf{x}}}\right]}^T\mathbf{p}-\mathbf{Q}\mathbf{x}$$

(10)

for $t\in [t_0,\infty ],\mathbf{x}\left(t_0\right)={\mathbf{x}}_0$, and $\mathbf{p}(\infty )=\mathbf{0}$, where

$${\displaystyle \frac{\partial \left(\mathbf{A}\right(\mathbf{x}\left)\mathbf{x}\right)}{\partial \mathbf{x}}}=\mathbf{A}\left(\mathbf{x}\right)+{\displaystyle \frac{\partial \mathbf{A}\left(\mathbf{x}\right)}{\partial \mathbf{x}}}\mathbf{x}.$$

(11)

Let $\mathbf{p}$ be a nonlinear combination of the state of the system

$$\mathbf{p}=\mathbf{K}\left(\mathbf{x}\right)\mathbf{x},$$

(12)

where $\mathbf{K}\left(\mathbf{x}\right)\in {\mathbb{R}}^{n\times n}$ denotes feedback compensator gain, and let $\mathbf{x}$ be the solution of the nonlinear state equation

$$\dot{\mathbf{x}}=\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}-\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}$$

(13)

for $t\in [t_0,\infty ],\mathbf{x}\left(t_0\right)={\mathbf{x}}_0$. Then, feedback control is

$$\mathbf{u}=-{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}.$$

(14)

Let $\mathbf{K}\left(\mathbf{x}\right)$ solve the state-dependent Riccati equation (SDRE), which is

$$\mathbf{K}\left(\mathbf{x}\right)\mathbf{A}\left(\mathbf{x}\right)+{\mathbf{A}}^T\left(\mathbf{x}\right)\mathbf{K}\left(\mathbf{x}\right)-\mathbf{K}\left(\mathbf{x}\right)\mathbf{B}{\mathbf{R}}^{-1}\left(\mathbf{x}\right){\mathbf{B}}^T\mathbf{K}\left(\mathbf{x}\right)+\mathbf{Q}\left(\mathbf{x}\right)=\mathbf{0};$$

(15)

then the equation below satisfies the optimality condition [25,32]

$$\dot{\mathbf{K}}\left(\mathbf{x}\right)+{\left[{\displaystyle \frac{\partial \left(\mathbf{A}\right(\mathbf{x}\left)\mathbf{x}\right)}{\partial \mathbf{x}}}\right]}^T\mathbf{K}\left(\mathbf{x}\right)=\mathbf{0}.$$

(16)

The suboptimal control

$$\mathbf{u}=-{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}$$

(17)

for performance index (objective function) Equation (19) subject to dynamics Equation (1) can be found solving the state-dependent Riccati Equation (15). Often, it is difficult to find the solution of Equation (15) analytically. One approach allows us to solve the SDRE via symbolic software packages such as Matlab. However, for complex systems, the solution may become complicated, and then, it is necessary to approximate the solution. To approximate, an interpolation method or Taylor series method can be used, for instance, ref. [25].

3. Modified SDRE Method

Let Equation (18) be the continuous time nonlinear system

$$\dot{\mathbf{x}}=\mathbf{F}\left(\mathbf{x}\right)+\mathbf{B}\mathbf{u},$$

(18)

where $\mathbf{x}\in {\mathbb{R}}^n$ and $\mathbf{u}\in {\mathbb{R}}^m$ are the state and input vectors. $\mathbf{F}\left(\mathbf{x}\right)$ is a continuous ($C^k$ class) nonlinear function of $\mathbf{x}$, and $\mathbf{B}\in {\mathbb{R}}^{n\times m}$ is the constant control matrix. The problem consists of finding an admissible control $\mathbf{u}\in {\mathbb{R}}^m$ for $t_0\le t$ that minimizes the performance index [28,29,30,31]

$$J\left(\mathbf{u}\right)={\displaystyle \frac{1}{2}}\underset{0}{\overset{\infty }{\int }}({\mathbf{x}}^T\mathbf{Q}\mathbf{x}+{\mathbf{u}}^T\mathbf{R}\mathbf{u})dt,$$

(19)

where $\mathbf{Q}\in {\mathbb{R}}^{n\times n}$ is a symmetric positive semidefinite, and $\mathbf{R}\in {\mathbb{R}}^{m\times m}$ is a symmetric positive-definite matrix.

The first modification (compared to the classic approach) is to rewrite the system Equation (18) as a sum of state-independent and nonlinear state-dependent coefficient (SDC) forms Equation [25]:

$$\dot{\mathbf{x}}=\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{B}\mathbf{u}={\mathbf{A}}_1\mathbf{x}+{\mathbf{A}}_2\left(\mathbf{x}\right)\mathbf{x}+\mathbf{B}\mathbf{u},$$

(20)

where

$$\mathbf{A}\left(\mathbf{x}\right)={\mathbf{A}}_1+{\mathbf{A}}_2\left(\mathbf{x}\right)$$

(21)

is a sum of a constant matrix and a state-dependent matrix [12,33].

The SDC parameterization of Equation (20) can be performed the same way as for Equation (4), for all $0\le \alpha \le 1$ [32]:

$$\begin{array}{cc}\hfill & \alpha \left[{\mathbf{A}}_{1,I}+{\mathbf{A}}_{2,I}\left(\mathbf{x}\right)\right]\mathbf{x}+(1-\alpha )\left[{\mathbf{A}}_{1,II}+{\mathbf{A}}_{2,II}\left(\mathbf{x}\right)\right]\mathbf{x}=\hfill \\ \hfill & \alpha \left[{\mathbf{A}}_{1,I}+(1-\alpha ){\mathbf{A}}_{1,II}+\alpha {\mathbf{A}}_{2,I}\left(\mathbf{x}\right)+(1-\alpha ){\mathbf{A}}_{2,II}\left(\mathbf{x}\right)\right]\mathbf{x}=\hfill \\ \hfill & \alpha {\mathbf{F}}_I\left(\mathbf{x}\right)+(1-\alpha ){\mathbf{F}}_{II}\left(\mathbf{x}\right)=\mathbf{F}\left(\mathbf{x}\right).\hfill \end{array}$$

(22)

To easily formulate controllability criteria, the state-dependent controllability matrix is introduced $\mathbf{W}\left(\mathbf{x}\right)$ [25]

$$\mathbf{W}\left(\mathbf{x}\right)=[\mathbf{B}({\mathbf{A}}_1+{\mathbf{A}}_2\left(\mathbf{x}\right))\mathbf{B}\dots {({\mathbf{A}}_1+{\mathbf{A}}_2\left(\mathbf{x}\right))}^{n-1}\mathbf{B}].$$

(23)

If $\mathbf{W}\left(\mathbf{x}\right)$ (state-dependent) has the full rank, then the system is controllable for all $\mathbf{x}\in {\mathbb{R}}^n$[34]. As in the previous case of the SDRE method, an admissible control $\mathbf{u}\left(t\right)\in {\mathbb{R}}^m$, that minimizes the performance index can be found for [28,29,30,31]

$$J\left(\mathbf{u}\right)={\displaystyle \frac{1}{2}}\underset{0}{\overset{\infty }{\int }}({\mathbf{x}}^T\mathbf{Q}\mathbf{x}+{\mathbf{u}}^T\mathbf{R}\mathbf{u})dt,$$

(24)

where $\mathbf{Q}\in {\mathbb{R}}^{n\times n}$ is a symmetric positive semi- definite matrix, and $\mathbf{R}\in {\mathbb{R}}^{m\times m}$ is a symmetric positive definite matrix. The control problem for the nonlinear continuous-time system Equation (18) can be formulated as follows. Given nonlinear functions $\mathbf{F}\left(\mathbf{x}\right)\in {\mathbb{R}}^n,\mathbf{B}\in {\mathbb{R}}^{n\times m},\mathbf{Q}\in {\mathbb{R}}^{n\times n}$ and $\mathbf{R}\in {\mathbb{R}}^{m\times m}$, we attempt to find a control vector $\mathbf{u}\in {\mathbb{R}}^m$ for $t\in [t_0,\infty ]$ that controls the system state vector from ${\mathbf{x}}_0$ to ${\mathbf{x}}_{\infty }$ while minimizing the performance index (Equation (19)).

If the integrand of the cost functions ${\mathbf{x}}^T\mathbf{Q}\mathbf{x}+{\mathbf{u}}^T\mathbf{R}\mathbf{u}$ and $\mathbf{A}\left(\mathbf{x}\right)\mathbf{x}+\mathbf{B}\mathbf{u}$ are continuously differentiable functions of each of their arguments, then we may imply that $\mathbf{u}\in C[t_0,\infty ]$ is a control that minimizes the functional $J\left(\mathbf{u}\right):C[t_0,\infty ]\to \mathbb{R}$. Note that, in this approach, two feedback compensators are defined $\mathbf{A}\left(\mathbf{x}\right)={\mathbf{A}}_1+{\mathbf{A}}_2\left(\mathbf{x}\right)$. To solve the problem, consider the Hamiltonian

$$H={\displaystyle \frac{1}{2}}({\mathbf{x}}^T\mathbf{Q}\mathbf{x}+{\mathbf{u}}^T\mathbf{R}\mathbf{u})+{\mathbf{p}}^T(({\mathbf{A}}_1+{\mathbf{A}}_2\left(\mathbf{x}\right))\mathbf{x}+\mathbf{B}\mathbf{u}),$$

(25)

where $\mathbf{p}\in {\mathbb{R}}^n$ is the co-state vector.

If $\mathbf{u}\in C[t_0,\infty ]$ is a control that minimizes Equation (19) subject to the state Equation (20), and if $\mathbf{x}$ denotes the corresponding state, then there exists a $\mathbf{p}\in C[t_0,\infty ]$ such that

$${\displaystyle \frac{\partial H}{\partial \mathbf{u}}}(\mathbf{p},\mathbf{x},\mathbf{u},t)=\mathbf{0}fort\in [t_0,\infty ]$$

(26)

and

$$\dot{\mathbf{p}}=-{\displaystyle \frac{\partial H}{\partial \mathbf{x}}}(\mathbf{p},\mathbf{x},\mathbf{u},t)fort\in [t_0,\infty ]and\mathbf{p}(\infty )=\mathbf{0},$$

(27)

where $\mathbf{p}$ satisfies the condition for optimal control that minimizes Equation (19).

It follows that any optimal input $\mathbf{u}\in {\mathbb{R}}^m$ and the corresponding state $\mathbf{x}\in {\mathbb{R}}^n$ satisfy Equation (26); that is,

$${\displaystyle \frac{\partial H}{\partial \mathbf{u}}}=\mathbf{R}\mathbf{u}+{\mathbf{B}}^T\mathbf{p}=\mathbf{0}.$$

(28)

Thus, the optimal control is

$$\mathbf{u}=-{\mathbf{R}}^{-1}{\mathbf{B}}^T\mathbf{p},$$

(29)

and subsequently, the adjoint differential equation is

$$\dot{\mathbf{p}}={\displaystyle \frac{\partial H}{\partial \mathbf{x}}}=-{\left[{\mathbf{A}}_1+{\displaystyle \frac{\partial \left({\mathbf{A}}_2\left(\mathbf{x}\right)\right)\mathbf{x}}{\partial \mathbf{x}}}\right]}^T\mathbf{p}-\mathbf{Q}\mathbf{x},$$

(30)

for $t\in [t_0,\infty ],\mathbf{x}\left(t_0\right)={\mathbf{x}}_0,{\mathbf{A}}_2\left({\mathbf{x}}_0\right)={\mathbf{A}}_{2_0}$, and $\mathbf{p}(\infty )=\mathbf{0}$, where

$${\displaystyle \frac{\partial \left({\mathbf{A}}_2\left(\mathbf{x}\right)\mathbf{x}\right)}{\partial \mathbf{x}}}={\mathbf{A}}_2\left(\mathbf{x}\right)+{\displaystyle \frac{\partial {\mathbf{A}}_2\left(\mathbf{x}\right)}{\partial \mathbf{x}}}\mathbf{x}.$$

(31)

Consider $\mathbf{p}$ as a linear and nonlinear combination of the state of the system Equation (18), which is the second modification of the classic approach

$$\mathbf{p}=\mathbf{K}\left(\mathbf{x}\right)\mathbf{x}={\mathbf{K}}_1\mathbf{x}+{\mathbf{K}}_2\left(\mathbf{x}\right)\mathbf{x},$$

(32)

where $\mathbf{K}\left(\mathbf{x}\right),{\mathbf{K}}_1,{\mathbf{K}}_2\left(\mathbf{x}\right)\in {\mathbb{R}}^{n\times n}$.

Consider $\mathbf{x}$ as the solution of the nonlinear state equation

$$\dot{\mathbf{x}}={\mathbf{A}}_1\mathbf{x}+{\mathbf{A}}_2\left(\mathbf{x}\right)\mathbf{x}-\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_1\mathbf{x}-\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_2\left(\mathbf{x}\right)\mathbf{x}$$

(33)

for $t\in [t_0,\infty ],\mathbf{x}\left(t_0\right)={\mathbf{x}}_0$, and ${\mathbf{A}}_2\left({\mathbf{x}}_0\right)={\mathbf{A}}_{20}$.

Note that, in this approach, two feedback compensators are defined.

Rearranging Equation (33), it is possible to obtain

$$\dot{\mathbf{x}}=({\mathbf{A}}_1-\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_1)\mathbf{x}+({\mathbf{A}}_2\left(\mathbf{x}\right)-\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_2\left(\mathbf{x}\right))\mathbf{x}.$$

(34)

The first bracket of Equation (34) is state-independent, and the second one is state-dependent; thus, it is possible to linearize it and solve the state-dependent gain matrix- ${\mathbf{K}}_2\left(\mathbf{x}\right)$ from:

$${\mathbf{A}}_2\left(\mathbf{x}\right)-\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_2\left(\mathbf{x}\right)=\mathbf{0}$$

(35)

as follows:

$${\mathbf{K}}_2\left(\mathbf{x}\right)={\left[\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T\right]}^{+}{\mathbf{A}}_2\left(\mathbf{x}\right).$$

(36)

Matrix $\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T$ is singular; thus, the state-dependent matrix gain ${\mathbf{K}}_2\left(\mathbf{x}\right)$ may be computed only by the pseudoinverse operation ${\left[\dots \right]}^{+}$.

In order to perform this procedure, a Moore–Penrose pseudoinverse is used, and the matrix is unique [27,35,36].

Equating the adjoint differential Equation (30) and the differential form of Equation (32), we have

$${\displaystyle \frac{\partial }{\partial t}}[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]\mathbf{x}+[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]\dot{\mathbf{x}}=-{\left[{\mathbf{A}}_1+{\mathbf{A}}_2\left(\mathbf{x}\right)+{\displaystyle \frac{\partial \left({\mathbf{A}}_2\left(\mathbf{x}\right)\right)\mathbf{x}}{\partial \mathbf{x}}}\right]}^T[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]\mathbf{x}-\mathbf{Q}\mathbf{x}.$$

(37)

Consequently, employing Equations (22) and (29), it is possible to obtain

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial }{\partial t}}[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]\mathbf{x}+[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]{\mathbf{A}}_1\mathbf{x}+[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]{\mathbf{A}}_2\left(\mathbf{x}\right)\mathbf{x}\hfill \\ \hfill & -[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]\mathbf{x}\hfill \\ \hfill & =-{\left[{\mathbf{A}}_1+{\mathbf{A}}_2\left(\mathbf{x}\right)+{\displaystyle \frac{\partial \left({\mathbf{A}}_2\left(\mathbf{x}\right)\right)\mathbf{x}}{\partial \mathbf{x}}}\right]}^T[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]\mathbf{x}-\mathbf{Q}\mathbf{x},\hfill \end{array}$$

(38)

and rearranging terms in Equation (39), we find

$$\begin{array}{cc}\hfill & \left({\dot{\mathbf{K}}}_2\left(\mathbf{x}\right)+{\left[{\displaystyle \frac{\partial \left({\mathbf{A}}_2\left(\mathbf{x}\right)\right)\mathbf{x}}{\partial \mathbf{x}}}\right]}^T[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]\right)\mathbf{x}+\left([{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]{\mathbf{A}}_1+[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]{\mathbf{A}}_2\left(\mathbf{x}\right)\right.\hfill \\ \hfill & -[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]+\left.{[{\mathbf{A}}_1+{\mathbf{A}}_2\left(\mathbf{x}\right)]}^T[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)]+\mathbf{Q}\right)\mathbf{x}=\mathbf{0}.\hfill \end{array}$$

(39)

Based on linearization Equations (34)–(36), let the matrix ${\mathbf{K}}_1$ solve Equation (39), the algebraic Riccati equation (ARE):

$${\mathbf{K}}_1{\mathbf{A}}_1+{{\mathbf{A}}_1}^T{\mathbf{K}}_1-{\mathbf{K}}_1\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_1+\mathbf{Q}=\mathbf{0};$$

(40)

then, the optimality condition is given by

$$\begin{array}{cc}\hfill & {\dot{\mathbf{K}}}_2\left(\mathbf{x}\right)+{\left[{\displaystyle \frac{\partial {\mathbf{A}}_2\left(\mathbf{x}\right)}{\partial \mathbf{x}}}\mathbf{x}\right]}^T{\mathbf{K}}_1+{\left[{\displaystyle \frac{\partial {\mathbf{A}}_2\left(\mathbf{x}\right)}{\partial \mathbf{x}}}\mathbf{x}\right]}^T{\mathbf{K}}_2\left(\mathbf{x}\right)\hfill \\ \hfill & +{\mathbf{K}}_2\left(\mathbf{x}\right){\mathbf{A}}_1+{\mathbf{A}}_1^T{\mathbf{K}}_2\left(\mathbf{x}\right)+{\mathbf{K}}_1{\mathbf{A}}_2\left(\mathbf{x}\right)+{\mathbf{A}}_2^T\left(\mathbf{x}\right){\mathbf{K}}_1\hfill \\ \hfill & +{\mathbf{K}}_2\left(\mathbf{x}\right){\mathbf{A}}_2\left(\mathbf{x}\right)+{\mathbf{A}}_2^T\left(\mathbf{x}\right){\mathbf{K}}_2\left(\mathbf{x}\right)-{\mathbf{K}}_2\left(\mathbf{x}\right)\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_1\hfill \\ \hfill & -{\mathbf{K}}_1\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_2\left(\mathbf{x}\right)-{\mathbf{K}}_2\left(\mathbf{x}\right)\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_2\left(\mathbf{x}\right)=\mathbf{0}\hfill \end{array}$$

(41)

for ${\mathbf{K}}_2\left(\mathbf{x}\right)$ obtained from (36). So, the suboptimal control

$$\mathbf{u}=-{\mathbf{R}}^{-1}{\mathbf{B}}^T\left[{\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right)\right]\mathbf{x}.$$

(42)

For index Equation (19), subject to Equation (18), it is possible to find the solution for the state-independent Riccati Equation (39). In general, it is difficult to find the solution of Equation (39) analytically, especially for complex systems [22,25,33,37]. Symbolic software packages (such Matlab or Mathematica) are commonly used to solve such problems. However, for complex systems, the solution may become complicated, and then, it is necessary to approximate the solution. To approximate, an interpolation method or Taylor series method seems to be most suitable [15].

4. Stability Analysis

Asymptotic stability of the closed-loop system ensures the possibility of controlling the states from the initial values to the final ones. The controlled system with the SDRE compensator-based feedback is locally asymptotically stable [25]. Let $r>0$ be the largest radius, such that ${\mathbb{B}}_r\left(0\right)\subseteq \Omega$. Assuming that the system is stabilizable at $\mathbf{x}=\mathbf{0}$, it is possible to use optimal control theory to define matrix ${\mathbf{K}}_1$, such that all eigenvalues of $({\mathbf{A}}_1-\mathbf{B}{\mathbf{K}}_1)$ have negative real parts. There exist $\beta >0$, such that $Re\left(\mathbf{p}\right)<-\beta$ for all eigenvalues $\mathbf{p}$ of $({\mathbf{A}}_1-\mathbf{B}{\mathbf{K}}_1)$, having the system

$$\dot{\mathbf{x}}=({\mathbf{A}}_1-\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_1)\mathbf{x}+({\mathbf{A}}_2\left(\mathbf{x}\right)-\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_2\left(\mathbf{x}\right))\mathbf{x},$$

(43)

where $({\mathbf{A}}_1+{\mathbf{A}}_2\left(\mathbf{x}\right))\mathbf{x}$ and ${\displaystyle \frac{\partial ({\mathbf{A}}_1\mathbf{x}+{\mathbf{A}}_2\left(\mathbf{x}\right)\mathbf{x})}{\partial \mathbf{x}}}$ are continuous in $\mathbf{x}$ for $\left|\right|\mathbf{x}\left|\right|<r$, where $r>0$ is the largest radius around the origin $\mathbf{x}=\mathbf{0}$.

Let

$$\mathbf{g}\left(\mathbf{x}\right)={\mathbf{A}}_2\left(\mathbf{x}\right)-\mathbf{B}{\mathbf{K}}_2\left(\mathbf{x}\right),$$

(44)

and $\mathbf{h}\left(\mathbf{x}\right)=\mathbf{g}\left(\mathbf{x}\right)\mathbf{x}$; then, the system (34) is described by

$$\dot{\mathbf{x}}=({\mathbf{A}}_1-\mathbf{B}{\mathbf{K}}_1)\mathbf{x}+\mathbf{h}\left(\mathbf{x}\right).$$

(45)

Evaluation of $\mathbf{h}\left(\mathbf{x}\right)$ shows that it is an almost linear system:

$$\underset{\left|\left|\mathbf{x}\right|\right|\to 0}{lim}{\displaystyle \frac{\left|\left|\mathbf{h}\left(\mathbf{x}\right)\right|\right|}{\left|\left|\mathbf{x}\right|\right|}}=0,$$

(46)

from the inequality

$$\left|\left|\mathbf{h}\left(\mathbf{x}\right)\right|\right|\le \left|\left|\mathbf{g}\left(\mathbf{x}\right)\right|\right|\left|\left|\mathbf{x}\right|\right|=\left|\left|{\mathbf{A}}_2\left(\mathbf{x}\right)-\mathbf{B}{\mathbf{K}}_2\left(\mathbf{x}\right)\right|\right|\left|\left|\mathbf{x}\right|\right|.$$

(47)

Thus, $\left|\left|\mathbf{g}\left(\mathbf{x}\right)\right|\right|\to 0$ when $\left|\left|\mathbf{x}\right|\right|\to 0$; then, $\mathbf{h}\left(\mathbf{x}\right)$ satisfies the condition Equation (46). The theoretical considerations of almost linear systems leads to the conclusion that, if eigenvalues $({\mathbf{A}}_1-\mathbf{B}{\mathbf{K}}_1)$ have negative real parts, $\mathbf{h}\left(\mathbf{x}\right)$ is continuous around the origin, and the condition of Equation (46) holds, then $\mathbf{x}=\mathbf{0}$ is asymptotically stable [38].

Let $\delta >0$; then, there exists a $\widehat{\eta }\in (0,r)$, such that $\left|\left|\mathbf{h}\left(\mathbf{x}\right)\right|\right|\le \delta \left|\left|\mathbf{x}\right|\right|$ whenever $\left|\left|\mathbf{x}\right|\right|\le \widehat{\eta }$. Let $\mathbf{x}\left(t_0\right)={\mathbf{x}}_0\in {\mathbb{B}}_{\widehat{\eta }}\left(0\right)$; assuming that f is continuous, the solution exists and remains in ${\mathbb{B}}_{\widehat{\eta }}\left(0\right)$ for $t\in [t_0,\infty ]$, then

$$\mathbf{x}\left(t\right)=e^{({\mathbf{A}}_1-\mathbf{B}{\mathbf{K}}_1)t}\mathbf{x}\left(t_0\right)+{\int }_{t_0}^t{e}^{({\mathbf{A}}_1-\mathbf{B}{\mathbf{K}}_1)(t-s)}\mathbf{h}\left(\mathbf{x}\left(s\right)\right)ds.$$

(48)

Considering the norm and the assumption $\left|\right|\mathbf{x}\left(t\right)\left|\right|<\widehat{\eta }$, it is possible to obtain

$$\begin{array}{cc}\hfill & \left|\left|\mathbf{x}\left(t\right)\right|\right|\le \left|\left|e^{({\mathbf{A}}_1-\mathbf{B}{\mathbf{K}}_1)t}\right|\right|\left|\left|\mathbf{x}\left(t_0\right)\right|\right|+\delta {\int }_{t_0}^t\left|\left|e^{({\mathbf{A}}_1-\mathbf{B}{\mathbf{K}}_1)(t-s)}\right|\right|\left|\left|\mathbf{x}\left(s\right)\right|\right|ds.\hfill \end{array}$$

(49)

There exists a positive constant G and $\beta$, such that

$$\left|\left|e^{({\mathbf{A}}_1-\mathbf{B}{\mathbf{K}}_1)t}\right|\right|\le G{e}^{-\beta t}$$

(50)

and

$$\left|\left|\mathbf{x}\left(t\right)\right|\right|\le G{e}^{-\beta t}\left|\left|\mathbf{x}\left(t_0\right)\right|\right|+\delta G{\int }_{t_0}^t{e}^{-\beta (t-s)}\left|\left|\mathbf{x}\left(s\right)\right|\right|ds.$$

(51)

Referring to the Gronwall inequality and after multiplication by $e^{\beta t}$, we obtain

$$\left|\left|\mathbf{x}\left(t\right)\right|\right|\le G\left|\left|\mathbf{x}\left(t_0\right)\right|\right|e^{-(\beta -\delta \mathbf{G})t}$$

(52)

for all $t>0$, such that $\left|\left|\mathbf{x}\left(t\right)\right|\right|<\widehat{\eta }$. $\delta$ is chosen from $\delta \in (0,{\displaystyle \frac{\beta }{G}})$; then, with respect to $\widehat{\eta }$ having $\gamma \in (0,\widehat{\eta })$, it is possible to obtain $\eta =$ min$\{\widehat{\eta },{\displaystyle \frac{\gamma }{G}}\}$. For all ${\mathbf{x}}_0\in {\mathbb{B}}_{\eta }$, it is possible to obtain $\left|\left|\mathbf{x}\left(t\right)\right|\right|<\gamma \le \widehat{\eta }$ for all $t>0$, and $\mathbf{x}=\mathbf{0}$ is stable.

The condition of Equation (52) is satisfied for all $t>0$ if ${\mathbf{x}}_0\in {\mathbb{B}}_{\eta }$, $\mathbf{x}=\mathbf{0}$ is asymptotically stable, since $\beta -\delta G>0$ [25].

For the proposed method, a global uniform stability can also be proved using the Lyapunov function. The proof can be obtained more easily than for classic SDRE, because feedback compensator gains are state-independent, and for a closed-loop system, there is no problem with generalization and definition of an attraction region.

Assuming that SDC parameterization is stabilizable and detectable for all $\mathbf{x}\left(t\right)$, then the closed-loop matrix ${\mathbf{A}}_{CL}$ is symmetric for all $\mathbf{x}\left(t\right)$, and the solution of SDRE is asymptotically stable when $t\in [t_0,\infty ]$. Let the control law be in the form

$$\mathbf{u}=-{\mathbf{R}}^{-1}{\mathbf{B}}^T({\mathbf{K}}_1+{\mathbf{K}}_2\left(\mathbf{x}\right))\mathbf{x},$$

(53)

and ${\mathbf{K}}_1$ satisfies ARE

$${\mathbf{A}}_1^T{\mathbf{K}}_1+{\mathbf{K}}_1{\mathbf{A}}_1-{\mathbf{K}}_1\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_1+\mathbf{Q}=\mathbf{0},$$

(54)

while ${\mathbf{K}}_2\left(\mathbf{x}\right)$ is taken from

$${\mathbf{K}}_2\left(\mathbf{x}\right)={\left[\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T\right]}^{+}{\mathbf{A}}_2\left(\mathbf{x}\right).$$

(55)

Then, the system in closed-loop form

$$\dot{\mathbf{x}}=({\mathbf{A}}_1-\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_1)\mathbf{x}+({\mathbf{A}}_2\left(\mathbf{x}\right)-\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_2\left(\mathbf{x}\right))\mathbf{x}$$

(56)

is globally asymptotically stable if ${\mathbf{K}}_1$ is the unique symmetric positive-defined matrix.

Let us define the as Lyapunov function

$$V\left(\mathbf{x}\right)={\mathbf{x}}^T{\mathbf{K}}_1\mathbf{x}$$

(57)

and

$$k_1{\parallel \mathbf{x}\parallel }^2\le V\left(\mathbf{x}\right)\le k_2{\parallel \mathbf{x}\parallel }^2$$

(58)

for $\mathbf{x}\in \mathbb{U}$, where $\mathbb{U}$ in ${\mathbb{R}}^n$ has the origin, and constants $k_1$ and $k_2$ are obtained from

$$k_1=\inf {\sigma }_i\left({\mathbf{K}}_1\right)$$

(59)

and

$$k_2=\sup {\sigma }_i\left({\mathbf{K}}_1\right)$$

(60)

for $i=1,2,...,n$.

Using Equations (54) and (56), it is possible to obtain

$$\dot{V}\left(\mathbf{x}\right)={\mathbf{x}}^T{\mathbf{K}}_1\dot{\mathbf{x}}+{\dot{\mathbf{x}}}^T{\mathbf{K}}_1\mathbf{x}.$$

(61)

Taking Equation (56), assuming that ${\mathbf{K}}_2\left(\mathbf{x}\right)$ assures linearization of Equations (45) and (46), and that the solution of ARE, Equation (54), is always symmetric, then

$$\dot{V}\left(\mathbf{x}\right)={\mathbf{x}}^T\left[{\mathbf{A}}_1^T{\mathbf{K}}_1+{\mathbf{K}}_1{\mathbf{A}}_1-2{\mathbf{K}}_1\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_1\right]\mathbf{x}.$$

(62)

Then, taking into consideration (54), it is possible to obtain

$$\dot{V}\left(\mathbf{x}\right)={\mathbf{x}}^T\left[-{\mathbf{K}}_1\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_1-\mathbf{Q}\right]\mathbf{x}.$$

(63)

Let us assume that

$$\dot{V}\left(\mathbf{x}\right)\le -k_3{\parallel \mathbf{x}\parallel }^2,$$

(64)

where

$$k_3=\inf {\sigma }_i({\mathbf{K}}_1\mathbf{B}{\mathbf{R}}^{-1}{\mathbf{B}}^T{\mathbf{K}}_1+\mathbf{Q})$$

(65)

for $\mathbf{x}\in \mathbb{U}$ and $i=1,2,...,n$.

Generally, in an SDRE control problem with infinite time horizon, the existence of constants $k_1,k_2,k_3$ do not assure global stability. The closed-loop matrix is state-dependent but can be generalized to global stability by defining the region of attraction [39].

In the proposed method, the existence of constants $k_1,k_2,k_3$ proposed in Equations (59), (60), and (65) assures global stability, because the matrix ${\mathbf{K}}_1$ is state-independent, so there is no reason to define the region of attraction for the Riccati Equation (54).

5. Numerical Simulation

To present a numerical example, the mentioned quadcopter model was chosen. Typical SDRE control technique was compared with its modified version.

Considering a quadcopter (presented in Figure 1) as a vehicle with four independent drives and with an electric power system located at its center of gravity, it is important to define coordinate systems in order to determine the position. A model device has six degrees of freedom, where vertical movement in the global coordinate system and three Euler angles are controlled parameters. There is a need to assume a body fixed frame and a symmetrical structure of the model with the origin in the center of mass, where every drive is independently controlled and aerodynamic effects are negligible. The mathematical model is taken from [26].

As mentioned, this model needs to be described using Euler’s angles: roll angle $\varphi \in \langle -\pi ,\pi \rangle$, pitch angle $\theta \in \langle -{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}\rangle$, and yaw angle $\psi \in \langle -\pi ,\pi \rangle$. These angles are a sequence of three rotations

$$\begin{array}{cc}\hfill & {\mathbf{R}}_{\mathbf{x}}\left(\varphi \right)=\left[\begin{array}{cccc}& 1& 0& 0\\ & 0& cos\left(\varphi \right)& -sin\left(\varphi \right)\\ & 0& sin\left(\varphi \right)& cos\left(\varphi \right)\end{array}\right],{\mathbf{R}}_{\mathbf{y}}\left(\theta \right)=\left[\begin{array}{cccc}& cos\left(\theta \right)& 0& sin\left(\theta \right)\\ & 0& 1& 0\\ & -sin\left(\theta \right)& 0& cos\left(\theta \right)\end{array}\right],\hfill \\ \hfill & {\mathbf{R}}_{\mathbf{z}}\left(\psi \right)=\left[\begin{array}{cccc}& cos\left(\psi \right)& -sin\left(\psi \right)& 0\\ & sin\left(\psi \right)& cos\left(\psi \right)& 0\\ & 0& 0& 1\end{array}\right].\hfill \end{array}$$

(66)

This leads to the rotation matrix $\mathbf{R}$, which describes rotations of the local system in terms of the global system.

$$\begin{array}{cc}\hfill & {\mathbf{R}}_{\mathbf{z}\mathbf{y}\mathbf{x}}(\psi ,\theta ,\varphi )={\mathbf{R}}_{\mathbf{z}}\left(\psi \right){\mathbf{R}}_{\mathbf{y}}\left(\theta \right){\mathbf{R}}_{\mathbf{x}}\left(\varphi \right)\hfill \end{array}$$

(67)

The representation of the actual model in the simulations requires us to determine differential equations that describe all relations in the model. The orientation of the vehicle is described by two vectors, ${\mathbf{r}}^T=(x,y,z)$ and ${\Omega }^T=(\psi ,\varphi ,\theta )$. The quadrotor’s acceleration is described by the differential equation

$$\ddot{\mathbf{r}}=-g\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]+\mathbf{R}{\displaystyle \frac{b}{m}}\sum _{i=1}^4{\omega }_i^2\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],$$

(68)

where:

g—the gravitational acceleration;

$\mathbf{R}$—the rotation matrix;

b—the thrust factor;

${\omega }_i$—the speed of rotor $i=1,2,3,4$.

The second differential equation is presented in

$$\mathbf{I}\ddot{\Omega }=-\dot{\Omega }\times \mathbf{I}\dot{\Omega }-\sum _{i=1}^4{J}_R\left[\dot{\Omega }\times \left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\right]{\omega }_i+\tau .$$

(69)

Vector $\mathbf{\tau }$ is defined as

$$\tau =\left[\begin{array}{c}lb({\omega }_4^2-{\omega }_2^2)\\ lb({\omega }_3^2-{\omega }_1^2)\\ d({\omega }_2^2+{\omega }_4^2-{\omega }_1^2-{\omega }_3^2)\end{array}\right],$$

(70)

where:

l—the length the of quadrotor’s arm;

d—the drag factor.

The quadrotor’s model has four control inputs (vertical movement in the global coordinate system and three Euler angles). It is assumed that the input values, which represent forces, are directly proportional to the squared angular velocity of the motor.

$$\left\{\begin{array}{c}\begin{array}{c}{u}_1=b({\omega }_1^2+{\omega }_2^2+{\omega }_3^2+{\omega }_4^2)\hfill \\ u_2=b({\omega }_4^2-{\omega }_2^2)\hfill \\ u_3=b({\omega }_3^2-{\omega }_1^2)\hfill \\ u_4=d({\omega }_2^2+{\omega }_4^2-{\omega }_1^2-{\omega }_3^2).\hfill \end{array}\end{array}\right.$$

(71)

Let us create a new variable that describes relative motor speed

$${\omega }_d={\omega }_2+{\omega }_4-{\omega }_1-{\omega }_3.$$

(72)

Using Equations (68)–(72), a dynamic model is presented as a system of six equations

$$\left\{\begin{array}{c}\begin{array}{c}\ddot{x}=(cos\left(\varphi \right)sin\left(\theta \right)cos\left(\psi \right)+sin\left(\varphi \right)sin\left(\psi \right)){\displaystyle \frac{u_1}{m}}\hfill \\ \ddot{y}=(cos\left(\varphi \right)sin\left(\theta \right)sin\left(\psi \right)-sin\left(\varphi \right)cos\left(\psi \right)){\displaystyle \frac{u_1}{m}}\hfill \\ \ddot{z}=-g+(cos\left(\varphi \right)cos\left(\theta \right)){\displaystyle \frac{u_1}{m}}\hfill \\ \ddot{\varphi }=\dot{\theta }\dot{\psi }{\displaystyle \frac{(I_y-I_z)}{I_x}}-{\displaystyle \frac{J_R}{I_x}}\dot{\theta }{\omega }_d+{\displaystyle \frac{l}{I_x}}{u}_2\hfill \\ \ddot{\theta }=\dot{\varphi }\dot{\psi }{\displaystyle \frac{(I_z-I_x)}{I_y}}+{\displaystyle \frac{J_R}{I_y}}\dot{\varphi }{\omega }_d+{\displaystyle \frac{l}{I_y}}{u}_3\hfill \\ \ddot{\psi }=\dot{\varphi }\dot{\theta }{\displaystyle \frac{(I_x-I_y)}{I_z}}+{\displaystyle \frac{l}{I_z}}{u}_4\hfill \end{array}\end{array}\right..$$

(73)

It is possible to describe the model in the state-space form, where

$$\mathbf{u}={\left[\begin{array}{c}{u}_1{u}_2{u}_3{u}_4\end{array}\right]}^T$$

(74)

is the control vector, and $\mathbf{x}\in {\mathbb{R}}^{12}$ is the state vector

$$\begin{array}{cc}\hfill \mathbf{x}=& {\left[\begin{array}{c}x\dot{x}y\dot{y}z\dot{z}\varphi \dot{\varphi }\theta \dot{\theta }\psi \dot{\psi }\end{array}\right]}^T={\left[\begin{array}{c}{x}_1{x}_2\dots x_{12}\end{array}\right]}^T.\hfill \end{array}$$

(75)

Combining Equations (73) and (75), we obtain

$$\dot{\mathbf{x}}=\left(\begin{array}{c}{x}_2\\ (cos{x}_{7}sin{x}_{9}cos{x}_{11}+sin{x}_{7}sin{x}_{11}){\displaystyle \frac{u_1}{m}}\\ x_4\\ (cos{x}_{7}sin{x}_{9}sin{x}_{11}-sin{x}_{7}cos{x}_{11}){\displaystyle \frac{u_1}{m}}\\ x_6\\ -g+(cos{x}_{7}cos{x}_9){\displaystyle \frac{u_1}{m}}\\ x_8\\ x_{12}{x}_{10}{I}_1-{\displaystyle \frac{J_R}{I_x}}{x}_{10}{\omega }_d+{\displaystyle \frac{l}{I_x}}{u}_2\\ x_{10}\\ x_{12}{x}_8{I}_2+{\displaystyle \frac{J_R}{I_y}}{x}_8{\omega }_d+{\displaystyle \frac{l}{I_y}}{u}_3\\ x_{12}\\ x_{10}{x}_8{I}_3+{\displaystyle \frac{l}{I_z}}{u}_4\end{array}\right),$$

(76)

where $I_1={\displaystyle \frac{(I_y-I_z)}{I_x}}$, $I_2={\displaystyle \frac{(I_z-I_x)}{I_y}}$, and $I_3={\displaystyle \frac{(I_x-I_y)}{I_z}}$.

Considering the attitude of the control, and applying SDRE method, the state vector

$${\mathbf{x}}_{\mathbf{I}}={\left[\begin{array}{c}{x}_7{x}_8{x}_9{x}_{10}{x}_{11}{x}_{12}\end{array}\right]}^T,$$

(77)

and the control vector

$${\mathbf{u}}_{\mathbf{I}}={\left[\begin{array}{c}{u}_2{u}_3{u}_4\end{array}\right]}^T.$$

(78)

After factorization, it is possible to obtain

$${\dot{\mathbf{x}}}_{\mathbf{I}}=\left[\begin{array}{ccccccc}& 0& 1& 0& 0& 0& 0\\ & 0& 0& 0& x_{12}{I}_1& 0& 0\\ & 0& 0& 0& 1& 0& 0\\ & 0& x_{12}{I}_2& 0& 0& 0& 0\\ & 0& 0& 0& 0& 0& 1\\ & 0& 0& 0& x_8{I}_3& 0& 0\end{array}\right]{\mathbf{x}}_{\mathbf{I}}+\left[\begin{array}{cccc}& 0& 0& 0\\ & {\displaystyle \frac{l}{I_x}}& 0& 0\\ & 0& 0& 0\\ & 0& {\displaystyle \frac{l}{I_y}}& 0\\ & 0& 0& 0\\ & 0& 0& {\displaystyle \frac{l}{I_z}}\end{array}\right]{\mathbf{u}}_{\mathbf{I}}.$$

(79)

The above state-space model has SDC structure, where $\mathbf{A}\left({\mathbf{x}}_{\mathbf{I}}\right)$ is state-dependent matrix, and $\mathbf{B}$ is constant, as assumed in Equation (18).

Applying the proposed methodology and replacing $\mathbf{A}$ by a sum of two matrices as presented in Equation (20) (for proposed method), it is possible to obtain

$${\dot{\mathbf{x}}}_{\mathbf{I}}=\left[\begin{array}{ccccccc}& 0& 1& 0& 0& 0& 0\\ & 0& 0& 0& 0& 0& 0\\ & 0& 0& 0& 1& 0& 0\\ & 0& 0& 0& 0& 0& 0\\ & 0& 0& 0& 0& 0& 1\\ & 0& 0& 0& 0& 0& 0\end{array}\right]{\mathbf{x}}_{\mathbf{I}}+\left[\begin{array}{ccccccc}& 0& 0& 0& 0& 0& 0\\ & 0& 0& 0& x_{12}{I}_1& 0& 0\\ & 0& 0& 0& 0& 0& 0\\ & 0& x_{12}{I}_2& 0& 0& 0& 0\\ & 0& 0& 0& 0& 0& 0\\ & 0& 0& 0& x_8{I}_3& 0& 0\end{array}\right]{\mathbf{x}}_{\mathbf{I}}+\left[\begin{array}{cccc}& 0& 0& 0\\ & {\displaystyle \frac{l}{I_x}}& 0& 0\\ & 0& 0& 0\\ & 0& {\displaystyle \frac{l}{I_y}}& 0\\ & 0& 0& 0\\ & 0& 0& {\displaystyle \frac{l}{I_z}}\end{array}\right]{\mathbf{u}}_{\mathbf{I}}.$$

(80)

To present the influence on the system, weight matrices were changed during this experiment. There were three different configurations:

$$\begin{array}{cc}\hfill & \mathbf{Q}=1·\left[\begin{array}{c}{\mathbf{I}}_{6\times 6}\end{array}\right],\mathbf{R}=100·\left[\begin{array}{c}{\mathbf{I}}_{3\times 3}\end{array}\right],\hfill \\ \hfill & \mathbf{Q}=1·\left[\begin{array}{c}{\mathbf{I}}_{6\times 6}\end{array}\right],\mathbf{R}=1·\left[\begin{array}{c}{\mathbf{I}}_{3\times 3}\end{array}\right],\hfill \\ \hfill & \mathbf{Q}=100·\left[\begin{array}{c}{\mathbf{I}}_{6\times 6}\end{array}\right],\mathbf{R}=1·\left[\begin{array}{c}{\mathbf{I}}_{3\times 3}\end{array}\right].\hfill \end{array}$$

(81)

The values of parameters used in modeling of the object are presented below in

Table 1. Parameters used in simulation.

Parameter	Value	Unit
g	9.81	${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$
m	0.5	kg
l	0.3	m
$I_x$	0.0081	kgm${}^2$
$I_y$	0.0081	kgm${}^2$
$I_z$	0.0162	kgm${}^2$
$J_R$	0.01	N

[40].

Initial state and velocities were set as

$${\mathbf{x}}_0=\left[\begin{array}{c}\pi /4\\ \pi /2\\ 3\pi /4\\ 0\\ 0\\ 0\end{array}\right]\left[\begin{array}{c}\mathrm{rad}\\ \mathrm{rad}\\ \mathrm{rad}\\ \mathrm{rad}/\mathrm{s}\\ \mathrm{rad}/\mathrm{s}\\ \mathrm{rad}/\mathrm{s}\end{array}\right].$$

(82)

Simulation time was $5s$ and time step was $1\times {10}^{-3}s$.

The aim of the control process is stabilization at “zero” point.

The comparison of the execution time for both methods (

Table 2. Simulation time.

Comparison of Simulation Time
classical SDRE
${\mathbf{R}}_1,{\mathbf{Q}}_{100}$	11.276407 s
${\mathbf{R}}_1,{\mathbf{Q}}_1$	11.978995 s
${\mathbf{R}}_{100},{\mathbf{Q}}_1$	11.224440 s
modified SDRE
${\mathbf{R}}_1,{\mathbf{Q}}_{100}$	2.893052 s
${\mathbf{R}}_1,{\mathbf{Q}}_1$	2.433471 s
${\mathbf{R}}_{100},{\mathbf{Q}}_1$	2.590412 s

) shows that the proposed method reduces computational effort, which can be seen in the shortened time of computation below. The computations were executed on Windows 7 Professional, 64-bit Intel^® Core™ i5-5200M CPU @ 2.40GHz.

Simulation results for configuration ${\mathbf{R}}_1,{\mathbf{Q}}_{100}$ are presented. Differences between classical and modified SDRE method for next two configurations are unnoticeable.

In every case of $\mathbf{Q}$ and $\mathbf{R}$ matrices configuration after a short transition phase, all angles are stabilized at a required point (Figure 2, Figure 3 and Figure 4). The waveforms for both methods are convergent. Changing the value of $\mathbf{Q}$ and $\mathbf{R}$ matrices affects only the speed of regulation. Small differences are due to numerical errors and pseudoinversion inaccuracy.

6. Conclusions

The modeling and control of a quadcopter using the SDRE method and modified SDRE-based proposition were presented in this paper. A new method for solving a suboptimal control problem is established, and a procedure for solving the problem was presented and illustrated with a numerical example. As shown, the modified SDRE method gives the same solution as the classic SDRE method, but in the modified case, there is no need to solve a state-dependent Riccati equation for each state of the control process. This allows us to save space for implementation in real-time control systems and simplifies computations.