Execution Time of Optimal Controls in Hard Real Time, a Minimal Execution Time Solution for Nonlinear SDRE

Many engineering fields, such as automotive, aerospace, and the emerging challenges towards industry 4.0, have to deal with Real-Time (RT) or Hard Real Time (HRT) systems, where temporal constraints must be fulfilled, to avoid critical behaviours or unacceptable system failures. For this reason, estimation of code’s Worst-Case Execution Time (WCET) has received lots attention because in RT systems a fundamental requirement is to guarantee at least a temporal upper bound of the code execution for avoiding any drawbacks. However, until now there is no approved method to compute extremely tight WCET. Nowadays, indeed, HRT requirements are solved via hardware, using multi-cores embedded boards that allow the computation of the deterministic Execution Time (ET). The availability of these embedded architectures has encouraged the designers to look towards more computationally demanding optimal control techniques for RT scenarios, and to compare and analyze performances also evaluating a tight WCET. However, this area still lacks deep investigations. This paper has the intent of analysing results regarding the choice between three of the most established optimal controls (LQR, MPC, SDRE), providing the first link between WCET analysis and control algorithms performances. Moreover, this work shows how it is also possible to obtain a minimal ET solution for the nonlinear SDRE controller. The results might be useful for future implementations and for coping with Industry 4.0 emerging challenges. Furthermore, this approach can be useful in control system engineering field, especially in the design stage for RT or HRT systems, where temporal bounds have to be fulfilled jointly with all the other application’s specifications.


I. INTRODUCTION
The embedded hardware systems used for control applications are becoming more and more performing to meet the growing needs, in terms of computational performances, required by the increasing complexity of control tasks, for which they are appointed in real-world applications. Many engineering fields, such as automotive, aerospace, and in The associate editor coordinating the review of this manuscript and approving it for publication was Choon Ki Ahn . recent years also the emerging challenges towards industry 4.0, have been already using Real-Time (RT) or Hard Real-Time (HRT) systems to deal with event-triggered and timetriggered tasks [1]. This is because of the increasing number of sensors and even more sophisticated control algorithms, require that the system cannot lose deadlines, especially if they involve safety concerns. The management and implementation of these algorithms are distributed among different tasks running on the embedded system while RT or HRT temporal constraints must be fulfilled as well, to avoid critical behaviours or unacceptable system failure. Therefore, these HRT systems must be designed according to the resource adequacy policy, providing sufficient computing resources to handle both the specified worst-case load and fault scenario. However, in order to achieve even more demanding objectives to control systems, the linear controllers have to be overcome because in most of the control applications the PID is still the most used controller. For example, in an industrial environment, linear control algorithms are preferred to facilitate the PLC to save resources since it has to manage different architectures and respects different RT constraints. Whereas in the Automotive field, RT or HRT systems are used to guarantee security and performance to the vehicles and they often use nonlinear controllers due to the need to handle complex dynamics models [2]. For example advanced models of two-wheeled vehicles, such as those proposed in [2] for an all-wheel-drive vehicle, or in [3] for single-tracked vehicle, require at least seven coupled nonlinear equations of motion. Nevertheless, to achieve even more sophisticated Industry 4.0 objectives and to control new forthcoming systems, not in trivial fashion, the widely used PID linear controller has to be overcome. To satisfy the temporal constraints and to guarantee correctly working systems, research on timing analysis of RT systems started many years ago and has been focused on the Response Time Analysis (RTA) [4], [5], which refers to the time that a message requires to be sent and received. Indeed, many works have been devoted to the development of methods based on deterministic RTA, for an estimation of the code ET. Even though many factors have been considered, it is still difficult to analyze a real system using those methods. One of the main reason is the unpredictable randomness of the Central Processing Unit (CPU) scheduling which cannot be accurately modelled. Because of these deficiencies, usually, some degrees of pessimism are added to the model as a price for indeterminism, and the Worst-Case Execution Time (WCET) is evaluated. The WCET has received lots of attention because, in RT or HRT critical systems, it is a fundamental requirement to at least guarantee a temporal upper bound for the code execution and avoid any drawbacks. However, until now, there is no approved method to compute extremely tight WCET, as shown in [6]. Nowadays, HRT requirements can also be solved via hardware, using multicores embedded boards that allow the computation of the deterministic Execution Time (ET). In these boards, the running task is considered on a single thread, there is no CPU scheduling and each routine has a dedicated core. There are different companies specialized in HRT embedded boards, amongst them, a few years ago the benefits of XMOS technology have been pointed out in comparing with other boards [7]. This allows us to calculate with deterministic certainty, the Best Case Scenario (BCS) and Worst Case Scenario (WCS) with native tools by the workbench. The availability of this embedded architecture has encouraged the designers to look towards more computationally demanding optimal control techniques for RT scenarios and to compare and analyse performances also evaluating a tight WCET. Indeed, this area still lacks in-depth investigations while plenty of papers have been produced in the literature addressing the implementation of different control laws on single-core embedded architectures [8]. Therefore, the aim of this paper has been primarily to compare optimal control techniques through performance and ET analysis. This was possible by exploiting the HRT board characteristics, and it has been proposed a method to evaluate in a deterministic way the Execution Time required by the controller's implementations. Furthermore, once evaluated both performance and ET of three most used optimal controls, in the light of this novel, proposed analysis, it has been possible to improve the execution time of the State Dependent Riccati Equation (SDRE) nonlinear controller. The improvements are related to implementation issues and it has been proposed a method to achieve a minimal ET solution, useful for control regulation problems, which decrease the computational effort required by the SDRE nonlinear control algorithm. Amongst various control techniques, the optimal controls ranging from linear to advanced linear up to nonlinear, have been considered. In embedded system energy saving is crucial due to limited energy availability and optimal controls allow to accomplish control tasks also taking into account energy minimization. Moreover, optimal controls can handle systems which have strong dynamics couplings, even with significant non-linearities where the need to manoeuvre the systems in a non-trivial fashion to achieve good performances is required. They can also be used to open-loop trajectory optimization, e.g. in robotics, where they are extremely useful for robot's motion-planning [9]. This paper focuses mainly on discrete-time implementations [10], being those of greater interest for practical applications. Discrete-time version of Linear Quadratic Regulator (LQR) [11], Model Predictive Control (MPC) technique [12] and nonlinear technique based on the State Dependent Riccati Equation (SDRE) [13] have been considered and analyzed with an HRT board. The control laws have been tested first in a simulation environment and then applied to an experimental set-up using the HRT board based on XMOS technology. To this purpose, a motorized four-wheeled mobile robot has been built, it is equipped with an inverted pendulum mounted on top of and joined by means of a rotational joint. A control law has to keep the pendulum in balance while having the cart tracking the desired position in space. The system's sensing is accomplished providing data by an optical encoder measuring the angle of the pendulum, and by a distance sonar sensor tracking the cart position w.r.t. a fixed obstacle.
The remainder of this paper is organized as follows: in Section II an overview of related work is presented, Section III introduces the linear and nonlinear discrete models of the cart-pendulum system in state-space form; Section IV recall the theory behind the optimal control laws used to stabilize the system and provides some simulation results; in Section V a new minimal ET solution for nonlinear SDRE controller is presented while Section VI describes the hardware platform and the sensors used in the real control application. Section VII describes the differences regarding the implementation between classic SDRE and the minimal ET solution proposed. The Simulations and Experimental results are shown in Section VIII are shown, and Section IX concludes the paper.

