2013 3rd International Conference on Instrumentation Control and Automation (ICA) Bali, Indonesia, August 28-30, 2013
Model Predictive Control for Obstacle Avoidance as Hybrid Systems of Small Scale Helicopter Salmah1, Sutrisno2, Endra Joelianto3, Agus Budiyono4, Indah E. Wijayanti5, Noorma Y. Megawati6. 1,5,6
Dept. of Mathematics Gadjah Mada University Yogyakarta, Indonesia 1
[email protected], 5
[email protected], 6
[email protected]
2
Dept. of Mathematics Education Sanata Dharma University Yogyakarta, Indonesia
[email protected]
3
Dept. of Physics Engineering Bandung Institute of Technology Bandung, Indonesia
[email protected]
Model predictive control (MPC) can be applied to control an MLD system with some modification such as the corresponding optimization problem. The predictive control for MLD system for finite time horizon and quadratic objective function can be solved by forming the vector prediction of all variables of MLD over time horizon, substituting it into objective function and solving the corresponding optimization problem. For case with quadratic objective function, MLD can be solved using mixed integer quadratic programming (MIQP) [3].
Keywords—model predictive control (MPC); unmanned aerial vehicle (UAV); small scale helicopter; obstacle avoidance; piecewise affine (PWA); mixed logical dynamic (MLD)
To determine the optimal trajectory for the obstacle avoidance purpose, the PWA model is formed by dividing the observation area of the UAV into two different area that are obstacle area and normal area [4]. The normal area corresponds to the dynamic of the UAV and the obstacle area corresponds to the dynamic with the matrix of the system is identity so that the UAV is never reach the target position if it enter the obstacle area.
INTRODUCTION
Unmanned aerial vehicle (UAV) has been developed for some applications like underwater, underground or air unmanned vehicles. A small scale helicopter is one of the air unmanned vehicles that its maneuver can be interpreted into nonlinear dynamical system. The linearization of this dynamics can be done using Newton-Euler with six degrees of freedom [1]. The maneuver of the UAV needs to be controlled in order to fly as the desired trajectory. If the UAV meets an obstacle on the trajectory, the UAV must determine the new trajectory that can avoids the obstacle and reaches the target position with the minimum effort. To design a controller that can avoid an obstacle, we use hybrid system approach.
The UAV that we used is a small scale helicopter (Yamaha R-50). The dynamics of this UAV was appeared in [5] and [6]. Some control purposes was developed like tracking a trajectory using some methods like coefficient diagram method [6], linear quadratic regulator [5], robust PID [7], nominal MPC [8], [9], Nonlinear MPC [10], MPC with obstacle avoidance [4], LMI and switched linear control [6]. The linear model of small scale helicopter refer to [6] is used to form a PWA model by following [4] for the obstacle avoidance purpose. The PWA model can be transformed into equivalent MLD model using HYSDEL given by [2] and [3].
Hybrid system is a mathematical model that consists of dynamics of real valued variables, discrete variables and their interaction. A hybrid model switches the dynamic depending on some events. For special case of linear discrete state vector event (or location), the hybrid model is called piecewise affine (PWA) system. PWA model can be transformed into equivalent mixed logical dynamical (MLD) system [2]. This MLD system is more suitable for solving optimization problem corresponding to the control problem [3]. The state of an MLD
978-1-4673-5798-2/13/$31.00 ©2013 IEEE
Dept. of Aerospace Information Engineering Konkuk University Seoul, South Korea
[email protected]
system contains the state and input of the PWA model and some auxiliary variables i.e. binary variable and real variable. Furthermore, besides the original constraint of PWA, MLD has auxiliary constraint in the form of matrix inequality.
Abstract—In this paper, we apply model predictive control for obstacle avoidance of small scale helicopter as a hybrid system such that this UAV can determine the optimal trajectory from initial position to the target position and avoid an obstacle. The hybrid dynamics of this UAV can be interpreted as a piecewise affine (PWA) model that can be transformed into equivalent mixed logical dynamical model (MLD). The PWA model is triggered by state events (coordinate of the UAV for this case). MPC for MLD can be solved using mixed integer quadratic programming (MIQP). We simulate the model and the controller with several shapes of obstacles. From the simulation results, UAV avoids the obstacle optimally using the track generated by MPC.
I.
4
In this paper, we use the hybrid model approach in the PWA form to of the small scale helicopter for the obstacle avoidance purpose. This PWA model will be transformed into equivalent MLD model using HYSDEL and mld MATLAB function on hybrid toolbox. By using MPC for MLD, the
127
optimal trajectory of the UAV will be determined so that from some initial position, this helicopter can avoid the obstacle and reach the target position. We will simulate the obstacle avoidance of the helicopter with three scenarios depend on the obstacle shapes.
II.
III.
OBSTACLE AVOIDANCE SCENARIO
Consider an UAV is located at initial position and will drive to the final position with presence an obstacle between them as described by Figure (1) for two dimension case.
DYNAMICS OF SMALL SCALE HELICOPTER
A linearization dynamics of the motion of Yamaha R-50 helicopter was derived from the Newton-Euler equation for a rigid body with six degrees of freedom [11]. The linear dynamics of small scale helicopter are given by [12] and [5] as follows. Let μ be the state vector of the helicopter with
μ = [ u, w, q,θ , a, v, p, r , φ , b,ψ ]
T
(1)
Figure 1. 2D view of the obstacle avoidance scenario
where u and v are the translational fuselage motions, p and q are the angular fuselage motions, w is the rigid body state, r is yaw rate, a and b are the rotor state for lateral and longitudinal flapping motions respectively, and the input vector is u = ⎡⎣δ coll δ long δ ped δ lat ⎤⎦ . The body coordinate state is
Assume the dynamic of the UAV for the normal area is a discrete dynamical state space LTI of the form x(k + 1) = Ax(k ) + Bu (k ) (7) y (k ) = Cx(k ) + Du (k ). (8) m
where x ∈ \ n , u ∈ \ p , and y ∈\ are the state, input, and output vectors respectively. The matrices A, B, C, and D are real constant matrices with appropriate dimension. Assume that system (7)-(8) is controllable and observable. The obstacle area is defined by labeling its area described as follows.
[x,y,z]T that satisfy t ) ⎤ ⎡ u(t ) ⎤ ⎡ x( ⎢ y( ⎥ ⎥ ⎢ ⎢ t ) ⎥ = ⎢ v (t ) ⎥ , ⎢⎣ z( t ) ⎥⎦ ⎢⎣ w(t ) ⎥⎦
(2)
then the full state of this helicopter is obtained by collecting (1) -(2) in a matrix form as follows. x (t ) = Ax(t ) + Bu (t )
(3)
y (t ) = Cx(t ) + Du (t )
(4)
where x = [ x
y z μ ] , and A, B, C, D are real constant matrices with appropriate dimension appeared in appendix. T The solution of vector [ x,y,z ] in (3)-(4) is the position of the helicopter in a body coordinate. The body coordinate can be transformed into local horizon coordinate using matrix T1 that appeared in Appendix 2 [12]. The matrix transformation T1 relates the position and the velocity in the two different coordinates that given by
[N
E
⎣⎡Vx
Vy
A] = TI [ x T
T
y
z]
T
Figure 2. Labeling the obstacle area The obstacle area and normal area is defined by ⎧ ⎡ a ⎤ ⎡ x1 ⎤ ⎡ b ⎤ ⎪Obstacle area : ⎢ ⎥ ≤ ⎢ ⎥ ≤ ⎢ ⎥ , ⎨ ⎣ c ⎦ ⎣ x2 ⎦ ⎣ d ⎦ ⎪ Normal area : others. ⎩ The dynamic model for the obstacle area is x( k + 1) = Ix( k ) (9) y (k ) = Cx (k ). (10) where x and y are the state and output vectors of the system (7) -(8) and I is an identity matrix with the same dimension of A. The hybrid model for this obstacle avoidance scenario is obstacle area ⎧ Ix(k ), x(k + 1) = ⎨ (11) ⎩ Ax(k ) + Bu(k ), normal area
(5)
Vz ⎤⎦ = TI [ u v w]
T
(6)
where (u, v, w) is the velocity in body coordinate, (Vx ,Vy ,Vz ) is the velocity in local coordinate, ( x, y, z ) is the local coordinate and ( N , E , A) = ( North, East , Altitude) is the position of the helicopter.
y(k ) = Cx(k ) (12) Hybrid model (11)-(12) is a PWA system with two events generated by state values.
128
IV.
Reference [2], [3] and [14] was shown that PWA is can be transformed into equivalent MLD and vice versa. This transformation can be done by using HYSDEL that can be generated in MATLAB. This MLD model will be used to control the system using MPC.
MPC FOR HYBRID MODEL
A hybrid model can be represented as a dynamic of the system that constituted by parts described by logical parameter such as on/off switches, event states, if-then-else rules, etc. The hybrid dynamics can be modeled into piecewise affine (PWA) and then it can be transformed into equivalent mixed logical dynamic (MLD). The MPC controller will be used to control an MLD model.
B. MPC Controller for MLD Model An MLD control problem is determining some input values such that the outputs track some desired set point or reference trajectories. The objectives of the MPC can be described as the state gain to the set point or references. MPC minimizes the objective function that can be described as the following optimization [4].
A. PWA and MLD Model Let s be the number of the model parts or events. The model of PWA is described as follows [13]. ⎧ A1 x( k ) + B1u ( k ) if δ1 = 1 ⎪ A x( k ) + B u ( k ) if δ = 1 ⎪ 2 2 x( k + 1) = ⎨ 2 ⎪# ⎪⎩ As x( k ) + Bs u ( k ) if δ s = 1
(13)
y (k ) = Cx(k ) + Du (k )
(14)
T −1
min J = ∑ ⎡ u (k ) − u f ⎢⎣ [u ,δ , z ]T t =0 + z (k ) − z f
time
y (k ) = Cx(k ) + D1u (k ) + D2δ (k ) + D3 z (k )
(16)
E2δ (k ) + E3 z (k ) ≤ E1u (k ) + E4 x(k ) + E5
(17)
(18)
where T be the prediction horizon length, Q1, Q2, Q3, Q4 are the symmetric and positive definite weighting matrices, xf, uf,
δ f , zf, are the final state, final input, final δ , final auxiliary variable z, and the notation v
2 Q
= vT Qv . The optimization
(18) can be transformed into mixed integer quadratic (MIQ) optimization by forming the vector predictions of state x , input u , δ , and z over horizon prediction T and substituting the prediction to the objective function. This transformation gives the following MIQ optimization min R′S1R + 2(S2 + x0′ S3 )R (19)
Let δ ∈ {0,1} be the binary variable, nc > 0 be the number of the state of the system. Following [14], the MLD model is described as (15)
Q4
⎤ Q1 ⎥ ⎦ 2
+ x( k ) − x f
− E4 x(k ) − E1u ( k ) + E2δ ( k ) + E3 z ( k ) ≤ E5
k,
x (k + 1) = Ax (k ) + B1u ( k ) + B2δ (k ) + B3 z (k )
2
Q3
x (k + 1) = Ax (k ) + B1u ( k ) + B2δ (k ) + B3 z (k )
⎧⎪ ⎡ x ⎤ ⎫⎪ Ω i ⎨ ⎢ ⎥ : H ix x + H iu u ≤ K i ⎬ , ⎪⎩ ⎣u ⎦ ⎭⎪ i = 1, 2,..., s are convex (possibly unbounded) polyhedral in the input and state space, δ i ∈ {0,1} , Ai , Bi , C , D, H ix and H iu are real matrices with appropriate dimensions.
at
2
+ δ (k ) − δ f
subject to : x (T ) = x f
⎡ x( k ) ⎤ n m subject to ⎢ ⎥ ∈ Ωi , where x(k ) ∈ \ , u(k ) ∈ \ and u ( k ) ⎣ ⎦ y(k ) ∈\ p denote the state, input and output vector respectively
2 Q2
R
subject to : F1R ≤ F2 + F3 x0
⎡ xc (k ) ⎤ nc where x(k ) = ⎢ ⎥ is the state vector, xc (k ) ∈ \ , ( ) x k ⎣ l ⎦ ⎡ yc (k ) ⎤ pc pl xl ( k ) ∈ {0,1}nl , y (k ) = ⎢ ⎥ ∈ \ × {0,1} is the output y ( k ) ⎣ l ⎦
A B R = x f − AT x0 , R = [u (0),..., u (T − 1),..., δ (0),...,
δ (T − 1), z (0),..., z (T − 1) ]
T
where S1 , S2 , S3 are the real constant matrices with appropriate dimension and the matrices F1 , F2 , F3 , A and B are appeared in Appendix (1). Optimization (19) can be solved using MIQP that embedded in hybrid toolbox [15]. The optimal value R* obtained from (19) by MIQP contains the optimal value for u*, δ *, and z *. The control action that will be applied to the * system is u .
⎡uc (k ) ⎤ mc ml vector, u (k ) = ⎢ ⎥ ∈ \ × {0,1} is the input vector, u ( k ) ⎣ l ⎦ z ( k ) ∈ \ rc and δ (k ) ∈ {0,1}rl are auxiliary variables. A, Bi , C , Di and Ei are real constant matrices, E5 is a real vector.
129
V.
SIMULATION RESULTS Matrix Dimension
A. System Setup The matrices of the system (3)-(4) is appeared in Appendix 3. On the other hand, the matrices of the system (7)-(8) is obtained by discretising (3)-(4) using MATLAB function c2d. These matrices are appeared in Appendix (4). The input constraint is given by ⎡−30D ⎤ ⎡δ coll ⎤ ⎡30D ⎤ ⎢ ⎢ ⎥ ⎢ D⎥ D⎥ ⎢−30 ⎥ ⎢δ long ⎥ ⎢30 ⎥ ⎢−30D ⎥ ≤ ⎢δ ⎥ ≤ ⎢30D ⎥ . ⎢ ⎥ ⎢ ped ⎥ ⎢ ⎥ D ⎢⎣−30 ⎥⎦ ⎢⎣δ lat ⎥⎦ ⎢⎣30D ⎥⎦ The matrices of the PWA model (11)-(12) is same as of the system (7)-(8).
Matrix Dimension
C 3×14
E1 178×4
D1 3×4
E2 178×26
D2 3×26 E3 178×14
D3 3×14 E4 178×14
E5 178×1
Since the dimension the MLD system matrices are relatively large, we do not append these matrices. By using this MLD model, the MPC is used to generate the optimal trajectory for the helicopter so that it can reach the target position and avoid the obstacle. The length of the prediction horizon is 4 samples with sampling time 1 second. The weighting matrices for objective functions (18) are Qi = I , i = 1, 2,3, 4 that are identity matrices with appropriate dimension. Simulation result for this obstacle avoidance is given by Figure (3).
B. Simulation Results We give a simulation with a box shape obstacle. The initial T position is [ N 0 , E0 , A0 ] = [ 5,5, 5] and the initial full state is
x0 = [ x0 , y0 , z0 , 4,0.001,0, −0.0145,0.0001,0,0,0,0,0,0]T where [ x0 , y0 , z0 ] is obtained by transforming [ N 0 , E0 , A0 ] using matrix T1. The final or target position is T
T
T
⎡⎣ N f , E f , Af ⎤⎦ = [80,80,80] and the final full state is T
x f = ⎡⎣ x f , y f , z f ,1, 0.001, 0, −0.0145, 0.0001, 0, 0, 0, 0, 0, 0 ⎤⎦ . The obstacle area is defined by a box bounded by
[ 40, 40, 40]
T
≤ [ N , E , A] ≤ [ 60, 60, 60 ] . T
T
Figure 3. The optimal trajectory of helicopter
By using T1 the corresponding obstacle area in the body coordinate is
[ 40, 40, 40]
T
Figure (3) shows that helicopter avoids the obstacle with optimal track.
≤ [ x,y,z ] ≤ [ 60, 60, 60 ] . T
T
The PWA model for this simulation is
⎧ ⎡40⎤ ⎡ x ⎤ ⎡60⎤ ⎪ ⎢40⎥ ≤ ⎢ y ⎥ ≤ ⎢60⎥ ⎪ Ix(k ), ⎢ ⎥ ⎢ ⎥ ⎢ ⎥ x(k + 1) = ⎨ ⎢⎣40⎥⎦ ⎢⎣ z ⎥⎦ ⎢⎣60⎥⎦ ⎪ ⎪ Ax(k ) + Bu (k ), others ⎩ y(k ) = Cx(k )
(20)
(21) where A, B, C are matrices obtained from the discretization of (3)-(4) using sampling time 1 second. The equivalent MLD of (20)-(21) is obtained by converting them using HYSDEL and mld function in hybrid toolbox. The matrices A, B1, B2, B3, C, D1, D2, D3, E1, E2, E3, E4 and E5 of this MLD model have the dimensions appeared in Table (1). Figure 4. The optimal input of the helicopter
Table 1. The dimension of matrices of MLD(15)-(17) Matrix A B1 B2 B3 Dimension 14×14 14×4 14×26 14×14
130
Figure (4) shows the optimal inputs corresponded to the optimal trajectory. These optimal control signals were generated by MPC that was solved by MIQP solver. VI.
[7]
CONCLUSIONS
[8]
Obstacle avoidance problem for an UAV of small scale helicopter (Yamaha R-50) was considered. The obstacle avoidance scenario was designed as a PWA model that was transformed into equivalent MLD model. The optimal trajectory of the UAV from initial position to the final (or target) position was determined by applying MPC to MLD model and solving it using MIQP optimization. Three obstacle shapes was used in the simulations. From the simulation results, helicopter can avoid the obstacle with optimal trajectory from initial position to target position.
[9]
[10]
[11]
[12]
REFERENCES [1]
[2]
[3] [4] [5]
[6]
Castillo, C.L., Moreno, W. and Valavanis, K.P., 2009, Unmanned Helicopter Waypoint Trajectory Tracking Using Model Predictive Control, The 15th Mediterranean Conference on Control and Automation proceedings of the international conference in Athens, Greece, July 27-29, 2009. Bemporad, A., 2004, Efficient Conversion of Mixed Logical Dynamical Systems Into an Equivalent Piecewise Affine Form, IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 49, NO. 5, MAY 2004., pp. 832−838. Bemporad, A., Hybrid Toolbox - User’s Guide, April 20, 2012. Keij, J.J.A.M., 2002, Obstacle Avoidance with Model Predictive Control in a Hybrid Controller, Tokyo Institute of Technology. Budiyono, A. and Wibowo, S.S., 2007, Optimal tracking controller design for a small scale helicopter, Journal of BionicEngineering, Vol. 4, pp. 271−280. Budiyono, A., Onboard Multivariable Controller Design for a Small Scale Helicopter Using Coefficient Diagram Method, A2 Program
[13]
[14] [15]
[16] [17]
Grant of the Directorate General of Higher Education of Indonesia. Pradana, W.A., Joelianto, E., Budiyono, A. and Adiprawita, W., 2011, Robust MIMO H ∞ integral backstepping PID controller for hovering control of unmanned helicopter model, Journal of Aerospace Engineering (JAE), Vol. 24, No. 4, pp. 454-462. Lazar, M., Heemels, W.P.M.H., Weiland, S. and Bemporad, A., 2006, Stabilizing Model Predictive Control of Hybrid Systems, IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 51, NO. 11, NOVEMBER 2006, pp. 1813-1818. Joelianto, E., Maryami, E., Budiyono A., and Renaning, D., , 2011, Model Predictive Control for Autonomous Helicopters, Aircraft Engineering and Aerospace Technology (AEAT): An International Journal, Vol 83, Issue 6, pp. 375-387. Wan, E.A. and Bogdanov, A.A., 2001, Model predictive neural control with applications to 6 DoF helicopter model, American Control Conference 2001 proceedings of the international conference in Arlington, VA, 2001, pp. 488-493. Mettler, B. , 2003, Identification Modeling and Characteristic of Miniature Rotorcraft, Kluwer Academic Publishers, Boston, Massachusett, USA. Sutarto, H.Y., Budiyono, A., Joelianto, E. and Hiong, G.T., 2006, Switched linear control of a model helicopter, Automation, Robotics and Vision proceeding of the international conference in Singapore, December 5-8, 2006, pp. 1300-1307. Kim, H.J., Shim, D.H., and Sastry, S., 2002, Nonlinear Model Predictive Tracking Control for Rotorcraft-based Unmanned Aerial Vehicles, American Control Conference, proceedings of the international conference in Anchorage, AK, pp. 3576-3581. Borrelli, F., Bemporad, A., Morari, M., 2011, Predictive Control for linear and hybrid systems, June 16, 2011. Bemporad, A., Ferrari-Trecate, G., and Morari, M., 2000, Observability and Controllability of Piecewise Affine and Hybrid Systems, IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 45, NO. 10, OCTOBER 2000, pp. 1864-1876. Maciejowski, J.M., 2002, Predictive Control with Constraints, Prentice Hall Inc., England. Salmah, Solikhatun, Megawati, N.Y., Joelianto, E., Budiyono, A., 2013, Control of Autonomous Helicopter Models with Robust H2-Type Switched Linear Controller, International Journal of Applied Mathematics and Statistics (IJAMAS), Vol. 35, No. 5, 137-148.
APPENDIX Appendix 1. A = ⎡⎣ AT −1 , AT − 2 ,..., A2 , A, I ⎤⎦ , B = [ B1
⎡ I ⎤ ⎡ E5 ⎤ ⎡ Ei ⎢ A ⎥ ⎢E ⎥ ⎢ ⎢ ⎥ G0 = ⎢ A2 ⎥ , E5 = ⎢ 5 ⎥ , Ei = ⎢ ⎢# ⎥ ⎢ ⎢ ⎥ ⎢ ⎥ ⎢ ⎢ # ⎥ ⎣ E5 ⎦ ⎣ ⎢⎣ AT −1 ⎥⎦
Ei
B2
B 3 ] , F1 = E − E4 G4 , F3 = E5 , F3 = E4 G0 , E = [ −E1
⎤ ⎡ Bi ⎤ ⎥ ⎥ , i = 1, 2,3 . ⎥ , i = 1, 2,3, 4, B = ⎢ % i ⎢ ⎥ ⎥ % ⎢⎣ Bi ⎥⎦ ⎥ Ei ⎦
cos θ cos ψ cos θ sin ψ − sin θ ⎤ ⎡ ⎢ Appendix 2. TI = ⎢sin ϕ sin θ cos ψ − cos ϕ sin ψ sin ϕ sin θ sin ψ + cos ϕ cos ψ sin ϕ cos θ ⎥⎥ ⎢⎣cos ϕ sin θ cos ψ + sin ϕ sin ψ cos ϕ sin θ sin ψ − sin ϕ cos ψ cos ϕ cos θ ⎥⎦
131
E2
E3 ] ,
Appendix 3. Matrices of (3)-(4) ⎡0 ⎢0 ⎢ ⎢0 ⎢ ⎢0 ⎢0 ⎢ ⎢0 ⎢0 A= ⎢ ⎢0 ⎢ ⎢0 ⎢0 ⎢ ⎢0 ⎢0 ⎢ ⎢0 ⎢ ⎣0
0 0 0 0
0 1 0 0 0 0 0 −0.0555
0 0 1 0.0085
0 0 0 0 0 0 0 0 0 −0.0010 −9.8090 −11.4711
0 0 −0.4699 −0.9969 3.9983 0 0 0.1148 −0.0231 −0.0292 0 0 0 0 0.9976
0.1419 0 0
0.0010 223.1810 0
0 0.0030 −0.0001 −1.000 0 −0.0118 0.0058 −0.0009 0 −0.0198 0.0662 −0.0032 0 0.3998 0.0149 0.0234 0 0 0 −0.0010 0 0 0 0 0 0 0 0
0 0.0098 0 0
−8.3500 0 0 0
0 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
0 0 0
0 0 −0.0692
0 0 0 −0.1362 0.0010 −3.9911 0 0.0322 −0.1715 1.2565 0 −0.2356 −0.0145 0 1 −1 0.0029 0 0 0 1
0⎤ 0 0 0 ⎤ ⎡ 0 ⎢ 0 0⎥⎥ 0 0 0 ⎥⎥ ⎢ ⎢ 0 0⎥ 0 0 0 ⎥ ⎥ ⎢ ⎥ 0⎥ 0 0 0 ⎥ ⎢−0.6189 ⎥ ⎢ −0.6792 0.0769 0 0 0 0 ⎥ −122.96 ⎥ ⎢ ⎥ 0 0 0⎥ 0 0 0 ⎥ ⎢ 0.02908 ⎢ 0 0 0 0⎥⎥ 0 0 0 ⎥⎥ , B=⎢ , ⎢0.11362 35.07 0 0 0⎥ 0 0 ⎥ ⎥ ⎢ ⎥ 9.7854 11.4708 0⎥ 0 −4.596 0 ⎥ ⎢ 0.84158 ⎥ ⎢ 0 420.4654 0 8.96576 0 −16.657 0 ⎥ ⎥ ⎢ ⎥ 0 0 0⎥ 0 122.048 0 ⎥ ⎢0.02247 ⎢ 0 0 0 0⎥⎥ 0 0 0 ⎥⎥ ⎢ ⎢ 0 −8.3500 0⎥ 0 0 0 35.07⎥ ⎥ ⎢ ⎥ 0 0 0⎦ 0 0 0 ⎦ ⎣ 0 0 0 0 0
0 0 0 0
C = [ eye (3), zeros (3,11) ] .
Appendix 4. Matrices of (7)-(8) ⎡1 ⎢0 ⎢ ⎢0 ⎢ ⎢0 ⎢0 ⎢ ⎢0 ⎢0 A= ⎢ ⎢0 ⎢ ⎢0 ⎢0 ⎢ ⎢0 ⎢0 ⎢ ⎢0 ⎢ ⎣0
0 1 0 0
0 0.9712 0.0044 −0.1571 −4.8089 −4.4413 0.0277 0.0009 0.0824 0.0568 0.0523 0 −0.1924 −0.0036 0.0135 0.5311 0.4268 0.3529 0.0588 −1.1735 3.0935 3.0206 1 −0.1580 0.6312 0.1186 0.6116 3.0213 −0.0021 −0.0060 −0.0084 −0.2531 −0.2438 0 0.9453 0.0094 −0.3331 −9.5258 −9.1097 0.0998 0.0045 0.2018 0.2685 0.2513
0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0
−0.2684 0.0073 −0.0006 −0.0005 −0.4593 −0.0003 0.1170 −0.0020 −0.0001 0.1163
0.3658 −0.0003 −0.0009 0 −0.0100 0.0005 0.0056 0.0008 −0.0001 0.0046
0.1093 1.6392 −0.0015 −0.0733 0.0379 0.9948 −0.0009 0.0047 0.0575 1.8898 0.0002 0.0031 −0.0399 −1.1368 0.0004 0.0087 0 0.0002 −0.0139 −0.4678
3.0483 0.1588 0.9860 −0.0028 1.6525 0.0049 −1.0770 0.0101 −0.0002 −0.4023
−0.0099 −0.0094 0.0008 0 −0.0250 −0.0018 0 0 −0.5238 0.0732 0.0009 0.0082 0.3687 0.0680 −0.0050 0.0200 −0.0002 −0.0007 0.3642 0.0261
C = [ eye (3), zeros (3,11) ] .
132
−0.0284 0.0016 −0.0224 −0.0001 −1.2117 −0.0008 −0.5201 −0.0036 −0.0004 0.3332
0⎤ ⎡−0.6367 ⎢ 0.5977 0⎥⎥ ⎢ ⎢−44.972 0⎥ ⎥ ⎢ 0⎥ ⎢−1.6179 ⎢−77.159 −0.4494 −0.4430 0⎥ ⎥ ⎢ 0.0025 0.0024 0⎥ ⎢ 0.1796 ⎢ 0.1580 −0.1007 −0.0960 0⎥⎥ , B=⎢ ⎢ −0.007 −0.0001 −0.0001 0⎥ ⎥ ⎢ 3.4556 3.5489 0⎥ ⎢ 1.4030 ⎢ 0.0749 0.0020 0.3047 0⎥ ⎥ ⎢ 3.5608 3.4836 0⎥ ⎢ −0.1311 ⎢ 0.1268 0.9765 0.9692 0⎥⎥ ⎢ ⎢−0.0086 0.0012 0.0035 0⎥ ⎥ ⎢ 1.4648 1.3972 1⎦ ⎣−0.0697
−50.601 2.9575 53.1084 −155.76 105.958 33.6852 33.3987 0.1204 14.9693 0.1838 −14.108 0.1146 −0.0162 −3.5525
0.3119 ⎤ 41.426 ⎥⎥ −2.953 ⎥ ⎥ 1.8347 ⎥ −0.9116 −8.548 ⎥ ⎥ 0.0781 0.0234 ⎥ ⎥ −2.5086 −0.9147⎥ , −0.0043 −0.0014⎥ ⎥ −145.8201 105.934 ⎥ −0.1907 34.7015⎥ ⎥ −38.5605 48.9993⎥ −0.7818 34.1052⎥⎥ −0.0228 0.0663 ⎥ ⎥ 36.5631 13.3002 ⎦ 2.7436 −60.460 −0.1850 9.9198
