! "
#
$
"
#
! "
%
&
%
%
!
'
%
#
% (
%
"
)
%
%
(
%
(
%
*
%
%
%
+
%
,
-
&
%
%
%
(
%
(
"
%
# %
!
%
%
.
!
!
# %
/
0%"
%
%"
# % 1
! "(
! "
#
"
#
2
"
%
%"
)
#
#
3
#
%
!(
%
!
!
4 % '
#
3 #
%
!
(
%
+
*
%
%
%
%
%
% 5 %
%
(
67
,
#
8
)
(
)
(
)
(
)
(
)
(
t
Cx
t
y
t
Bu
t
Ax
t
x
=
+
=
.9 9:/
' %"
%
)
"
"
(
+
!
%"
;
+
%
67
%
< =
( %#
% 5 %
%
)
"
!
7
[
]
∞
+
=
0
)
(
)
(
)
(
)
(
2
1
dt
t
Ru
t
u
t
Qx
t
x
J
T
T
.9 9>/
,
7
% 5 %
%
(
,
(
%
#
* #
7
%
%
!
# " #
!
%
%
%"
%
67
8
)
(
)
(
t
Kx
t
u
−
=
.9 9?/
; %
8
X
B
R
K
T
1
−
=
.9 9@/
.9 9@/
<A=
(
#
.9 BC/
+
B
C
)
(t
u
A
)
(t
x
)
(t
x
)
(t
y
K
+
F
)
(t
w
0
1
=
−
+
+
−
X
B
XBR
X
XA
X
A
T
T
.9 BC/
+
#
.9 9?/
%"
#
3.9 9:/
!
%
"
!8
)
(
)
(
)]
(
[
)
(
)
(
t
Cx
t
y
t
Kx
B
t
Ax
t
x
=
−
=
.9 BD/
)
(
)
(
)
(
)
(
)
(
t
Cx
t
y
t
BKx
t
Ax
t
x
=
−
=
.9 B9/
)
(
)
(
)
(
)
(
)
(
t
Cx
t
y
t
x
BK
A
t
x
=
−
=
.9 BB/
%
#
%"
%
)
(
)
(
)
(
)
(
)
(
)
(
t
Cx
t
y
t
w
t
x
BK
A
t
x
=
+
−
=
.9 BE/
F
#
;( % #
#
%
,
4 %
!
67(
#
!
#
%
+
4 %
(%#
"
%
%
4 %
"
4 % 8
[K,S] = LQR(A,B,Q,R)
;(
&(
#
,
"
4 %
&(
"
;(
!
#
+
" !
#
%
!
G
!
"
%
;
%
+
,
-
&
%
"
%
+ 6?D?6
"
% H
% &
%
"
%
# %
(
%#
%
! "
&
%
%
)
%
"
# % (
%
# %
"
%
! "
"
#
I
%"
# !
+
67
! "
#
*
"
4
%
"
#
! "
%
*
!
!
(
;
67
;
%
+ 6?D? ,
;
,
4
! "
&
&
%
+ ,
M
! "
# %
J %
"
%
&
8
(
)
(
)
(
)
+
+
+
+
+
=
0
0
0
1
0
0
0
0
0
0
0
0
1
0
2
2
2
2
Mml
I
m
M
mgl
m
M
Mml
I
m
M
gl
m
A
K
(
)
(
)
(
)
+
+
−
+
+
+
=
2
2
2
0
0
Mml
I
m
M
ml
Mml
I
m
M
ml
I
B
K
=
0
1
0
0
0
0
0
1
C
K
=
0
0
D
%
+
4 %
;(
LD
7 1
%
.
7
2(
7
4 %
/
)%
8
,LC(
CBM
% N1
# %
LC(
CD?M
% N
! "
"C(
E 1
"
! "
LC(
C9>M
% N
! "
"C(
: 1
"
! "
LC(
9M N C(
BM N. "
%
%
/
F
L.
9
/
-
BM
% O
9
N1
"
. 1
"
%
"
! "/
+
;
<
2=
G
!
%
%
% %
%
G
%
<
= %
!
4
%(
,
(
%
!
%
% #
.
/ ,
G
!
%
<
%
%"
! "
"
+
%
"
%
%
# ! %
!
!
%"
% .% 3 # %
< $,=
/
&
%
G
DCP(
%
+
%"
"
%"
(
"
%
+
"
%
!
%
"
38
•
%
.
#
/
"
•
%
.
#
%/
"
+
%
!
%
8
•
%" %
(
!
)
%
(
%
%
• G
"
#
%" %
$
%
%
(
"
&'
%"
$
( Q %
%
%
• )
"
%
"
(
%
%
0
!
%
%
%
%" % $
$
"
( 5% %
% %
• +
!
%
! "
#
(
%
%"
"
%"
%
• )
;
"
(
! "
#
)
! "
#
(
"
% (
%
%
%
• 2
% 3
"
! "
%
% 3 # %
%(
"
!
$
%
#
%
%
"
%
"
%
&
%
#
!
! %" .
( "
"
/ ' %
%<
,
+
%
%
! %" #
"
#
%"
%
! "
$
%
%
!
#
G
"
% (
#
%
(
%
!
# %
G
%
!
#
%
!
! "
%
%
%
G
"
% (
(
5