II. RELATED WORK
In this subjects, most relevant works are presented considering the issues discussed above. Engineers of computer science have investigated a lot of methods to achieve a deterministic estimation of the WCET. The determination of upper bounds on ET is a necessary step in Real-time Computing (RTC) which refers to systems subject to RT constraints. Different work [14]- [17], highlight the importance of the predictability's timing behaviour for RT embedded systems. Some work propose static and probabilistic analysis of the WCET as shown in [18], [19] and [20], while the probability response time distribution of periodic tasks on a uniprocessor system was developed in [21]. The approaches can be either pragmatic with a simulation [17] or based on the statistic methods [14]. Another possibility is to use estimation algorithms, to obtain a prediction of the future execution timing [15]. However, most existing analysis methods are based on deterministic RTA, and there is still no approved method to compute extremely tight WCET [14], [15]. Several tools and algorithms are available on the market that allow calculating the ET of the tasks with some degree of pessimism. Nevertheless, these tools can ensure reliability in HRT system only considering a higher limit that exceeds in every task the real WCET [22], [23]. In the field of embedded system, these problems are overcome using micro-controllers developed to HRT problems. Amongst various producers, the XMOS family has been chosen because deemed suitable for the novel analysis carried out in this paper and introduced in the section above. Indeed, until now at best knowledge of the authors, there are no works focused on the WCET of the control algorithms used for RT or HRT applications which aims to improve the controllers' feasibility. In literature, progress based on sensor selection in control design receives a substantial interest in the last few years. For example, in [24] a Linear-Quadratic-Gaussian (LQG) control is applied to a Maglev suspension and it has been pointed out the significance of achieving also, for RT scenario, even more performing solutions to combine multi-objective optimization through an adequate sensor choice. Regarding the control techniques, different works deal with optimal control laws and suggest a discrete implementation on microcontrollers [10]. Some works are focused on the evaluation of the performances of these optimal controllers, mainly in simulation and sometimes also experimentally. In [25] the study was performed on a simulated model of an inverted pendulum, and it has been shown that the LQR algorithm works better for stabilization problems and disturbance rejection, while the MPC controller is more suitable for the trajectory tracking task. In [26], two optimal control techniques such as LQR and SDRE, have been applied to a double inverted pendulum on a cart and these were investigated and compared. In particular, simulations reveal the superior performance of the SDRE over the LQR under strong nonlinear conditions, and some improvements that could be provided by the Neural Networks, which compensates model mismatching in the case of LQR. Differently in [27] was tested a DC motor speed controller. The simulations show that the control efforts are lower in PID and LQR than in MPC, but the MPC outperforms for reference tracking and constraints handling. In [28], assuming the same linear control parameters, the effectiveness of the SDRE control over the LQR control is demonstrated despite a more complex design. Besides, in [29] traditional optimal strategies such as LQR and SDRE are investigated when applied to the control of spacecraft formation flying. In this case, the accuracy and the cost of maintaining the requested orbital configuration are evaluated, and the analysis shows as the SDRE allows to better take into account the real dynamics at an increasing level of approximation. However, despite the above-mentioned analyzes and comparisons performed on these techniques, a timing analysis seems not has been deepened. In this regard, this paper aims to propose a novel approach to evaluate control techniques by a thorough analysis of both the computational performances and the WCET of the optimal control algorithms taking into account the already well-known validated implementations. The analysis carried out on an RT experimental set-up (a cart-pendulum mobile robot depicted in Fig.1) has the intent of showing results regarding the choice between three of the most established optimal controls, providing the first link between WCET analysis and the control algorithms performances. Moreover, a novel minimal ET solution for the nonlinear SDRE controller is proposed. These results might be useful also for future implementations to solve Industry 4.0 challenges, where Multi-Agents Systems have to collaborate and exchange messages without loose deadlines in RT. Indeed, the tendency to bring the calculation of CPS towards the edge computing, and therefore on lowcost and lower performance embedded devices, as described in [30], [31], requires the use of less computationally demanding control algorithms. Otherwise, once designed an SDRE nonlinear controller based on the system model, it is possible to develop more easily a digital twin to monitoring parameters and system's conditions in broader functioning range and to apply a predictive maintenance procedure, as proposed in [32]. However, the proposed analysis could be useful in control systems engineering especially during the control's design stage for RT or HRT systems, where temporal bounds have to be fulfilled jointly with all the other application's specifications.

III. CART-PENDULUM MODELS
In this section, the model of the cart-pendulum robotic system depicted in Figure 1 has been derived. Firstly, the non-linear model is presented, and it is shown how to obtain both the nonlinear State-Dependent Coefficient (SDC) Factorization, and the discrete-time model in state-space form, which have been used in control implementation. Then, the model derived has been trivially linearized for developing the LQR and MPC linear control techniques as well. The different control laws will be recalled in the next sections.
To obtain the above mentioned discrete-time models, the nonlinear continuous-time dynamic equations for the cart-pendulum system in Figure 1 has been developed first. Then, the system has been rewritten in its statespace linear-like form for subsequent nonlinear control implementation.
Meaning and description of the variables and physical parameters in Eq.(1) are listed in Table 1. The system's control input is the motor voltage, here defined as E, it actuates the electric motors connected with the wheels, their dynamics generates external forces and moments, here defined as F (see Fig. 1 ). The modelling details of motors-cart interaction are reported in Appendix B.

B. SDC FACTORIZATION OF NONLINEAR CART-PENDULUM MODEL
The system (1) can be written in linear-like form obtaining the SDC matrix. The system's state is defined as x s = [x,ẋ, φ,φ] T , the continuous-time system's SDC matrix is A c nl (x s ), the system is control-affine, its input matrix is B c nl , the output matrix is C nl while u = E is the control input. Therefore the linear-like form of the system can be written as: The variables x,ẋ represent the cart's distance and the linear velocity respectively, while φ,φ are the pendulum's angular displacement and angular velocity, respectively. Appendix A, explains how to derive a SDC matrix solution. The SDRE control technique, has been chosen for consistency of analysis, wishing to focus the paper on optimal control methods. By manipulating equations (2) following recommendations are given in Appendix A, the system matrices become the follows (3), as shown at the bottom of the next page, where for reasons of space the dependence on the state (A c nl (x s )) of has been here omitted with s 3 = sin (x 3 ), c 3 = cos (x 3 ) and η nl = (J p +mL 2 )(M +m)−m 2 L 2 cos 2 (x 3 ). To ease the reading, the matrices A c nl (x s ), B c nl (x s ) can been arranged as: In order to implement the nonlinear controller on the proposed board for computational performance and WCET analysis, the discrete-time non linear model of (3) is derived by using the Euler method with sampling time T , giving VOLUME 8, 2020 rise to following discrete-time model: with u[k] = E[k] and discrete-time matrices Ta 23 , In (14) the terms a ij ,b ij are the elements of continuous matrices A c nl , B c nl described in the equations (5) to (12).

C. DISCRETE-TIME CART-PENDULUM LINEAR MODEL
This subsection, briefly introduces the discrete-time linear model here used to design the linear optimal control strategies described in the following Section. The model is obtained from the equations of motion (1) under small perturbation hypothesis φ 0 around the upright position, and hence (15), it is trivial to derive the discrete-time state space model using Euler method with sampling time T . It has the form: with

u[k] = E[k] and matrices
where k is the sampling instant and the terms a ij , b ij are elements of the matrices A c nl , B c nl in (3).

IV. OPTIMAL CONTROL LAWS
The present section is devoted to briefly recall the theory behind the three different optimal control techniques considered in this work to regulate the upright pendulum position of the cart-pendulum robotic system. The controllers have been used both in simulation and on the experimental set-up.
In order to ease the paper, some implementation details which require longer description are given in the appendix, while only necessary conceptual steps for a clear understanding are presented below. Moreover, considering the issues related to this work, different control features will be pointed out because useful for further performance's analysis. We refer to the discrete-time version of the three methods. The linear techniques LQR and MPC, make use of the model (16). The nonlinear SDRE control uses model (13).

A. THE DISCRETE-TIME LQR
The LQR design technique is well known in modern optimal control theory and has been widely used in many applications.

1) LQR FUNDAMENTALS
In LQR theory a stabilizable discrete-time linear system: is the state vector and u[k] the system's input, the optimal inear control problem is to determine the linear optimal feedback matrix K opt ∈ R nxm where n, m are the state and input vectors dimension respectively, such that: we refer to (16) as the discrete-time linear plant model (24). In this paper, the state feedback controller is designed using the linear quadratic regulator and the discrete-time linear model of the system (16). Here, the infinite-time regulation problem has been solved to guarantee stability for further performance and WCET analysis. This solution leads to search for optimal control u k which minimizes the following cost function: where Q k , R k are the state and input weighing matrices respectively, they are symmetric and positive defined, and k is the sampling instant. In order to ease the paper readability, the subscript s in the state vector (x s ) has been omitted. It has been demonstrated that the optimal control input u k is given by: where the matrix P is the solution of the Discrete Algebraic Riccati Equation (DARE) problem: Then, once computing off-line the DARE the linear optimal control input is obtained as well.

2) LQR FEATURES
In the conventional LQR design method, the DARE problem (28) is solved using numerical or iterative methods. For regulation problems, the DARE is solved once. Although, the LQR provides a simple solution its application is limited to linear systems or a linear approximation of nonlinear systems. Linear approximations are quite simple to handle in regulation problems, but for trajectory tracking problems, the LQR finds limits when dealing with not negligible nonlinearities. Often the solution can be found linearizing the system in a subset of the trajectory points along the whole trajectory but this is time-consuming whit respect to other techniques involving nonlinear controllers [39].

B. DISCRETE-TIME MPC
MPC has become a widely used control in the last decades, especially in industrial applications because of its versatility and capability to perform industrial process optimization. The linear MPC approach has been here considered to undertake a common comparison framework. A comprehensive theory of the MPC can be found in [12], but it is out of the scope of this paper. Thus, here the main implementation steps have been recalled, while more details are given in Appendix C.

1) MPC FUNDAMENTALS
For discrete-time MPC implementation, we refer to the discrete time plant model (16). The MPC theory is based on the extended model (53) reported in Appendix C, and is derived by defining an augmented state x e [k], wherein the variable u[k] becomes the new input to be controlled in place of u [k]. How to derive the extended model and all the related mathematical manipulations is detailed in the appendix C. As for the other optimal controllers, the optimal control input u[k] is obtained by minimizing a cost function J , defined as follow: where, the first term is related to the minimization between the predictive output Y and the setpoint R s , while the second one U T R U is referred to the size of U. Indeed, R = r ω I , and r ω is used as a tuning parameter for closed-loop performance. The optimal U minimizing the cost function J holds: is the state feedback control within the framework of predictive control, where the F matrix meaning is derived from the extended model formulation given in Appendix C.

2) MPC FEATURES
The optimal input U of equation (30), contains the controls with the receding horizon window (N c ). However, only the first sample of this sequence needs to be implemented, i.e., u[k i ], while ignoring the rest of the sequence. When the next sample period arrives, the more recent measurement is taken to form the state vector x e [k i+1 ] for calculation of the new sequence of the control signal, as shown in the following: where K y is the first element of ( T This procedure, when iterated in real-time gives rise to the receding horizon control law. Moreover, the optimal input u[k i ] implies that a discrete integrator is to be embedded in the closed-loop to derive the right u[k] for the plant. This integration acts as a low-pass filter on the controlled quantity making its transitions smoother; indeed the predicted optimal control input at time-instant k is obtained as The MPC key feature which should be highlighted is the versatility of control input implementation (31). Indeed, the derived implementation is valid both for stabilization and trajectory tracking problems. This means that in terms of computational effort and WCET of the code, having a constant or variable set-point which is changing over time does not affect the VOLUME 8, 2020 design and the computational effort required by the MPC control algorithm. Indeed, in (31) K y is a constant weighting matrix multiplied for the reference value r(k i ) at the k i instant. Thus, for each next instant, a change in the set-point value is taken into consideration producing the respective optimal control input u[k + 1].

C. THE DISCRETE-TIME SDRE
The SDRE strategy has become popular within the control community over the last decade, it provides an effective model-based method for nonlinear feedback control synthesis, by allowing nonlinearities in the system states while additionally offering good design flexibility through statedependent weighting matrices. Different papers have been produced regarding the design and the implementation issues related to the non-uniqueness of the SDC matrix factorization [33], [34], this also for trajectory tracking control problems [35]. It has been demonstrated with a correct SDC factorization strong nonlinear behaviours can be handled as well [36]. However, achieving a new SDC matrix design is out of the scope of this paper and for this reason, a common approach has been used as proposed in [37], and the latter has been discussed in more detail in the dedicated appendix A.

1) SDRE FUNDAMENTALS
The SDRE technique here recalled has been based on the SDC parametrization of the plant model obtained above in (3), which represents a linear-like state space form of the nonlinear system. The cost function to be minimized with respect to the control u[k] is: (32) One of the key advantages offered by SDRE is the tradeoff between control effort and state errors, and it can be achieved through tuning of weighting matrices Q(x) and R(x). These weighting matrices can be chosen to be either constant or state dependent. However, in our application Q and R are assumed to be constant matrices to simplify the implementation and because constant matrices ensure the closed loop performance. In this case, taking into account the finite horizon problem, the sub-optimal control u[k] is given as: where P(x [k]) is computed at each sampling time k as the solution of the following DARE: Thus, unlike the linear case, the DARE equation has to be solved on-line at each control iteration.

A. MET-SDRE BACKGROUND
The MET-SDRE algorithm aims to decrease the code-ET of the discrete-time SDRE control algorithm maintaining the same performance. This is achieved by reducing the computation time required by the DARE (34) in the SDRE algorithm in an adaptive way. This means the need to solve the DARE is evaluated adaptively by introducing a sort of system's distance from the linearity range, which is based on the degree of similarity of the SDC state-matrices between two consecutive sampling times. This result has been found analysing the convergence problem of the P ( Taking into account these considerations, we propose to modify the DARE solver of the SDRE control algorithm making it adaptive with respect to the system's state. This means allowing it to have a faster convergence for the DARE solution. In the following, the implementation details of the proposed solution are explained. The computational solution implements two basic steps that are: 1) evaluation of matrices' similarity and 2) adaptive P initialization.

1) EVALUATION OF MATRICES' SIMILARITY
In order to evaluate the similarity (or distance) between two numerical matrices, the tools proposed in [40] have been used. Given a discrete-time nonlinear system represented in the linear-like form (3), considering two consecutive instants k and k + 1, and the the corresponding consecutive numerical SDC matrices, namely A d nl (x[k]) and A d nl (x[k + 1]), two similarity criteria (d A ; d λ A ) can be established as follow: where d A represents the pure distance between two numerical SDC matrices in two time intervals, while d λ A represents the distance between the eigenvalues of the frozen matrices in the two time intervals.
Thus, out of the linear-range the biggest numerical difference between two consecutive matrices can be measured with the above introduced metrics, the bigger the distance the larger the number of backward iterations for solving the DARE.

2) ADAPTIVE P INITIALIZATION
As explained in the above Section, the P chosen to initialize the SDRE algorithm affects the convergence speed of the DARE solution P(x[k]). For this reason, an adaptive P initialization based on the current system state x[k] can improve the ET required by the SDRE algorithm. A possible solution is to calculate off-line P(x[k]) for different working ranges (e.g. linear and nonlinear ranges) and to define different SDRE algorithm initialization conditions. Trivially, the proposed methodology should be evaluated depending on the application. In this paper, the proposed solution is applied to the cartpendulum mobile robot and is detailed in the Experimental results Section VIII-C.

VI. HARDWARE SET-UP
In this Section are presented the hardware characteristics of the whole system and the control strategies implemented in the HRT board to stabilize the pendulum in upright position while the cart is maintaining the desired distance from obstacles.

A. CART-PENDULUM ROBOTIC SYSTEM
As discussed in the Introduction section, the hardware chosen to control the cart-pendulum system consists of the XMOS XK-1A board, a low cost development platform intended for exploring parallel computation. The XK-1A comprises 128KBytes SPI FLASH memory, four LEDs, and two pressbutton switches. An XTAG-2 debug adapter can be connected to a PC to debug the XK-1A operations. The XK-1A is based VOLUME 8, 2020 on a single XS1-L1 device in a 128TQFP package. The XS1-L1 hosts four deterministic cores operating at 100Mhz and it provides a tightly integrated general purpose I/O pins and 64 KBytes of on-chip RAM. The YUMO E6A2-CWZ3E Rotary Encoder sensor provides the measure of the pendulum angle while the HC-SR04 ultrasonic distance sensor provides precise, no contact distance measurements up to about 3 meters from the reference obstacle. It is simple to connect the sensor to micro-controllers requiring only one I/O pin. The DC motors are operated by the shared output voltage provided by a Sabertooth system 2 × 12 motor driver. This circuit can be powered with voltage ranging from 6 to 24 V, with up to 12 A continuous per channel. The board is used in R/C Mode/Microcontroller mode with a PWM from 1000us to 2000us, respectively corresponding from −12V to +12V. The whole system is powered by a LiPo battery, 2200mAh, 3S (3 cells), with discharge rate 65C and 12V.

B. HRT CONTROL STRATEGIES
The XK-1A board of the XMOS family has been used to implement the control algorithms described above. This technology was developed a few years ago, and currently new more performing boards are available from the XMOS company but the XK-1A hardware characteristics are still appropriated for this paper. The closed-loop controls are based on the direct measures of the cart position (x) and the pendulum angle (φ) provided by the sensors. At each sample time the controllers compute the feedback control law and the optimal voltage (u[k]) to be applied to the motors driver (i.e. the sabertooth device), in order to keep the pendulum in balance (φ = 0) while maintaining the cart to the desired x position. The sensors are directly connected to different I/O pins of the board. The three different control strategies have been entirely coded on the XK-1A XMOS board: where at each of the four deterministic cores has been assigned a task. The core 1 runs the task 1 that manages the gathering of data from the position sensor. The core 2 hosts the task 2 that manages the gathering of data from the encoder for the feedback on the φ angle. Both tasks are in charge of making available their data, through the respective channels, to the task 3. This latter runs on core 3 and retrieves the sensor data from the proper channel every 20ms, within this time laps task 3 must compute the control law and send the message to the task 4 running on core 4 which generates the PWM to drive the motors. All the tasks are parallel and their ET is computed statically by means of the TimeAnalyzer, a feature provided by the development environment XTimeComposer of XMOS. The synchronization between the cores operations has been achieved by calculating statically the computing time of each task.

VII. CLASSIC SDRE AND MET-SDRE NONLINEAR IMPLEMENTATIONS
In this subsection the implementation of the algorithms regarding the classic SDRE and the proposed MET-SDRE algorithm solution, in the HRT board are explained in more detail.

A. CLASSIC SDRE ALGORITHM
Regarding the classic SDRE algorithm, common features have been already explained in Subsection IV-C2. In this part, the classic SDRE implementation used for stabilization problems on the cart-pendulum mobile robot is analyzed in more detail. Further implementation issues and feasibility aspects have been deeper discussed in [33], [38], where are given also the guidelines regarding the construction of the SDC matrix when the SDRE solvability condition is violated. The implementation steps are the following: i) The DARE (34) is solved off-line until the P(x[k]) off reaches the convergence imposing specific system condition (e.g. φ = 0.06 rad andφ = 0 rad/s).
ii) The necessary backward iterations for on-line implementation are found as discussed in Subsection IV-C2. It has been experimentally found that within 20 backward iterations the norm error criteria imposed as P(x[k]) off −P(x[k]) 20 < with = 0.1 was satisfied. This test, was repeated for different points of work of the cart-pendulum mobile robot to guarantee a consistent control law u(x[k]) during the experimental phase.
iii) Every sample time k, the SDRE control algorithm solves the DARE (34) 20 times backwards iteratively.

B. MET-SDRE ALGORITHM
The idea of this MET-SDRE solution has been achieved through the WCET analysis experienced in this work and it aims to improve the classic SDRE approach in terms of ET. In the following, the implementation steps are listed below: i) The linear working range of the system has been estimated through experimental tests, e.g. in our case the linear working range of the cart-pendulum is φ m ≤ φ ≤ φ M , namely boundary conditions, where in our experimental setup φ m = −4 • and φ M = 4 • .
ii) It has been calculated off-line a convergence solution P(x[k]) 1000 in 1000 backward iterations for the three different boundary conditions of the system φ = −4 • , 0 • , 4 • . These solutions have been named iii) The previous solutions have been used to initialize the adaptive DSDRE control algorithm to reach a faster convergence speed depending on the system's working range.
In Figure 2, the flow chart of the MET-SDRE control algorithm is shown. The initialization of the Algorithm is composed by the reading of the initial system's output (Y 0 ) and the computation of the state (X 0 ), then the matrix P 0 is chosen based on the initial state (X 0 ). The periodic control task in this implementation last 20ms, it consists of the computation of the current state based on the output acquisition Y k . In order to compute the DARE convergent solution P(  in Section V. Finally, the control input u(X k ) is computed, and the periodic task stops upon termination decided by the user.

VIII. PERFORMANCE'S ANALYSIS AND WCET OF THE OPTIMAL CONTROL ALGORITHMS
In this section, the simulation and experimental results regarding the cart-pendulum robotic system (Fig. 3) subject to the three above mentioned controllers are shown. The control performance is evaluated under different working condition of the real robot. The control parameters are introduced in Subsection VIII-A, while in Subsection VIII-B the simulations are compared with experimental results to demonstrate the validity of controllers' implementation. Lastly in Subsection VIII-C the experimental results are presented and discussed, with related performances and WCET analysis.

A. CONTROL PARAMETERS
Each controller has been designed and tuned to stabilize the cart-pendulum around the pendulum's vertical position while maintaining a certain distance from obstacles detected by the sonar and for disturbance rejection. All the tuning parameters are chosen to obtain a control system which first achieves as small as possible oscillations around the vertical position, secondly it maintains the desired x position and finally to have it also has a certain reactivity to disturbances. The values of the weighting matrices and tuned parameters used by the  Figure 4 shows the comparative results of the pendulum's φ angle when the SDRE controller is applied. In Figure 5 the comparative results for the cart position x are showed, lastly in Figure 6, the control effort U in both cases are compared. The trend over time shown in the figures, highlights similar oscillations in term of amplitude, frequency  and phase. Furthermore, small deviations between simulated and real system can be noticed in cart and pendulum direction changes, this may be attributed to the not perfect modelling of the tyre-floor contact point (tyre slip conditions). However, despite the small predictable mismatches between model and reality, by focusing on Figure 6 it can be noticed as the control input seems to be the same in both cases. This proves the effectiveness and consistency of the controller implementation in the HRT board. Besides, it is appropriate to point out that the achievement of perfect model matching with reality is out of the scope of this work.

C. EXPERIMENTAL RESULTS
This subsection is devoted to describe the experimental results obtained by applying optimal control laws in different working conditions of the experimental set-up in a RT scenario. The experiments concern the performance evaluation for the regulation problem of a cart-pendulum system. The challenge is: balancing the pendulum in a vertical position (pitch angle φ = 0) while the cart maintains a desired position in space (x). In the design phase, the regulation around a fixed set-point is the primary goal to achieve by any control system. The second one is often the trajectory tracking, in which the control law forces the closed-loop response to follow a specified reference, generally time varying. Though the trajectory tracking is not the subject of this investigation, it will be shown how some considerations on it can be inferred in terms of WCET, considering the implementation of the algorithms adopted. For evaluating performance and WCET of the considered optimal controllers on regulation problems two experiments have been carried out. In the first one, the system works within the linearity range of the pendulum's vertical unstable equilibrium point, which is φ = 0. For our system, it has been experimentally proofed that the range is The second experiment aims to evaluate the performance of the optimal controllers when nonlinear behaviours arise. In our case this happens by choosing the initial condition of the φ angle out of the linear region. During experiments, continuous monitoring of the WCET is performed by implementing the control Algorithms in the XMOS board. Furthermore, computational efforts required to the microcontroller by the control laws are taken into account.

1) TEST 1 -LINEAR WORKING CONDITIONS
The Figures 7, 8, 9 show the comparison of control results respectively for the pitch angle φ, the cart position x and control input U in linear working conditions. Overall, all the controllers accomplish the regulation task stabilizing the system around φ = 0 with small oscillations mainly due to physical limits of the experimental set-up. A comparative analysis of the controllers in terms of a) ET, and b) performance, is detailed as follows. Regarding the point a), Table 2 reports the code execution BCS and WCS values, which have been computed using the XMOS development environment xTimeComposer for each of the task executed by the board, including those related to the control algorithms.   The WCS provides the values of the WCET for each task, this analysis gives the possibility of setting the temporal upper bound limit for RT or HRT applications. A first aspect to be noticed from Table 2, even if it is quite obvious, but here it has been quantified, is that for regulation problems LQR and MPC algorithms require less time to be executed. The SDRE requires instead almost four times the WCET in respect to the previous ones. This first result suggests that attention must be taken, in terms of ET, in using nonlinear algorithms, as the SDRE, for linear tasks. This becomes even more notable in higher order MIMO systems because the ET of the DARE solution is related to the system's order, as well as with the initial conditions. In classical applications where a single microcontroller must execute more tasks (e.g. sensors acquisition, control, etc.), a high WCET of the control algorithm could lead to critical situations. These temporal constraints are crucial in HRT applications. Differently, using the MET-SDRE controller previously introduced the algorithm ET decrease. In particular as long as the system is working within linear conditions the BCS value obtained can be comparable to the linear controllers and with respect to classic SDRE it decreases significantly. Regarding the point b), related to the performance analysis, it can be noticed that under linear conditions the LQR performs similar to the SDRE disregarding the ET, while the MET-SDRE keeps the same performance of SDRE as expected. For visualization reason, here the MET-SDRE performances, are not plotted because they match with the classic SDRE and they will be given for the second test. Figure 7 shows how the stabilization is reached for both the LQR controller and SDRE controller. However, focusing on Figure 8 it is possible to notice that, when using LQR controller, the cart position x is kept with a constant bias from the set-point. It happens because the LQR controller gives priority to the angle stabilization, indeed it has been demonstrated that in nonlinear systems when the state has coupling dynamics (as e.g. in our case where the φ and x behaviours are strictly related) a linear LQR cannot manage them perfectly [28]. However, some works sustain that in terms of performance SDRE approach provide at best a quite limited and case dependent, benefit over LQR [29]. Therefore LQR is often preferable due to its less computational effort especially in application where system's nonlinearities are negligible. Finally, the MPC takes more time to reach the steady-state condition with respect to the others and when reached is not able to well regulate the cart position x within 25 seconds. This may be because of determining the value of the control signal, the MPC analyzes the whole prediction horizon (N p ). As a result, the control effort is smoother but the system is also less reactive because the disturbance rejection task is averaged for the entire time horizon. Therefore the prediction horizon should not be too long, because it slows down the reaction of a controller to disturbances in the system, though this feature is interesting in some cases. Indeed, the control signal of MPC controller is smoother than the others (Fig. 9) and this may be important, especially in an industrial environment, during the start-up phases of plants and actuators, because these systems are adversely affected by sudden changes in the control signal. Moreover, this control strategy has less impact on energy consumption because the power required for a hypothetical actuator is less. Therefore, the prediction horizon N p is an important tuning parameter that must be set case by case. Lastly, regarding trajectory tracking tasks, the MPC controller has advantages in terms of design and ET. This because the MPC control algorithm implementation is valid both for constant and variable system's reference signals. Therefore, the WCET of the MPC calculated in Table 2 for control regulation problem would remain the same also for the trajectory tracking problem. Differently, the LQR design phase in case of trajectory tracking issues is pursued [27], [39].  Figures 10, 11, 12 show the trends over time of the pitch angle φ, the cart position x and the control input U . In this case, not all the controllers can accomplish the regulation task stabilizing the system around φ = 0, for this reason in Figures 10, 11, 12 the divergent trends for the linear controllers are obtained. Also in this second test, a comparative analysis of the controllers in terms of a) ET, and     are proven to be the only control strategies able to stabilize the system while LQR and MPC lose control. At the same time, in the second scenario, the MET-SDRE decreases the algorithm's WCET of about 35% with respect to the classic SDRE (as in Table 2), by maintaining the same performance as well. This result is due to its adaptive nature, indeed, as discussed in section V, the number of backward iterations of the DARE (34) and then the ET of the control algorithm are based on the current state of the system. Then, when nonlinear behaviours arises the biggest difference between two consecutive SDC matrices A d nl (x[k]) and A d nl (x[k + 1]) requires solving the DARE (34) with more iterations to obtain a convergent solution P(x[k]). Regarding the point b) instead, which is related to the performance analysis, the system nonlinearities are triggered by imposing to the pendulum an angle φ = 11 • out of the linear range. The linear controllers (LQR, MPC) are obviously not able to perform the regulation task. Indeed, in Figure 10 it can be noticed how the MPC tries to approach the reference value (φ = 0) but the control law is not fast enough and the cart diverges from the position x = 0, as shown in Fig. 11. A similar result is obtained with the LQR even if the controller tries within 5 seconds to stabilize the pendulum but because of the large oscillations (Fig. 10) the control is lost. Differently, the SDRE and the MET-SDRE, within 2 seconds are able to bring the system near to the equilibrium point and stabilize the pendulum (with small oscillation around φ = 0) in 5 seconds. The performance of these two nonlinear controllers are shown separately from the others to evaluate better the similarity of their behaviour. They are presented in Figures 13, 14, 15, where respectively are compared the pitch angle φ, the cart position x and the control input U for the classic SDRE and the proposed MET-SDRE. It can be noticed in these figures, that the MET-SDRE achieves the same performance as classic SDRE implementation but decreases the algorithm's WCET. There is a small time-shift on the Figures 13, 14, 15 which depends on three factors: on the acquisition instant, on the small error which might be between the two initial angles ( φ = 11 • ) and on the possible reading lag introduced by the sonar. However, it is trivial to see how the MET-SDRE performances are consistent with its classic version. The performance analysis of SDRE, LQR and MPC instead, are shown in the Figures 10,11,12. Here, only the nonlinear controller (SDRE) is able to manage the increasing dynamics coupling of the system under nonlinear conditions. This is expected, indeed, when the systems dynamics become complex or the disturbances forces the system to work out of the linear range. In this case the MPC and LQR performance are overcome by the SDRE. Indeed, the SDRE taking into account system's nonlinearities can better handle the system dynamics as also experienced in [28], [34].

IX. CONCLUSION
The advancement in electronics, computing and communication technologies have made it feasible to extend the application of embedded systems to more critical applications, such VOLUME 8, 2020 as automotive, avionic and many others. Often, they involve Hard Real Time (HRT) requirements, where systems must be designed according to the resource adequacy policy and must provide sufficient computing resources to handle the specified worst-case computational load and fault scenario. The HRT embedded resources, as shown in this work, mostly are required by the control algorithms and the implementation of nonlinear control laws are often computationally demanding. For this reason in some cases, the performances are not suitable because of specification on application constraints. However, in order to achieve even more demanding objectives to control systems, the linear controllers have to be overcome because in most of control applications the PID is still the most used controller. In this work, a method has been provided to evaluate in a deterministic way the Execution Time required by the controllers' implementations for embedded devices with HRT characteristics. Implementation solutions for discrete-time linear and nonlinear optimal control techniques have been carried out with encouraging perspectives. Once evaluated both performance and ET for three of the most used optimal controllers, this work shows how to improve the execution time of the SDRE nonlinear controller, by proposing a new method named MET-SDRE. With the MET-SDRE, in terms of ET, a Best Case Scenario (BCS) comparable with linear controllers have been achieved. The Worst Case Scenario (WCS) has been improved by 35% with respect to the common approach maintaining the same performance. In general, the results obtained, could be useful in the filed of control systems engineering especially during the design stage in RT and HRT systems, where temporal bounds have to be fulfilled also taking into account all the other functional specifications. In future, further investigation is required in order to prove the performance of these solutions on higher-order systems.

APPENDIX A SDC FACTORIZATION FOR SDRE DESIGN
In this appendix, the design flexibility for the SDC matrix is discussed, and general recommendations are provided to derive a correct factorization. The extended linearization, also known as SDC factorization is the process of factorizing a nonlinear system into a linear-like structure which contains SDC matrices. Considering the cart-pendulum model in (2), this can be generally written as: where f (x) = A(x)x. Without loss of generality, the origin x = 0 can be assumed as an equilibrium point, such that f (0) = 0. Under this assumption, a continuous nonlinear matrix-valued function A(x) always exists such that is a n × n matrix found by mathematical factorization and it is nonunique when n > 1. Because of this non-uniqueness of the A(x) matrix, in literature different works have been proposed methods to improve the system stability performances through the factorization process. For example, in [37] a method has been proposed to design the SDC matrix when working with a conforming system. In [36], it has been studied how a different factorization of the A(x) matrix can affect stability performances. Another possible SDC parameterization solution is given in [34] and [28], where in both cases an inverted pendulum has been controlled with SDRE for both the swinging-up and stabilization. However, in this work, the focus is not to investigate a new parameterization method but to carry out a comparative analysis of control algorithms execution time, in particular LQR, MPC and SDRE. Consequently, a classic approach regarding the design flexibility has been followed, as proposed in [37]. The parameterization steps adopted to derive the SDC matrix in (3) are presented below with related motivations. Considering a nonlinear systems and under the assumption of x = 0 being an equilibrium point. The SDC A(x) matrix terms can be state-independent or state-dependent. In case of state-independent terms, named also ''bias'', it can be handled to satisfy the assumption f (0) = A(0) x(0) = 0 by augmenting the system with a stable state: so that the bias can be factorized as: which converges to zero only when z = 0. Differently, regarding state-dependent terms non-unique solutions can be adopted. Defining A(x, α)x as an infinite family of SDC parameterization, in general, terms which do not converge to zero as the state approaches zero, violates the fundamental condition f (0) = 0 outlined above. Like biases, these terms prevent a direct factorization of f (x) into A(x, α)x but they can be handled as discussed above. However, it is more desirable to capture their state dependency in the proper element of the matrix A(x, α). For example, supposing to have a system with two state variables, where equations are given by:ẋ where β can be considered as a bias term, discussed above, the SDC matrix A(x) has the form: The cosine term of the A(x, α) matrix it is desirable to be nonzero to reflect the state dependency. A solution could be to re-arrange the cosine function as: where [cos(x 1 ) − 1] approaches to the origin when x 1 goes to zero and can be factorized as: and the remaining term +1 of equation (43) can be handled like a bias.

APPENDIX B THE MOTOR MODEL
The four-wheeled robot is powered by four identical motors which provide the total force F acting on it. The first order model in Laplace domain has been derived by means of an identification process which has provided the following transfer function: Knowing thatω = τ m J = F R r J and ω =ẋ R r the (46) can be rewritten w.r.t. the force acting on the robot as: Then, the force equation is replaced in the system model (1) where the term β = (J K p ) (τ p R r ) , and γ = J τ p R 2 r . The parameters K p and τ p are found through the identification process, R r is the wheel radius. The rotor inertia J is calculated by adopting the reduced order model of the DC motor, where the electrical time constant is neglected. Since K e and R a are measured, and K t is computed as a good approximation as τ stall I stall , it is possible to compute the viscous friction B m with: Then J can be computed with: the physical and electrical parameters are completely defined and listed in table 3.

APPENDIX C MPC EXTENDED MODEL DERIVATION
In order to derive the extended model let consider the following extended state vector x e : Using the iteration of the model (16) where the variable N p is the length of the prediction window and N c is the length of the receding window with N c ≤ N p . It can be proven by using the model (54) iteratively that the output vector Y can be expressed in compact form as: For a given set-point signal r(k i ) at sample time k i , within a prediction horizon the objective of the predictive control VOLUME 8, 2020 system is to bring the predicted output as close as possible to the set-point signal, where we assume that the set-point signal remains constant in the optimization window. This objective is then translated into a design to find the 'best' control parameter vector U such that an error function between the set-point and the predicted output is minimized. Assuming the data-vector which contains the set-point information as: The cost function J that reflects the control objective is defined as: where the first term is related to the minimization between the predictive output and the set-point, while the second is referred to the size of U.
ANDREA BONCI (Member, IEEE) is currently an Assistant Professor in automatic control and the Head of the Automation Laboratory with Università Politecnica delle Marche. His research interests include modeling and control of dynamic systems, system and control theory, vehicle dynamics, automotive systems, modeling and control of autonomous systems, mechatronic, robotics, embedded systems applications, industrial automation, cyber-physical systems, and smart manufacturing.
SAURO LONGHI (Senior Member, IEEE) is currently a Full Professor in robot technologies and a Rector with Università Politecnica delle Marche. His main research interests include modeling, identification and control of linear and nonlinear systems, control of mobile robots, service robots for assistive applications supporting mobility and cognitive actions, home and building automation, and automatic fault detection and isolation.
GIACOMO NABISSI (Graduate Student Member, IEEE) received the master's degree (Hons.) in computer and automation engineering from Università Politecnica delle Marche, in 2019, where he is currently pursuing the Ph.D. degree. He also holds research fellow contract for the Regional platform for Industry 4.0. His research interest includes Industry 4.0 related issues. In particular, he works on predictive maintenance applied to electrical motor-operated devices and on robotics concerning in modeling, planning, and control.
GIUSEPPE ANTONIO SCALA received the master's degree in computer and automation engineering from the Università Politecnica delle Marche, in 2018, where he is currently pursuing the Ph.D. degree in automation engineering with the Dipartimento di Ingegneria dell'Informazione. His main research interests include robotics, nonlinear system analysis and control, virtual simulations, and computer science. VOLUME 8, 2020