Chapter 3

Kinematics and Dynamics

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide1
 Aircraft State Variables

r ib u
jb q roll axis
kb
v
<latexit sha1_base64="QZSVJGewF2upclPoXo7Ovq/bU+g=">AAAHt3iclZVtb9MwEIDD2CiU1wESH/hi6ECbhKamH3j5gMSbePkAGogNpKZUjnNJzBw7s52VKs0f4B/yM/gHnLNqIy0vraVKF9/dc77z+Rrmghvb7f44tXJ6de1M6+y59vkLFy9dvrJ+dc+oQjPYZUoo/TmkBgSXsGu5FfA510CzUMCncP+50386BG24kh/tOIdBRhPJY86oxa3h+ur3IISEy9JSdKnaAQNpQXOZtE8UhaC6KidsIiZokbpY7Xc0A3KXvADDNM8djATBVEmmNhv5UG6g0RsJ2nIqiFTapiRXhtcOKiZvn+4R+IZHNgYiQoWSCdkIMmrTMC559YVvEC6nO4yK8qXbCgLyWwhohABq7OIRvi4UIWpEiNRILh5hf4EIheO/0qqQETkEoRi3439WJZwnhg3i4VLErwsQR0sR9xcgBnnKHfSDEoJQmQggEcSoisiIY5sgOQdmiVVNSnnYq5ogm4KlDrXDLUuXY/kzrNzUh3oNNMJHQDbHdLS1DBEr32geB3umorFjuHdENjXmu0U0tbD8HTfYB/Ps3BXgf/C/XXcDrufhdSn+jf7bvZ/MhnYAMjoeKsdfbvYMr3S62916kXnBnwodb7p2husr14NIsSLDmcUENabvd3M7KCk+VFZPs8JATtk+TaAvwOJgy2kO+h7JqMbZ9ri33eVyUCagMrB63HRAUeKQM4OyHrMVuVO4jGOl8SctqXd/9yhpZsw4C9HSJW9mdW7zT7p+YeOHg5LLvLAg2VGguBCut9zMJhHX2GpijALFaYvJEZZSTRkmhCQJI6ayjGIlgzyCqt8blEGMavyk9czq+NWJ3Kuqpg/2NRyggbF4Hg2iDKzmdctX5eNZ4wzLXJX17Zq4zGbVGn2OtGFYfqhm1fgHkFV9H88nILbBpOMHmicpSlWzWiJRmGiacXbvWJ5h0dCcoI5Jk7qP/NmumRf2etv+/e3e+17nSW/aUWe9m95tb9PzvQfeE++1t+Ptemz159qNNbJ2q/WoNWzFrfTIdOXU1Oea11itg198kZ9P</latexit>

Name Description
w
pitch axis pn Inertial north position of MAV expressed along ii in F i
yaw axis
pe Inertial east position of MAV expressed along ji in F i
pd Inertial down position of MAV expressed along ki in F i
u Ground velocity expressed along ib in F b
v Ground velocity expressed along jb in F b
w Ground velocity expressed along kb in F b
Roll angle defined with respect to F v2
✓ Pitch angle defined with respect to F v1
Heading (yaw) angle defined with respect to F v
p Body angular (roll) rate expressed along ib in F b
q Body angular (pitch) rate expressed along jb in F b
r Body angular (yaw) rate expressed along kb in F b

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide2
 Translational Kinematics
0 1 0 1
ṗn pn
4
@ ṗe A = d @ pe A = vv = Rbv vb
dt
ṗd pd
0 1
u
= Rbv @ v A
w
0 1
u
= (Rvb )> @ v A
w
0 10 1
c✓ c s s✓ c c s c s✓ c + s s u
= @ c✓ s s s✓ s + c c c s✓ s s c A @v A
s✓ s c✓ c c✓ w

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide3
 Rotational Kinematics
0 1 0 1 0 1 0 1
p ˙ 0 0
@q A = @0A b
+ Rv2 ( ) @✓˙A + Rv2
b v2
( )Rv1 (✓) @ 0 A
r 0 0 ˙
| {z } | {z } | {z }
˙ is defined in F b ✓˙ is defined in F v2 ˙ is defined in F v1
0 1 0 10 1 0 10 10 1
˙ 1 0 0 0 1 0 0 cos ✓ 0 sin ✓ 0
= @ 0 A + @0 cos sin A @✓˙A + @0 cos sin A @ 0 1 0 A @0A
0 0 sin cos 0 0 sin cos sin ✓ 0 cos ✓ ˙
0 1 0 ˙1
1 0 sin ✓
= @0 cos sin cos ✓ A @ ✓˙ A
0 sin cos cos ✓ ˙
<latexit sha1_base64="(null)">(null)</latexit>

r ib u
jb q roll axis
Inverting gives: kb
v
w
pitch axis
yaw axis

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide4
 Kinematic Equations of Motion
Six of the 12 state equations for the MAV come from the kinematic
<latexit sha1_base64="WGDJF3nxQrGZwI/AzepqIfEeWsg=">AAALv3iclVbdb+NEEJ8rB7QJH1fgBfHAiojTAdcqjhAgpEp34gUeQAdceyfFUbVerx2r9trd3TQXGf+h/Cm8Mbt2HX81zdmKPZ6P3/5mdjyOl8WR0tPpvw8O3nn47nvvHx6Nxh98+NHHj44/uVDpSjJ+ztI4la89qngcCX6uIx3z15nkNPFi/sq7+sXYX91wqaJUvNSbjC8SGoooiBjVqEqPH/4GLgjgsAYGKSR4Unz2IUe9Ag1LKKys0ecNXnPUFnBpNUvUafQvYLQDhQ2isLdEUZDt4GJs0b08hhDYWyAYDmoHB7UXhz4C6yGUp8FJUWOiOV41EPgbn9/gPYUAr2X1CDgww6uyddRWw+EaVvYpQl+BNoIRKchG1O/wHC7wXvLk1kNaeetzZdfnNguDxVDn4jkaXEGiNq50AkLUZGgzWTW9ynoQuLHeKWKWdo7Wn6vMPXwKLUqO/jFKIcrfVvVtWrOKm7SVKSw/H1G1tZnKiopz38LvtPhWx6ud66/xGM6sx31Myu4n9d6bSFL3M6nfsqbHSf20rH0V5v24p+9GftfDLvXljjXZqAabu/ioFup2XVbxIXU2Q5xUKxvViSY1q5NGVLc2rKVnHf1oxw7d3yWreu9vamm9c9dHdR337b/tXOn3WXv2Ddlv58FuTvt0YrlXTqPCxu+WnWs5iBajbc3N+7vbs6yKWWF6R1ypPemtPBzbZadwJbYXu77nqOq0u+tH9niPs3p/rmvJTNL70EmrR7tzbGjCj+BlNXllNXWN9Xaaqmr6D83eNcoxnsPznCK3DK2bGusP+43SJp58bhFm1Vw2E3yNTIxt+yUwWVG7VtxYmdoVbye6tL3btSZWW2q6Xy6Tn0TGBjuw+Y8uH02mp1N7kL7gVMIEquPF5fHBZ66fslXChWYxVWruTDO9yKnUEYt5MXJXimeUXdGQz2OuNZcZzbh8ShIqw0iczU6nkVjkIU8TruWmHYCioAlXi9z+3SrI16jxSZBK/AlNrLYZkdNEqU3ioWdC9VJ1bUY5ZJuvdPDTIo9EttJcsHKhYBUTnRLz3434keRMxxsUKJMRJkfYkkrKMCFEEnzN0iShws/dzOfFfLbI3QDN+GhKQeOJU2zlWVG0Y3we8Gt0UBr5SB7nrpYRFSFWMD/rOidY5sLcMIkgT7pmiTGl1fPyv4quWaQyKeYO8ot5oN1/Jo4ro3CJUtGuVhymmOgyidjTWu5gUU9toWokBMI+crpd0xcuZqfOD6ezP2eTZ99XHXUIX8BX8ATn5Y/wDH6FF3AO7PC/oy+Pnhx9M34+DsdinJWuBw+qmE+hdYw3/wPh55mM</latexit>

equations relating positions and velocities:

0 1 0 10 1
ṗn c✓ c s s✓ c c s c s✓ c + s s u
@ ṗe A = @c✓ s s s✓ s + c c c s✓ s s c A @v A
ṗd s✓ s c✓ c c✓ w
0 ˙1 0 10 1
1 sin tan ✓ cos tan ✓ p
@ ✓˙ A = @0 cos sin A @q A
˙ 0 sin sec ✓ cos sec ✓ r

The remaining six equations will come from applying Newton’s 2nd law to
the translational and rotational motion of the aircraft

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide5
 Differentiation of a Vector in Two Reference Frames

Define the vector p in terms of the axes in the body frame:

p = px i b + py j b + pz k b .
Frame F b rotating wrt frame F i
Di↵erentiation with respect to the inertial frame gives Vector p moving in F b

d d b d b d
p = ṗx ib + ṗy jb + ṗz kb + px i + py j + pz k i b ,
dti dti dti dt
b/i
where
dpb⇤
ṗb⇤ = .
dt
Let
d
p = ṗx ib + ṗy jb + ṗz kb .
<latexit sha1_base64="(null)">(null)</latexit>
dtb

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide6
 Differentiation of a Vector in Two Reference Frames
Recall from physics that for any vector v fixed in F b we have
b/i
d
v = ! b/i ⇥ v.
dti
Frame F b rotating wrt frame F i
Therefore Vector p moving in F b
d b
i = ! b/i ⇥ ib
dti
d b
j = ! b/i ⇥ jb
dti
d b
k = ! b/i ⇥ kb .
dti
Therefore
d b d b d b
px i + py j + pz k = px ! b/i ⇥ ib + py ! b/i ⇥ jb + pz ! b/i ⇥ kb
dti dti dti
= ! b/i ⇥ px ib + py jb + pz kb
= ! b/i ⇥ p,

resulting in
d d
p= p + ! b/i ⇥ p.
<latexit sha1_base64="(null)">(null)</latexit>
dti dtb

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide7
 Translational Dynamics
Newton’s 2nd Law:
<latexit sha1_base64="Yp9qKghyyhGgjjvmx11MgyJRi/0=">AAAGRXicrVTNbhMxEN42BEr4a4EDEheLBAGiKkmQACFVVOLCAaGCmrZSNoq83tmN1V3vYjttg2vxNFzhMXgGHoIb4grjTdpkE+CElWhHnm8+z8w3dpAnXOlm89vScuVc9fyFlYu1S5evXL22unZ9V2VDyaDDsiST+wFVkHABHc11Avu5BJoGCewFBy+df+8QpOKZ2NGjHHopjQWPOKMat/prlVu+gCOWpSkVofGVHljjazjWRtm+rwegqa2VIGwKYX+BqLzEkg/4PEde4lgEqFyVGNQigyoxLAB2gwgBKdWDIDK71tbQnXERgtDkDRzpTNxTpC1C8poePSe4an63hniliB9JykzoKPqxNaHuI7mDbJJTwghP681S7g2oJlyRxvTIftx4gZAAYi4M15DyD4BhziJTXGQbLg7bSNQwJVlEaJIQrAukoAmJMtRZkUlYo0jwLKDIFiOcTblkkkb6FLrDUyAhSH6ISh8C0fQABOHuB1JzRy0pQhAP2K+z/GZq6igu4oIcjnGolBuiokv/6M/ENdOEAhBY8rCA+EGWhGqU4sf4WQoxtX0TPOLW15iwmg10HY4xdTWjTAKRvv8/DyG+5PFAP1jUtr9ab240i0UWjdbEqHuTtd1fW77phxkbptg7lmC63VYz1z1Dsd0scdIPFeSUHdAYugloVDinOch11FHijGw+3mhy0TMxYMpajsoBaArUS/VMcektuYs7oZsP/KNYxe5shKGpcj1ApCtMzfvc5p983aGOnvUMF/lQg2Djg6JhQnRG3AtCQi6B6WSEBmWSY3GEDShKggWp8iXMQ7Ddds+MFfNzWkxevWWndtvOXdwQInhv3VOE+UhABbXkVMTYQbM5D3ZTMbnmKjLpvFtizOQRCMw7O+8WmUxtt4X5ubnyT+qt8TT4J7bcrSTOsNBBytn6mT3HRQM1pTpjQiKco9b81Cwau+2N1pON9tt2fas9magV77Z3x7vvtbyn3pb3ytv2Oh6rfKx8qnyufKl+rX6v/qj+HEOXlyYxN7zSqv76DUjGNDA=</latexit>

dVg
m =f
dti
p

u
What is Vg ?
r ib
jb q roll axis
kb • f is the sum of all external forces
v
w
pitch axis
yaw axis
• m is the mass of the aircraft
• Time derivative taken in inertial frame

Using the expression

dVg dVg
= + ! b/i ⇥ Vg
dti dtb
gives
✓ ◆
dVg
m + ! b/i ⇥ Vg =f
dtb
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide8
 Translational Dynamics
<latexit sha1_base64="V3x3eV61mRE0kFLPInpjD9elSbQ=">AAAICnicpVXNjts2EFa2cZ2qf9m2t16IegOkSHZr+9D2skCAtkCP6c9uApiOQVEjiY1+aJKy12X4Bn2a3ope+xI99F06lLXZlexsUpSApAHnm29G31CjSOZCm/H471sHb90evD2880747nvvf/Dh3cOPznVVKw5nvMor9TRiGnJRwpkRJoenUgErohyeRM+/8f4nK1BaVOXPZiNhXrC0FIngzODW4vD2P7SENa+KgpWxpdpkzlIDF8Zqt6AmA8Nc2IHwKwh/BUTLDovMRJ9Ddjh2AVrqDoPeZdAdBg+410EUTHtIwUymE1u4HsF5lLTeKLHn6A3RX4kyhtKQ7y5QRK1FmZLwqGEiNIfE3KeJYtzGV3GL1NnYLCJHHoQEF42qPNabAh+WVgWkzC1s9IVw1IgC9PVAQpVIM/M5OSWX24k7IqIkqCmJqnhDMFsBJBUr0CGdhTdW8ix6g1oQtLcaH7y3nmdRSOfhOgMFpC3heghCI0hFaSXuK3HhSE0oJSt/W2MRFFDrS19Il8uaxeENde0hlJ5r6W/qTQh9yXtYksWFp0gWm+3j112uefiTKDn4jt+k7S43jStja+eJG3N1Za5dP89RuAaSsRVgl5lpJP0vdL2iT8nxDXp1wW3PX92vLpw8IFsdJq79mF4v6k51/Qi1Ol6uPViuj1WTelkfy1U/c/h/M2Mzm7W4OxqfjJtFdo1Ja4yCdj1eHB58gkrzusAxwHPMPZuMpZlbpozgOSBzrUEy/pylMMvBGFCSSVAPScEUlng6PRmLcm5TwINt1KYbgGaJX7Se22ZyO3IPd2KSVAovnDvN7vUIywrtDzYim0HW9/nNfb5ZbZKv51aUsjZQ8m2ipM6JqYj/DZBYKOAm36DBuBL4coRnDEXHF9LdSSljcLPp3G57QqWXguUj35xLe9qfrjEksHT+f4L1KMDvHPvCyhQVtKd98GsmtcKYdlJH9kfXd5eVKtxsgvX5uUhfjCbbOUZfuK5aeVrhi2aF4A9f2j0uFukrqpdMSITnaNI/NbvG+fRk8uXJ9Ifp6NG0PVF3gk+Dz4L7wST4KngUfB88Ds4CPvh28MtAD8zwt+Hvwz+Gf26hB7famI+Dzhr+9S9wZ98k</latexit>

⇣ ⌘
dVg
Expressing m dtb + ! b/i ⇥ Vg = f in the body frame gives
!
dVgb
m + ! bb/i ⇥ Vgb = fb
dtb

where 0 1 0 1 0 1
u p fx
Vg = @ v A
b
! b/i = @q A
b
f = @ fy A
b

w r fz
0 1
u̇
dVgb
Since dtb = @ v̇ A we have that
ẇ
0 1 0 1 0 1 0 1 0 1 0 1
u̇ p u fx rv qw fx
@ v̇ A = 1
@q A ⇥ @ v A + @fy A = @pw 1
ruA + @fy A
m m
ẇ r w fz qu pv fz

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide9
 Rotational Dynamics
Newton’s 2nd Law:
<latexit sha1_base64="jvrOMYxvfxr/6PC1eOtSGNrE/Qg=">AAAGcHichVTrahtHFN5YjZqotzglEOiPTCuHlja4kkouBAwhIdAfpaTFdgJaWczMnt0dPJfNzEiKMp0H6tP0b/MafYKelTeWV3LTAaHDOd/5zn1ZJYXzg8G7Kzudj652P752vffJp599/sWN3ZvHzswshyNupLGvGHUghYYjL7yEV5UFqpiEl+z0WW1/OQfrhNGHflnBRNFCi1xw6lE13e08TS1oWHCjFNVZSD288QuR+TKGn4SOvV560epq/QoTXJymvgRPYxvC1xD+HxBXtViqUmxyVC2ObYCrXIvBbTO4FkMNuNtCKOpqiKK+dHlQcYPgmOWNleXhOK4aYYTOQHvyKyy80d86MtIZ+YUuHhN8vXTcS3NLecjeu5UxhsxPRSQH5L1OYZxJL2VQCB2EByXeAqpqiZC9teceEY5g8wjVxUxSS5RRGHumyBy4N5Y0Pntr4nMXhyiTEyolwQaA1VQ27q7xOhQKSAZWzHEP5kA8PQVNFtYT3CPrBTpgKYjBMIDtOE/0QhcOS7CQGwtkAaSkc/hQB3rYgUtNLJIfSMqMzNxS4V9IMdGCxmlgP4qYekzUrV22Gvn8Da67c0IXmPmqeGayZZN8gaW5S7M6YR8MfsIuD3/CSO9iBieszmF6oz/YH6we2RaGjdBPmvdiurtzK80Mn9UT4RL3cDwcVH4SKHaey3odZg4qyk9pAWMJHkdY0QrsPaKoxb05GO0/vC/0JBSA+Xq7bHugqLF8Nwmr70Mkd1GTERwV/nBwK+1Fj0CVqxuAyNU5bNpq5WW28cznjyZB6GrmQfOzQPlMEm9I/bEhmbC4rHKJAuVWYHWElxRngRW59r1VGcTxaBLORpVWdLWF/WFcy6PNG80gh9ex/iphPhZwfN4KvBdsYTjYBP/PvVv0ae6dhd/jplkbq+J4iPlJyH36R3+YWlGUKMV2t2RhsNBSCX7vXN7gosytqc6ZkAgXabi5NtvC8Wh/+GB/9Nuo/2TUrNS15Kvkm+S7ZJg8TJ4kPycvkqOEd/7s/NX5u/Pu6j/d29073a/PoDtXGp8vk9brfv8v8vFLTw==</latexit>

dh
p =m
dti
r ib u
jb q roll axis • h is the angular momentum vector
kb
v
pitch axis
w • m is the sum of all external moments
yaw axis

• Time derivative taken wrt inertial frame

Therefore we have
dh dh
= + ! b/i ⇥ h = m
dti dtb
Expressing in the body frame gives

dhb
+ ! bb/i ⇥ hb = mb
dtb

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide10
 Rotational Dynamics
For a rigid body, angular momentum is defined as the product of the inertia
<latexit sha1_base64="gYoxV2Px6U3x7UQg/PVu2a/IzX4=">AAAIOniclVVNj9xEEHWWLBMcIFk4cmkxySrA7jAzEh+KtFJQIoRyISB2N9J4dtRul+3Wdrud7vZmPKav8Kf4I1y5IS4c+AEpez49swuiL1OuevX6dXV1TZgLbmy///utvbdu77/dufOOf/fd996/d//ggzOjCs3glCmh9MuQGhA8g1PLrYCXuQYqQwHn4eXTOn5+Bdpwlf1kyxzGkiYZjzmjFl2Tg9t/Bxm8ZkpKmkVVYGzqqsDC1FbGTQKbgqXOb0HYGsJugJi8xZKnfJsjb3HsAkxuWgxml8G0GGrAwxZCUlNDJLWpiSvptgjOwngRDePqzG1nRxDDK1cXhLJLDQL30pxmiQBXnSAayRTPIsgs+VZpQonmCY9IqKLyiCCuEFQTqSQCCkm4IUiIVxQRagiWjORaRQWzRMXNJ4a05dRHPZpPkSBq3EuiKxCKcVuiwazSj/1g5C+1p+4iJHO9ZOl77oJQiciUEn+qAGUk1E2q8HOOYD8Yb6h/nYIGPwgh4VlFBU+yT52/5iGHJz7BtQDkc4Fu7uOY/6i8GJLPyOxi+ElwFDVVJ4fkuIlNy5VruVah2SoUBA3bv6TMN5pet9EirbyJbnajgvK60OZO5cZO8/MCdsayAiQQNMS+gFePL/H20MuZOcbCB/7h/DZurNvzSYX5jZLaLN3SmrmV+lUAjXINLrcgM7cRaMAzt6vVb76Wl7tx+c84TVRGBQEBdasaQjUQRoXATp13r6lbdNGePUK+j+PjaJnlX5e26OzNPNzyyuSUQdX7gsmWggfrVnuAsiOwoGXzUGKtJGmuBxnzmgcMEpKn3zyrPYmmkuDLk0BNoSHyYYogXstBHSUpDM8SfJchDrv6CeVYgkIU0p/c7/Z7/WaRXWOwMLreYr2YHOwNgkixoqZmAgWNBv3cjiuKkhjOAz8oDODpLmkCIwEWT5BT1HKE6jVe/smw1+fZuEoAC2p12U5AM6MSzLhqhrkjD9GDp8ejxQoL1Hg3MyoqTf2wEdnMtu1Y7bwuNips/PW44lleWMjYfKO4EMQqUv8zkIhrnC5YOrxfpjkejrCUasrwQKY9PPMI3Gg4roIYw/hZl4KK7sCt7eH2wP2vifp/hrfGnMXwDqsf3XY4U1q60QD1CYht8HN3EOB4TtFy7WqJROFBU8nZ0cre4qKhWVOtmNpEo0xlMK7SMk/xMi02ODbZYLuldo2zYW/wZW/4w7D7ZLhotzveR97H3iNv4H3lPfG+8154px7bP993+7/s/9r5rfNH58/OX3Po3q1Fzodea3X+eQNX79dR</latexit>

matrix and the angular velocity vector:

4
hb = J! bb/i

where
0R R R 1
(y 2R+ z 2 ) dm R 2 xy 2dm R xz dm
J=@ R xy dm (x R+ z ) dm R 2 yz 2dm
A
xz dm yz dm (x + y ) dm
0 1
Jx Jxy Jxz
4
= @ Jxy Jy Jyz A
Jxz Jyz Jz

Diagonal elements are called moments of inertia. O↵-diagonal elements are

called products of inertia

J determined from mass properties in CAD program or measured experimentally

using a bifilar pendulum

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide11
 Rotational Dynamics
Recalling that
<latexit sha1_base64="ypun8Mq688/626uaIVyLoz+siYQ=">AAAGPXicjVTLbtNAFHUbAsW8WliyGZEiFbUNcSQem0gVbFBXpaIPYbvV2L6xR7XH7nhCiaYjsYWf4jv4AHaIDQu2XNt5OS1VR4pyM/d1zr1n4mUxy2Wn82NhsXGjefPW0m3zzt179x8srzzcz9OB8GHPT+NUHHo0h5hx2JNMxnCYCaCJF8OBd/K28B98ApGzlH+QwwzchIac9ZlPJV4dryz+cXjKeABckl3waYyFQiIjKk3HNp2+oL4KnITKyOurSB95WgXy2NNknTheGgf5MMEv5aQJhBTdx8p7zrQjWQL5bBoxe2T8Oyl/O645bf0GWw9yIKvjmG29SlhOBtyPKA8LTIwjLCBeGgwJwkpgA6Pr+Lb1GF2PdFYJ5QEpWUzcZJxwEXoFfMLPJOW5BkvixNCXa2S2y//LE0ewMJLPRvXrQ6nPZBeoECPyZ0BCqFYSpFJd0mDaoTcD5UhtWroEaJubV8AaLaxkMs2+KmFEY72+1eraLYicRSDgmohxGtfeTBXtAc5FZdhbsM+6bJHh4B1SmqdTU6AJPJiEFtCOl1uddqc85KJhjYyWMTo7+EwsLOYPEtyLH9M8t61OJl1FhWR+DFgUxZtR/4SGYMcgJYiMZiA2SEIFAu112y8Yd1UISEmKYT0BTY56zl1VvmdNnuJNQPqpwA8KobydzVA0yYsZYWQx/HzeV1xe5rMHsv/aVYxnAwncrxr1BzGRKSn+HEjABPgyHqJBfcGQHMH3h4tBQliJw5mfJgm+K+VkAWi766pqb05WjILGLUtP7a7W9ZwA+nCKAblEPAJww7gSVDhOUPXmgxMcs1aluvJCXXNugTmV1/PUrp5381Qk2rYQX6Fp57xlVdJ0znV9WnGYItEoYf7GxJ6rRb18WmpSqV7I5ikHV0XDLMJlSm0WIrPmJXXR2O+2rZft7vtua6s7ktuS8dh4YqwZlvHK2DLeGTvGnuE3Pja+NL42vjW/N382fzV/V6GLC6OcR0btNP/+A6I2MbA=</latexit>

dhb
+ ! bb/i ⇥ hb = mb
dtb
dJ
Because J is unchanging in the body frame, dtb = 0 and

d! bb/i ⇣ ⌘
J + ! bb/i ⇥ J! bb/i = mb
dtb
Rearranging we get
h ⇣ ⌘ i
!˙ bb/i =J 1
! bb/i ⇥ J! bb/i +m b

where 0 1
d! bb/i ṗ
!˙ bb/i = = @q̇ A
dtb
ṙ

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide12
 Rotational Dynamics
<latexit sha1_base64="55f1gpNGg+z/zCj64R/13t9Oa7c=">AAAHBHiclVRLk9tEEFY2xgTzysKRy4AdV6B2XZYPCRdXpYpDYE8bKpukyuPdGkkta1hpJGbGxPIwV34NN4oLB/4H/4YePfxQlgNTpVKru79u9dfdExQpV3o6/efeyf3ee/33H3ww+PCjjz/59OHpZ69UvpYhXIV5mss3AVOQcgFXmusU3hQSWBak8Dq4/c7ZX/8CUvFcvNRlAcuMrQSPecg0qm5O7//1iAp4G+ZZxkRkaAQx/GwNVZqFtxJSQ7XkTKxSsGZu7WBARc5FBEKTH2KiEyCMy1CyWBOuiCqzDBAQEhbka13ZRzRjOgliw+11QDVstDm3re4WdSNSpEzAmfMWZHRxYzalneOr3Nr5dETwv8iALgYt5sKS+YDgoQGsuDAF6iXf2Ern0BbfYzIl9RmTc6fcWkJp5bI3uBytc2NsfcfHbts6OgXkaJePLg/YeJns6y8JU2qdFY5ikkBaKKJzoniGHY3LmjXB0lJxNSGIBMKFaxKQPN7zdWFHJADsDKhBUypL+Up8Yw+ouDbnfv1v4zmhsWShqTlm0U/28d7va9voI9DH+gq8w95BaU1ric+2omXcfB1yWvPqLBt0a0i8nh0y23Znax28DbRxwRr7EbttbtM6VfnPyS60U+3yu+L/49frwhCMDDxnOOa2Sd4aXLy9rVNQ7eRb9Cttt5y7I3Sj700tsDNG7qvt7NsEJFTTXkFIvZANrwflu+G7eTicTqbVIe8KfiMMveZc3pye+DTKw3WGAxumOKMLf1ropWFS8xA3fEDXCgpcfLaCRQpagyxYAfKMZEwiufPZZMrF0qwgr8b8GICiYDisS1NdS5Y8Qk1E4lzigxtSaQ8RhmUKNyZATzePqmtzyrtsi7WOv10aLoq1BhHWieJ16pbM3XEk4hJCnZYosFByLI6ECcN+YEEY6fC+KyKwi9nSNONfOCpYOsSG7+QZzuL/uiOPnDOk2Zpq4VRssq5ZIqa2BoH50XbNIpeZXfj4fynEmv469KnkqwQle8xWusqx0CTj4dlO7sRigdqH2kU6DrQQuYClScoC72Km8b7HIfO7I/Wu8Go28Z9MZi9mw2ezZtweeF94X3mPPd976j3zvvcuvSsv7H3Ze9677L3o/9b/vf9H/8/a9eReg/ncOzr9v/8FZWZKdA==</latexit>

If the aircraft is symmetric about the ib -kb plane, then Jxy = Jyz = 0 and
0 1
Jx 0 Jxz
J=@ 0 Jy 0 A
Jxz 0 Jz

This symmetry assumption helps to simplify the analysis. The inverse of J

becomes
0 1
Jy Jz 0 Jy Jxz
@ 0 2
Jx Jz Jxz 0 A
adj(J) Jxz Jy 0 Jx Jy
J 1= = 2 J
det(J) Jx Jy Jz Jxz y
0 Jz 1
0 Jxz
1
=@ 0 Jy 0 A
Jxz Jx
0

where
4 2
= Jx Jz Jxz

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide13
<latexit sha1_base64="FjXmD/oECzBhBldiVhZ+OFSWrJ0=">AAAKinic3RbbjttE1F0uKaZAlz7yMiLtkmWbJfb2hlCklagE6lNB3bZSJhuNnXFi1R474zFNMp3v4Wt4BfE3nJmxk9jZRfDACyMlc+bcb54zQZ7EhRgM/rxx8N77H3zYufmR+/GtTz797Pbh5y+LrOQhvQizJOOvA1LQJGb0QsQioa9zTkkaJPRV8OZ7TX/1C+VFnLEXYpXTcUpmLI7ikAhATQ475/cwo2/DLE0Jm0o8pRFdKIkLQcI3nCYSCx4TNkuokkOlXBePXCzoUvBUPqURWEUKp0TMg0im6jJAVgPCAZ3FTOZA4vFSoQRhjFL9xxCmYGlDwYuSTDc6wQnEaUiSBIk5ERUZ1SYIIESc0mKDCRQaIrdlzkWwBugI9cnkDDYy8cG0wVpERfMqbF8zaDZP09yGg23dKDBisPl2O0Mt/rH7Yk6ZydQ0ExIHWTItVilsEmcpnRGlJjL4JtbpGm4CeaYuZd9TOKGRGLn9K6RqIZsBw9jbSv+dAI9nc3HsnqBmqQx6rB0OCUMBRXQJzVMUdIpIUcdNknjGvt5Pg4ktVzoHBlxsQa5aKTkathvC1EIvHHESymeTNTTdDwS6UFXlqQlyuVZbWlXFur6WyVPAt7KCOwxXa2hr35KMYNNxg7EVMeA1QWiNXLfUYtd+n1fG8l3sQvPlps+usXeFEeMo7FqfXaDERlbr3hIAv6qZK2LNe9RkW18bc6vYOgKkg4Mw97lP/vnnbrXbxkOYZaxMA8q1m//vHrEW8gXkqgd+QD3Wxwu+9cTSe/zS7+eX/rFlWwPb8jjfYQPkEpCrY9DUr4QW/Bqn/suy2HTATWgcqU4+gpBO6tMZmNscHmiLeFf0IYLANqKPUA/i7nMbe7NkaUvyccOo1zD6YNfoE8TcvcyYU32pvZ1TTtFdy3/3qwIROEYlC/V0LFAWoRSuUiaAAIMp59m0DIXBw+TjIiauO7ndHZwOzEL7gFcBXadazyeHBx7ckmGp9YYJKYqRN8jFWBLQF8KcdXFZ0BzGL5nRUUKFoDwnOeX3UUo4FGLonw5iNpYzCq4JvmoKAMgIjIexNI8Dhe6V+j6PMg4/JpDB7kpIkhZ6bACnHg5Fm6aRV9FGpYiejGXM8lJQFlpDUQlzO0P6pYGmMQxykawAICGPITgUzgkUFgICTbuvjnxK1cgfS1t3nOtUkKQLDbCBfXh8/KuXSoM5hTQraaZfoadfi8xBxlKDQP6s2mSW8VSNPPBPf+b4Xdeznwp+p5rZSmYZBDpP4/D+Bm7pIkGxVbXR1FQ0gg+QjuV8lcM7Ah5Bpsm8dkvtAy/9U+/Rqf+T3z33q3a76XzhfOn0HM957Jw7PzrPnQsn7Pza+a3ze+cP95bru9+631nWgxuVzB2nsdynfwH34254</latexit>

0 1
Rotational Dynamics
0 10 1
l 0 a3 a2 b1
b 4 @ A
Define m = m and recall that a ⇥ b = @ a3 0 a 1 A @b 2 A
n a2 a1 0 b3

Then h ⇣ ⌘ i
!˙ bb/i =J 1
! bb/i ⇥ J! bb/i +m b

can be expressed as
0 1 0 Jz Jxz 1 20 10 1 0 1 0 13
ṗ 0 0 r q Jx 0 Jxz p l
@q̇ A = @ 0 1
Jy 0 A 4@ r 0 p A@ 0 Jy 0 A @q A + @mA5
ṙ Jxz Jx q p 0 Jxz 0 Jz r n
0
0 Jz 1 20 1 0 13
0 Jxz Jxz pq + (Jy Jz )qr l
1
=@ 0 Jy 0 A 4@Jxz (r2 p2 ) + (Jz Jx )prA + @mA5
Jxz Jx (Jx Jy )pq Jxz qr n
0
0 1
1 pq 2 qr + 3 l + 4 n
= @ 5 pr 6 (p
2
r2 ) + J1y mA
7 pq 1 qr + 4 l + 8 n

where ’s are functions of moments and products of inertia

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide14
 Equation of Motion Summary
The equations of motion are a system of 12 firstorder ODE’s:
<latexit sha1_base64="1t7VaEbkhv9pwqsXpg1+5jGGoiQ=">AAATTnicrVhJb9tGFJ64m6tusnMqehnUSJBNhqjEjhHAQIC2aOFL0yJ2AliqQY6GEmFunhlZlln+iF7bP9Vr/0hvRftmONwpSnYjQdLwLd/75i1D2lboOlz0+3/d2Xjv/Q8+/Gjz484nn372+Rfdre0THswYocckcAP21jI5dR2fHgtHuPRtyKjpWS59Y51/I/VvLinjTuC/FouQjjxz4ju2Q0wBomBrcwMNkY8omiOCAuTB24TrMYpAzpFAUxSrtQCbK/iOQBqjMyWZgkyAfYw6LSikEYXcEIWjsIWL1DkreTQhkBsgSA68hQNfi0MdgdQQ7rVgyDXPUOSVzCFHNlx7IG1ncIIssCz7Wsr3RPu2xR6D3IbPhUbgqnIEnSMGUlfvi8EupM8EJFRZHmrshFkAel9h+WCN0WvVA1jhzhQnB2x8QMfwa8O3B7+pFIMFU/YmfDhaKBYUbFJrAw3g2wZ7pnQB/MpoDKQ/om/Rd/hLkL9QbCyQTxSfCPBcWE1g/UhnsagNdb7k7q5Aj1U+AlXBUFXQV7Jhg4Yu1YyVjOoMV2N00H3IHV6DSTJlOOsxDJ44mxucTXPRopddTTNbDjGxft+v6asIj2sxErnca45T5scL/JYx5CX8nAFZmyEv7ZFXEHADx17Bv5o7UpKTijxHaatkbtNey1nWK5fZar42tgsaS8+inKcXMJsyUg+u5DT3wGOhzsE0A+t3+SzzKkovG6VzLf2/nc0Avwf7mGcxQlj3QJ7nSZ4aPZBftkaU3VTMlA0aeXZFcF7ElZN1NS8bZvcqYyCvFqWr67UrdrMa5Heqes7Ld9MmfXqHWVUZvEZt8j0YhYmRtinHoWLil3jlMxSAdbtleUL7S3wTaa8Wfbl/XvmqT6zv0gl+OZt13zKP5b7v9nwIC32frtiKzr9pl4WN/XPRKGXvZNKL+/8eNMlzx5nqrVDttFfTDVQOWMNpXrbbUxisEWMfJA9A+wugyVNF/j5cifi8hZWhWXXQujV/fMvcPAWJi9K7ZFHzDCR+wx6qZ96ROrdk/bwVO362NNaBirVej+e66rPWXM2LfK5b/lQmsR4p3nsarZr3tNPyfcodRhD9GjweqKsrXbUjfWY/1utrqHsywzlqPvcX+tl0XIs7WBr3Wke8hnhJtIeFeCkr2XG3ifu0JW4TXnoGVKvanrPbMNtrYdbTNSh33yrE/bV45nhNe33eiJH2RF6h5HrdKtW541rkg1b26+W4ipoiJmyP9BNHr4F189SlsyT/JuucdXf6u331wvWFoRc7SL9enW1tGMNxQGYe9QVxTc5PjX4oRpHJhENcGneGM05Dk5ybE3rqUiEoC82QsifYM9nE8Q8Hu33HH0UTGnhUsEXZAZa+6VE+itQ/OGJ8DyRjbAcMPr7ASlr0iEyP84VngaVniimv6qSwSXc6E/bBKHL8cCaoT5JA9szFIsDyvyV47DBKhLuAhUmYA5vDZGoyk8CGAMmncxJ4numPo2E4pvHpYBQNbVDDpUyF6e4Ycb4exHHZZ0xtegEGXAAfRt1oKJhj+hPIYHRYNfYgzbH8gU3YkVdVM/BJtJYV/RxX1X7AvPjUAH4utcXw1x1jyJzJFFZxOVvuJICNTj2HPMnWFSzT4jlUhlQGOvUDn46i6SKcQjFF3JFNZlRbqr44Gewa+7uDnwY7L/u63TbRV+hrGFQDRvgl+gG9QseIdM+7v3V/7/6x/ef239v/bP+bmG7c0T53Uel1d/M/rYLlVg==</latexit>

0 1 0 10 1
ṗn c✓ c s s✓ c c s c s✓ c + s s u
@ ṗe A = @c✓ s s s✓ s + c c c s✓ s s c A @v A
ṗd s✓ s c✓ c c✓ w
0 1 0 1 0 1
u̇ rv qw fx
@ v̇ A = @pw ruA + @fy A 1
m
ẇ qu pv fz
0 ˙1 0 10 1
1 sin tan ✓ cos tan ✓ p
@ ✓˙ A = @0 cos sin A @q A
˙ sin cos
0 cos ✓ cos ✓
r
0 1 0 1 0 1
ṗ 1 pq 2 qr 3l + 4n
@q̇ A = @ 5 pr 6 (p
2
r2 )A + @ J1y m A
ṙ 7 pq 1 qr 4l + 8n

where
2
Jxz (Jx Jy + Jz ) Jz (Jz Jy ) + Jxz Jz
1 = 2 = 3 =
Jxz Jz Jx Jxz
4 = 5 = 6 =
Jy Jy
2
(Jx Jy )Jx + Jxz Jx 2
7 = 8 = = Jx Jz Jxz

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide15
 Quaternions
The attitude of a rigid body can be represented by a unit quaternion, which is
<latexit sha1_base64="Q2pYD8V6JsJO4BhA5uPRDeSPVg4=">AAADeHicdVJNb9NAEHUcPor5SuHYy4ikopVCZDsguFSqxIVjkZq2Uhyi9XoSr2qvze66JaT5C/w4bvwQLpyYjQ2FFPbgeXoz8/bNrOMyE9r4/reW2751+87drXve/QcPHz3ubD850UWlOI54kRXqLGYaMyFxZITJ8KxUyPI4w9P4/K3Nn16g0qKQx2ZR4iRncylmgjND1HS79SWShZAJSuMdpwjMkEiVIBQzYKDEXCQQF8kCOJMQIygkeU3VSPyCSiopDHysmEElSbEPl6ngKQhNuZcvLpCbQnnR2EM4gCjGuZDLMmdGiU8rwKkPUUQhqENYhyFEKJPrMi+aeJcpKoQedfT6NgR1CCkwmVg47AGjEs1ZxpSu6aYrusLo6iDoeZ7XWBAGc/EZV15kUa1rPVNzRpOZtBZCBSVTxi7DUtdj/urbIyd969x+hvsfIlOUm0L1Dv4rZEf9bcebdrr+wF8fuAmCBnSd5hxNO1+jpOBVTk/CM6b1OPBLM1nSZYJndsBKY8n4OZvjmKBkOerJcv3jrGCXmARm5G5WSANr9s+OJcu1XuQxVdJjpHozZ8l/5caVmb2ZLIUsK4OS1xfNqgxMAfYvhEQo2kq2IMC4EuQVeMoU47QWbZcQbI58E5yEg+DVwH8fdg/DZh1bzo7zzNlzAue1c+i8c46ckcNb390dt+fuuj/a0H7e3q9L3VbT89T567TDn31rENs=</latexit>

a 4-vector 0 1
e0
Be1 C
e=B @e2 A
C

e3
where e0 , e1 , e2 , and e3 are scalars, and where kek = 1

• e0 is called the scaler part of the quaternion

• (e1 , e2 , e3 )> is called the vector part of the quaternion

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide16
 Quaternions

For a rotation of ⇥ about unit vector v the scalar part is defined as

<latexit sha1_base64="wBArkAfgz/Wd3j5PtZxSHQJEqOg=">AAADBHicfVJLb9NAEF6bVzGPpnCDy4gEqVwiOwjBBakSEuJYpKatlI2i9XqcrGrvWrvjiMjygQt/hQsHEOLKj+DGv2GdRCq0FXOZT/PN89tNq0I5iuPfQXjt+o2bt3ZuR3fu3ru/29t7cOxMbSWOpSmMPU2Fw0JpHJOiAk8ri6JMCzxJz950/MkSrVNGH9Gqwmkp5lrlSgryodle8Ihro3SGmqK3xoIAa2jNgclhwI8WSGIAIjU1Qa0VwRIl+cQBLwUt0rxZtgOgBYKTohAWKmEJlIMMc79SBsJFfBLhLIbXwKVxvMCc9nluhWw23dtm1HKr5gt6BhGfRkJn64bbQVc3PJ8O3Cn9/65+copzpZvKV1n1oQWcJcC5d6ONew4cdXbOd3tEs14/HsZrg8sg2YI+29rhrPeLZ0bWpRdTFsK5SRJXNG38AUoW2Ea8dlgJeSbmOPFQixLdtFk/YgtPfSSD3F+cG02wjv5d0YjSuVWZ+szudneR64JXcZOa8lfTRumqJtRyMyivCyAD3Y+ATFmvdLHyQEir/K4gF8IrSf7fdCIkF0++DI5Hw+TFMH4/6h+MtnLssMfsCdtnCXvJDtg7dsjGTAYfg8/B1+Bb+Cn8En4Pf2xSw2Bb85D9Y+HPPzTA8QQ=</latexit>

✓ ◆
⇥
e0 = cos
2

and the vector part is defined as

0 1
✓ ◆ e1
⇥
v sin = @e2 A
2
e3

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide17
 Quaternions
Conversion Between Euler Angles and Quaternions
<latexit sha1_base64="mzoW2Oub0g8w1mB2rsRrL2EMe24=">AAAFuXicjVRdb9MwFM1YC6N8bfDIyxUrqGNtlaTiQ0JIgwnE4yaxD2npKse5acwcJ9gOo4r6G5F4499gp6Ur7RC1lOTm2vecc48ThzlnSrvur7Ub67X6zVsbtxt37t67/2Bz6+GxygpJ8YhmPJOnIVHImcAjzTTH01wiSUOOJ+HFvp0/+YZSsUx81qMc+ykZChYzSrRJDbbWf5RBGEOQqJxQLN1uj4kx7GdiWgTvUV8iCvhQcJTwTgw5KiAigsOCaJTCrFHjRhDikImScDYUz81rnjB49hYCjd91STQR/hgCjrFugd/CgYsDD3YBBz4OejttsKlzf9e8nPsdM1fd/XN/BwLJhok2z6AR6AQ1mYNVVuoU9Q+sDx0D61nYWW1VnKv/KepVikypP69opqVTqZuhNgIU0azhRiAyJiIUuvFRZikYqTAil23ImaZJuzJMZpxPbSQTG1tNK6vZhqY1rHpWPTaNAItAMylR5ZmImBiaLuDrzHNAjqmhM3shcdF+o73qlWYqiCWhpaUZl/54LlMRLeSMCJvZDYyzC4VXmavCuVWTQmu03dnVuZcgOityL4mecPv/4F4J4rq+VxI94e5V3KtBLHF3VhS9zL3wIQ42t92uWw1YDrxpsO1Mx8Fg82cQZbSwHxPlRKkzz811vyRSM8rR/MuFQnM2XJAhnplQkBRVv6xOnjE8NZkI4kyaS2iosvMVJUmVGqWhWZkSnajFOZu8bu6s0PHrfslEXmgUdEIUFxx0BvYYg4hJpJqPTECoZEYr0IQYY8z/oawJ3mLLy8Gx3/VedN1Df3vPn9qx4Tx2njgtx3NeOXvOJ+fAOXJo7WUtqGEtrr+pk3pS/zJZemNtWvPI+WvU1W/zPeuT</latexit>

= atan2 2(e0 e1 + e2 e3 ), (e20 + e23 e21 e22 )

✓ = asin (2(e0 e2 e1 e3 ))
= atan2 2(e0 e3 + e1 e2 ), (e20 + e21 e22 e23 )

From the yaw, pitch, and roll Euler angles ( , , ✓), the corresponding
quaternion elements are
✓ ✓
e0 = cos cos
cos + sin sin sin
2 2 2 2 2 2
✓ ✓
e1 = cos cos sin sin sin cos
2 2 2 2 2 2
✓ ✓
e2 = cos sin cos + sin cos sin
2 2 2 2 2 2
✓ ✓
e3 = sin cos cos cos sin sin
2 2 2 2 2 2

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide18
 Quaternions
Conversion Between Quaternion and Rotation Matrix
<latexit sha1_base64="XYeYuWFmjRnTEOKgi77cjP6d76U=">AAAD3HicbZJLb9QwEMfTXR4lPNrCkYtFC2rVdpVkheBSqaIXOCC1FX2g9e7KcSa7VhM72E5hFa24cAAhrnwwbnwNPgETN2X7wFKs0X9mfjOeTFxkwtgg+D3Xat+4eev2/B3/7r37DxYWlx4eGlVqDgdcZUofx8xAJiQcWGEzOC40sDzO4Cg+2an9R6egjVDynZ0U0M/ZSIpUcGZRGi7N/alonBI6NgXjUAWdSMgp2VGySSKvwH4EkGSvZBa0rCUmE7KvrCOQt8xq8Wnq09NzQvgcEb5PpRIyAWn9NymxYyAfZoQVmjM7jtMKpsN4ILZWYRhsEBiG9RXVV3dtQK0qVogGfI9BjCGM6POqqVa5g8YqmRCrnI0j0FawDL0sh41ak87BlUZGoWQi5GgGyV3rRBif9vz9uhGyRWgMIyGrIm/ehU0NonXsbxBtYm/u7g4i8oxE2HWIUi2gf20mdTE+chKl/nnY+iyMONA/aHgB6hCY2910WZcQtRZdqBSdVQobySGuQLGMT0Ems/fQvj9cXA46gTvkuhE2xrLXnN3h4i+aKF7m+Bd4xozphUFh+xXDafMMkFkawJ9/wkbQQ1Pi9E2/css5JU9RSUiqNH7SEqdezKhYbswkjzGyXgpz1VeL//P1Spu+7FdCFqUFyc8KpWXmtgE3nSRCA7fZBA3GtcBeCR8zzThuoamHEF598nXjMMJ17gR70fJ21Ixj3nvsPfFWvdB74W17r71d78Djrfetz62vrW/tQftL+3v7x1loa67JeeRdOu2ffwH+IC24</latexit>

If the quaternion eib = (e0 , e1 , e2 , e3 )> represents a rotation from the body to
the inertial frame, then the corresponding rotation matrix is
0 2 2 2 2
1
e1 + e0 e2 e3 2(e1 e2 e3 e0 ) 2(e1 e3 + e2 e0 )
Rbi = @ 2(e1 e2 + e3 e0 ) e22 + e20 e21 e23 2(e2 e3 e1 e0 ) A
2(e1 e3 e2 e0 ) 2(e2 e3 + e1 e0 ) e23 + e20 e21 e22

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide19
 Equation of Motion Summary
<latexit sha1_base64="+amA82fClC4t4m+fvpVoX5VQYZQ=">AAARrHicrVhbb9s2FGbbbe28m70+DXshFqxomiqwnaYNCgwosA0b8tIOSNIMiRtINGUL0S0kHccW/Cf3tsf9kx1S1F2W5W42bJPn8p2P5/DQtK3Qdbjo9/++d//BJ59+9vDR550vvvzq62+6vW/PeDBjhJ6SwA3YuWVy6jo+PRWOcOl5yKjpWS59b13/LPXvbynjTuCfiEVIR5458R3bIaYAUdB7+A+6RD6iaI4ICpAHTxPmYxSBnCOBpmilxgJs7uA9AukKXSnJFGQC7Feo04BCalHIligchQ1cpM7ZyKMOgWyBIDnwBg68FYcqAtkCQY55iiFnMoMc2TD3QNrsfYYssCz6Wsr3bKPvGOQ2vG60P1dVI+gaMZC6ek0MViB9JiChyvInjdzR6AFY+ArNB3uMTtQOwAp5pjg5YOMDPoZPG949+EykGCyYsjfhxdFC8aBgk1gP0AG822DPlM4AOVPxGMjfol/Qr/g70LxWfCyQTxSjCBBdGE1g/EznIq8NdcbkCu9Aj1VOAlXBUFXQV7LLGg1dqxkrGdVZLsfooCeQP9yCSdxlON1jGDxx2jc47ea8hZHOpqkth5hYP59U9GWEvUqMWC7XmuEU+fEcv3UMeQE/Y0BaM+SFNfISAq7haOT8y7kjBTkpyTOUpkpmNs21nKV75TYdzVtju6CxdD/KjnoN/SkjGTCTHW2Ax0Kdg0kG2u/yWeqVl97WSuda+l93NgN8A9YxT2OEMDZAnuVJnhsGyG8bI8rdlM+UDRp5fkVwYqxKZ+tmXjb07l3KQM4WhdmydcW2qwFVp0a/JuOxZrBWM1yrOWhZqXLGhi0yla20r/tHVioZ3aQjVunIxCrxYwWvom0RJ+8VVmwTi8wni9SmkzfVihbqQws1oYU60Ba5336HhLX9eFMrZf9Ll+az9hto4ntDvO5Q5dmo6IaqAizXA/UYhwqD1WK8BMlT0H4ANFl5+bm7EfFVA6uBZtVBm3ZBYrH3kbmRdXdR8g2X17wAiV+zhnL3HaszR9bP27DiF2tjHalY7b6/Ml35njRXNzh5K1t/o5JYzxTvQ41Wznv1lJErjCD6EjyeqtmdrtqxPm/39HgJdY/P8Ax1hZLv6xt9sxxX4g7Xxl3qiEuIF0fbzcVLWH1QZ+D2cQ8a4tbhJWdAuarNOfsYZocNzAxdg+Lu24T4shXPDK9ura9qMZI9kVUo2SNt61Rljyuxjxr5t8tyGTVBjPke6/uCUcO6vu+SblK/qq66O/39vnrg6mCgBztIP95ddf+6HAdk5lFfENfk/GLQD8UoMplwiEtXncsZp6FJrs0JvYChb3qUjyL1Z8MK/wiSMbYDBi9fYCXNe0Smx/nCs8DSM8WUl3VSWKe7mAn7aBQ5fjgT1CdxIHvmYhFg+c8FHjuMEuEuYGAS5gBXTKYmM4mgjMskDMpLrg7OhvuDw/3+H8OdN891Oh6h79EPsJUGsMneoN/RO3SKSHe3+7Z73v2zt9876V30RrHp/Xva5zEqPHr2v4n6QDY=</latexit>

The equations of motion are a system of 13 first-order ODE’s:

0 1 0 10 1
ṗn c✓ c s s✓ c c s c s✓ c + s s u
@ ṗe A = @c✓ s s s✓ s + c c c s✓ s s c A @ vA
ṗd s✓ s c✓ c c✓ w
0 1 0 1 0 1
u̇ rv qw fx
@ v̇ A = @pw ruA + 1 @fy A
m • Quaternion EOM are simpler (linear)
ẇ qu pv fz
0 1 0 10 1 • Require conversion to Euler angles
ė0 0 p q r e0
Bė1 C 1 Bp 0 r qC B C • Must be normalized after each
B C= B C Be 1 C integration step
@ė2 A 2 @q r 0 p A @e 2 A
ė3 r q p 0 e3
0 1 0 1 0 1
ṗ 1 pq 2 qr 3l + 4n
@q̇ A = @ 5 pr 6 (p
2
r2 )A + @ J1y m A
ṙ 7 pq 1 qr 4l + 8n

where
2
Jxz (Jx Jy + Jz ) Jz (Jz Jy ) + Jxz Jz
1 = 2 = 3 =
Jxz Jz Jx Jxz
4 = 5 = 6 =
Jy Jy
2
(Jx Jy )Jx + Jxz Jx 2
7 = 8 = = Jx Jz Jxz

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide20
 Quaternions
In Python, e needs to be normalized after applying the RK4 update step
<latexit sha1_base64="zGgi3pqOT8A34qCaifNvyB/8BMQ=">AAAHXXicjVRfj9tEEHePtCmmlB596AMvIxKqgnKRbSjtS1AlQILywFF6baU4Pa3tsbPK7tq3u7671Ofvwit8I574KoydP3fJBdSV4szOn9/Ob3Zmo0JwYz3v7xt7H3Ru3ure/tD96M7Hdz+5t//pK5OXOsajOBe5fhMxg4IrPLLcCnxTaGQyEvg6mn3f2F+fojY8Vy/tvMCJZJniKY+ZJdXxfud+WDAdqpyrBJUF+FnB4dxOczWAPvZBISYGbA4Rgsq1ZIK/wwRYalEDKwox5yoDO0V48cs3UBYJswjGYtHEoDKlRrIyC/3wAsOLkd933fDUFCzGyhs+jmXtrg936ezfuSyJy2wAZwgxUzswQjpW5+fg9yGaNzmgSlZJnJR0vlZEDWZUEUksYwPMgBtGmHFVUf6Z+qpebQvy0Py8hjDJbYX1sQdhuN74VzfB1c3XENKp63D34QjCVLO48usqILRNdBdoefAQDormc9J8NMG1+kbV2PTCtNQufRamYqVtFCcrHK9VbeSxzQoXfHDBBBcccDt7OIBQUMskbMGh6QjLmYDn9aWMdZPEe/BcQsEj/6C9rrfBl//DfLf3f9Rit/Pu6uzwvV4v2GbwnhVz292ql86mSB3afz5al+ZpfeXYt0EfmEpg6bZM7DuvPwR4ST1rMM7JTH0rIc1prM2q0WmE/MaL5oLmHfC8QM1RxTgABqdMlAh5ukYc+Z7n9QkOZTuxZ7me0RQJMYCotBRhLE9TMLmgBwEEnyH8+sOP/mMDsjS2GfDS0GiD6x7f63lDr11wXfCXQs9ZrsPj/T2fJiMuJU1xLJgxY98r7KRqeicWSPUiaJr5GctwLNAS14IRmQFIpqn+o2DocTWpMswlWj3fDCBRMYlmUrXvXQ1ftIlSrehHb1arvRpRMWnMXEbkSRc2Ndu2RrnLNi5t+nRScVWUlsq8OCgtRVPN5vGEhGuMrZiTwGLNiRzEU0aXToQISeFZnEtJl01zk2A9DibV5kj1/MuR6gV1vRmTYIon5GAs5aNRVCF1G1MZVbAabTtLKnPd/BGJtJLbZk0xC2sUVS/qbXPzltdjn/ITmNrwoueHmmdTkurNaoksJ6JTyePBWt7CYpG5hFojbQKNVa5wUk3nxZQu09Ztk/nbLXVdeBUM/W+HwW9B71mwbLfbzmfO584jx3eeOM+cn5xD58iJO+86f3T+7Px165/uze6d7t2F696NZcx9Z2N1H/wLwI9lCg==</latexit>

to ensure that kek = 1

In Simulink, we can ensure that kek ⇡ 1 by appending the quaternion

kinematics as
0 1 0 10 1
ė0 0 p q r e0
Bė1 C 1 Bp 0 r qC B C @J
B C= B C Be1 C
@ė2 A 2 @q r 0 p A @e2 A @e
ė3 r q p 0 e3
0 2
10 1
(1 kek ) p q r e0
1B p (1 kek2 ) r q C Be1 C
= B CB C
2@ q r (1 kek2 ) p A @e2 A
r q p (1 kek2 ) e3

where J = 18 (1 kek2 )2 and where > 0. The second term forces kek ! 1.
In our experience, a value of = 1000 seems to work well, but a sti↵ solver
like ODE15s must be used

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide21
 Runge-Kutta Integration
The objective is the numerically solve the di↵erential equation
<latexit sha1_base64="8aXZ2R7h4JEz5xbegxHt0Afjg9I=">AAAFuXicfVPdj9NGEDc5QqnpB7SPfVkaWt21IbJTtSDQSUh9KS+IojtAyqbR2h7bC+tdszu+S7T430Tq/9KHju3j7hLQrWR7PDO/+fjtTFIr6TCK/r022rs+vvHFzS/DW199/c23t+9899KZxqZwnBpl7OtEOFBSwzFKVPC6tiCqRMGr5O2fnf3VCVgnjT7CTQ3LShRa5jIVSKrVnZHhDgilCyw9R1jjqcywbH0cSd2GIddG6gw0hkclMJO8gRTlCTDpGJJCNxVYCqbUhjmjyNBpM5nnYAkkhWLwrulzhXwR8tyK1Gfr1mfY7uMBO2T5/pqEKWvoTR9O3hkj1So6OFyvopAvLxfxVCMUluLpgiUGS+ZkBo4VVJLrEqyHoAOe/cq41Ljy9NP+g30qjqLps/XCAZ9mnbCT5ZlBeMTYvS7aPSbqGoR1zOjLKU3et/qxu7uMEUEWcmNh2kGsWcuqN3VcWfKTFrIZpTlxtUjBR7Pffk+r9lLaF3D/1EqEjslmQA7tKiaouwQKqb1QstC/tEOnP3++VY/3j1auvaJhFrILSO98JT+McR4OyTrngyux611uQWfnZYer25NoFvWHfSrEZ8IkODvPaURjnpmUBk1jqoRziziqcemFRZkqIAIbB8ToW1HAQgEi2FrUYKesEpYYO5zPaJSXvgBTAdrNNoBELSpwS9/vUst+Ik3G6Brp0ch67WWEF5VzmyohT7re0u3aOuXnbIsG84dLL3XdIOh0SJQ3iqFh3WLS0ljaLdqjTIqUxkCmLC0F7Qs1RJE0nKamqgRxyesM2sV86Yd14nVHhVCTuL2Q5227jckgh3fk4JDqsaBo2a0UuiAG/eGuc0U0t92Hmsh9tWu2hBmsSeJftLtmbWzVLmKqT0GO/P0k5lYWJUntNluqMNRoWcl0ei7vxBKJuwh1Hmk70EIbDUtfbuqSLhOHIYt3R+pT4eV8Fv8xm/89nzyZn43bzeCH4MdgP4iDB8GT4K/geXAcpKMPo//2xns3xo/HYlyO3wyuo2tnmO+DrTN2/wMYyekU</latexit>

dx
(t) = f (x(t), u(t)), x(t0 ) = x0
dt
Integrating both sides gives
Z t
x(t) = x(t0 ) + f (x(⌧ ), u(⌧ )) d⌧
t0

Note: x(t) appears on both sides of the equation! Therefore, approximation

is required.

Re-write solution integral as

Z t Ts Z t
x(t) = x(t0 ) + f (x(⌧ ), u(⌧ )) d⌧ + f (x(⌧ ), u(⌧ )) d⌧
t0 t Ts
Z t
= x(t Ts ) + f (x(⌧ ), x(⌧ )) d⌧
t Ts

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide22
 RK1 Algorithm
The integral can most easily be approximated using Euler’s method
<latexit sha1_base64="Aw4rebxOu8doyCGQgdliCp8N+xI=">AAAD23icbVNLb9NAEHYTHsW8WjhyGVEBCTSVXQnBBakIISH1Uqo+ImVTa71eO6vYa2t3TBNZ4cIBhLjyx7jxM/gHjO1Q9cHKsj7NzDePb2fDIlUWPe/3Sqd77fqNm6u33Nt37t67v7b+4MjmpRHyUORpboYhtzJVWh6iwlQOCyN5FqbyOJy+q/3Hn6SxKtcHOC/kOOOJVrESHMkUrK/8YVYSSyc4qRjKGZ6qCCeLyveUXrgu07nSkdQIBxMJSqNMDE9BcA1ZbhEktyqdQyiBF4XJZyrjKCMordIJvC9TaZ5ZyCRO8shlI5dRhoCfhBD32Ez1I/oBa5nQCwe8H/d432Vj93zlt0LkJqKE6XzzLEmFg4PALk6QUs16DHnZ34SyBX22GdXgLDVFNmENp4lrUVvq4oy6zKQhfdJ/09ZCgclLJImbwWnYU6MQpQZuXRbKROmKpyrRzxcuwCzw4OkboGqB1wdKjjwBxsgzDPzaQ50E1XTgL6iRFvRb/yyYNszWCC+avonkMqmjswoX+lUW6EPqe7/UiRzslogcYmUsDkg0aZbiQ25gf9enIm6wtuFtec2Bq8Bfgg1nefaCtV8sygXJolGk3NqR7xU4rrhBJVK5cFlpZcHFlCdyRFDzTNpx1ezmAp6QJYKYysc5ddxYzzMqnlk7z0KKpNWZ2Mu+2vg/36jE+PW4Uroo6SJEWyguU8Ac6kWHSBkpkHYzUlzQdSkBYsINF0jPoRbBvzzyVXC0veW/3PI+bm/seEs5Vp1HzmOn5/jOK2fH+eDsOYeO6Aw7nztfO9+64+6X7vfujza0s7LkPHQunO7Pv8exNGc=</latexit>

Z b
f (⇠)d⇠ ⇡ (b a)f (a)
a

Accordingly,
Z t
f (x(⌧ ), u(⌧ )) d⌧ ⇡ Ts f (x(t Ts ), u(t Ts ))
t Ts

The numerical integration routine can be written as

x0 = x(t0 )
X1 = f (xk 1 , uk 1 )
xk = xk 1 + Ts X1

This is the Runge-Kutta first-order method or RK1

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide23
 RK2 Algorithm
To improve accuracy, the integral can be approximated using the trapezoidal
<latexit sha1_base64="CI7qswnPktC/hR68Iyle60fCHNY=">AAAFRnicpVTPj9NGFDYktNSl7VKOXJ66FMXdZGVHqtoLEtBLJS4U7cJKmWCNx2NnFHvGGj+zCVb+Oi49c+NP4NJDq6rXvrGzKy+0J0aW/fx+fu+9T5NUhaoxDN9duz4a3/js85tf+F/e+urrbw5uf/u8No0V8lSYwtizhNeyUFqeosJCnlVW8jIp5Itk/Yuzv3glba2MPsFtJZclz7XKlOBIqvj2iLFaUpTOcdUylBs8Vymudm0UKr3zfaaN0qnUCCcGVFlZ80oCF6KxXGyngCsJSqPMLS9AcA0JWSvy2qiSo0yhqZXOOze0vJKvjUrJ0zaF9NnCZxQb85cJZBO2UUFKL2B9PEySGQ9YITOcsIyqtdmEB0fZJAl27XzHrMpXGPhsOcT4SAhjU6pYbKeX+VucncT17iVSlc2EIW+CKTS9QCXpc1mzL+S8XYlE5QsX0sUH02YvBHDUZcIuDQaBc1w6JPec8h6oGhq91uZcQ21oAhKePYkAzXAyoBB47bNE5kq3ZVOg2+APu0+E/Ph/IPuMXaDuFPRDn4Gia6X3HYguH3UmdTqEOCQFLVY3pbREqOKCCY5ZYE2D5H5BinOrEKUe9MwLlWvKtolDuP8ACEgcBsCYfxZHTkHY4nY9i3aEpxf21jnch4G5b4WCrvpt4nWX9tLpyqCgJxa4Wkfgcu4JBX23F+CutEp7pcdx+Vmjczl70iByqKUwOp0R76SFUuLKpGAsrXzu+/HBYXgcdgc+FqK9cOjtz9P44C1LjaCBahQFr+tFFFa4bLlFJQpJ7CA2VVyseS4XJGpeynrZdtfADr4nTQoZFc8MAe60w4iWl3W9LRPyJAqu6g9tTvlftkWD2c/LVumqoRWKvlDWFI7S7k6BVFkpsNiSwAUtWgkQK07jRrp53BCiD1v+WHg+P45+PA5/mx8+nO7HcdO7633nTbzI+8l76P3qPfVOPTF6M3o/+nP01/j38R/jv8f/9K7Xr+1j7nhXzg3vXxQYsP4=</latexit>

rule Z b ✓ ◆
f (a) + f (b)
f (⇠)d⇠ ⇡ (b a)
a 2
Accordingly,
Z t
Ts ⇥ ⇤
f (x(⌧ ), u(⌧ ))d⌧ ⇡ f (x(t Ts ), u(t Ts )) + f (x(t), u(t))
t Ts 2

x(t) is unknown so use RK1 to approximate it as

Z t
Ts h
f (x(⌧ ), u(⌧ ))d⌧ ⇡ f (x(t Ts ), u(t Ts ))
t Ts 2
i
+ f (x(t Ts ) + Ts f (x(t Ts ), u(t Ts )), u(t Ts ))

The numerical integration routine can be written as

x0 = x(t0 )
X1 = f (xk 1 , uk 1 )
X2 = f (xk + T s X 1 , uk 1 )
1
Ts
xk = xk 1+ (X1 + X2 )
2
This is the Runge-Kutta second-order method or RK2

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide24
 RK4 Algorithm
<latexit sha1_base64="GZOUfTyDiHTIwzJByaiGTJUCmiE=">AAAEknicnVNLb9NAEHabAMU82gI3LiMqIKGickLLoxLQqhw4cCjQl5QN0Xo9dla119buum1k+f/we7jxbxjHoUofFMRItmbn8e18szN+FktjPe/nzGyjee36jbmb7q3bd+7OLyze2zNprgXuijRO9YHPDcZS4a6VNsaDTCNP/Bj3/cOtyr9/hNrIVO3YUYb9hEdKhlJwS6bB4ux3ZpCyVGSHBbN4Yo9lYIdl8Uaq0nWZSqUKUFnYVIBHqMBHa1EDzzKdnshkDAPSQOpbTjUEkBupIvgqk8yk6qmBL3mM0OJUFOQERamQcc39NObtdZf1XCaVHfBvPoQtdiLbAf2A1fjAYgxti4Wai8J/zsviZcm0jIa2XXvCFm/DMqyG04F82S+L7u9Acoctvz05uaw/RWpniGAsVwHXASmaW4xGFRubwrGWFsFSxFmu3FxS9H/U3L265r+4z1H6gCE132U+RlIVPJaRela6B4MOPHkL4wsZo2O3PtITLEMNvDMwFTBFtk8JXHHzGOXFn1G6/4RSF7M6BUMAQMBT2cTPZaiCUzKDhSVvxRsLXFQ6E2XJmcj2YOEHC1KRJ/TQIubG9DpeZvsF11aKGEuX5QYzLg55hD1SFU/Q9IvxSpXwmCwBhKmmjwZlbJ3OKHhizCjxKZLmYmjO+yrjZb5ebsPX/UKqLLeoRH1RmMfVxFX7CYHUKGw8IoULGkEpQAxpXwRtnama0DlP+aKy113prK14n7tLG96kHXPOQ+eR03I6zitnw/nobDu7jmjMN9Ya7xrvmw+a683N5lYdOjszybnvnJHmp1/tAnSW</latexit>

An even better approximation is obtained using Simpson’s Rule (area under a

parabola):
Z b ✓ ◆✓ ✓ ◆ ◆
b a a+b
f (⇠)d⇠ ⇡ f (a) + 4f + f (b)
a 6 2

The standard strategy is to write the approximation as

Z b ✓ ◆✓ ✓ ◆ ✓ ◆ ◆
b a a+b a+b
f (⇠)⇠ ⇡ f (a) + 2f + 2f + f (b)
a 6 2 2

Define

X1 = f (a)
✓ ◆
Ts a+b
X2 = f (a + X1 ) ⇡ f
2 2
✓ ◆
Ts a+b
X3 = f (a + X2 ) ⇡ f
2 2
X4 = f (a + Ts X3 ) ⇡ f (b)

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide25
 RK4 Algorithm
The numerical integration routine can be written as
<latexit sha1_base64="DtpYXTUNloXgzSHZpJdYpPDfY98=">AAACRXicbVA9SwNBEN3z2/MrammzGASrcCdILAUbSwWjQi6Evb1Jbsne7rE7p4Yjf87G3s5/YGOhiK1uPgpNHFh4vJk3b+fFuRQWg+DFm5tfWFxaXln119Y3Nrcq2zvXVheGQ4Nrqc1tzCxIoaCBAiXc5gZYFku4iXtnw/7NHRgrtLrCfg6tjHWV6AjO0FHtShRZcCLVxbSMEB7wXiSYDsq6UAPfj5QWKgGF9CoFqooMjFNKKhRC14xWUKMLdOaUM0VjoPdGIIKizPp+u1INasGo6CwIJ6BKJnXRrjxHiebORiGXzNpmGOTYKplBwSUM/KiwkDPeY11oOqhYBrZVjlIY0APHJLSjjXvuxyP2t6JkmbX9LHaTGcPUTveG5H+9ZoGdk1YpVF64w/jYqFNIipoOI6WJMMBR9h1g3J0vOOUpM4yjC34YQjh98iy4PqqFx7Xg8qh6GkziWCF7ZJ8ckpDUySk5JxekQTh5JK/knXx4T96b9+l9jUfnvIlml/wp7/sH6way5A==</latexit>

<latexit sha1_base64="TZE/FdSy1ZV50a5erovwa5+IXgs=">AAADJHichVJdi9QwFE3r11q/ZvXRl+CgzLo6tF11RRAWfPFxhZ3dgelQ0jTthEnT0tzudij9Mb74V3zxwQ988MXfYtopizMVvBByOOeem9ybBJngCmz7l2FeuXrt+o2dm9at23fu3hvs3j9VaZFTNqGpSPNpQBQTXLIJcBBsmuWMJIFgZ8HyXaOfnbNc8VSewCpj84TEkkecEtCUv2u88RTTLhnDovKAlXDBQ1jU1SGXtWV5AYu5rIjgsXxaWxiXvo2fvMXlCHx7D3uepqa+01DRqPSr5XOnfoaLNbjU3Q0d433sRTmh1Ymv6sqtdYG+5+B/HrfvedHz6Oym1nZq6S/bLrrEjdqvajxqWtrHbnPzdjvQmy6/Z3lMhpfT8AdDe2y3gfvA6cAQdXHsD757YUqLhEmggig1c+wM5hXJgVPBassrFMsIXZKYzTSUJGFqXrWPXOPHmglxlOZ6ScAt+7ejIolSqyTQmQmBhdrWGvJf2qyA6PW84jIrgEm6PigqBIYUNz8GhzxnFMRKA0Jzru+K6YLoeYH+V80QnO2W++DUHTsvx/YHd3hkd+PYQQ/RIzRCDjpER+g9OkYTRI2Pxmfjq/HN/GR+MX+YP9epptF5HqCNMH//AQE47Tg=</latexit>

x0 = x(t0 ) <latexit sha1_base64="nZiqsz21tjwPxt8mDDJmQGLcoO8=">AAAB+XicdVDLSgMxFM3UV1tfoy7dBItQQUpS0LYLoeDGZUX7gLaUTJppQzMPkkxpHfonblwo4tbPcOdC8G/MtAoqeuDC4Zx7k3uPEwquNELvVmppeWV1LZ3Jrm9sbm3bO7sNFUSSsjoNRCBbDlFMcJ/VNdeCtULJiOcI1nRG54nfHDOpeOBf62nIuh4Z+NzllGgj9Wy70w90PJnBM+jmJ8fRUc/OoQJCCGMME4JLp8iQSqVcxGWIE8sgV03fvL1c1TK1nv1qnqCRx3xNBVGqjVGouzGRmlPBZtlOpFhI6IgMWNtQn3hMdeP55jN4aJQ+dANpytdwrn6fiImn1NRzTKdH9FD99hLxL68dabfcjbkfRpr5dPGRGwmoA5jEAPtcMqrF1BBCJTe7QjokklBtwsqaEL4uhf+TRrGATwroEueqebBAGuyDA5AHGJRAFVyAGqgDCsbgFtyDByu27qxH62nRmrI+Z/bAD1jPH4jJldc=</latexit>

ẋ = f (x, u)
X1 = f (xk 1 , uk 1 )
Ts <latexit sha1_base64="sUMv1nE2Be11qcjxtvGNJz17Btk=">AAACHnicdVDLSgMxFM34tr6qLt1cFKEi1MxotV0IghuXClZH2jJk0owGMw+SjFiGfoYrN/6KGxeKCK507Sf4AZqxCip6ILmHc+4lucdPBFca42err39gcGh4ZLQwNj4xOVWcnjlQcSopq9NYxNL1iWKCR6yuuRbMTSQjoS/YoX+6nfuHZ0wqHkf7upOwVkiOIx5wSrSRvGIFzj0bNs2NYRmagSQ02/dUN1vvQsk11jI4ruf0yqoprre2VPCKC7jsVHBtowq4XMF2bc0xBONqbR2DbUiOhS24WHl5PXrb9YqPzXZM05BFmgqiVMPGiW5lRGpOBesWmqliCaGn5Jg1DI1IyFQr+1ivC4tGaUMQS3MiDR/q94mMhEp1Qt90hkSfqN9eLv7lNVIdVFsZj5JUs4j2HgpSATqGPCtoc8moFh1DCJXc/BXoCTERaZNoHsLXpvA/OXDKdqWM90waJdTDCJpD86iEbLSBttAO2kV1RNEluka36M66sm6se+uh19pnfc7Moh+wnt4BXFehYQ==</latexit>

Ts
X2 = f (xk 1 + X1 , uk 1 ) x1 = x0 +
6
(X1 + 2X2 + 2X3 + X4 ) <latexit sha1_base64="DKpd5lTM5wGiFoKSGvXTyGv9BXI=">AAAB6nicdZDLSgMxFIYz9VbrrepGcBMsQldDplZHVxbcuKxoL9AOJZOmbWgmMyQZoQx9BDcuFHHrwufwEdz5BD6FYKatoKI/BH6+/xxyzvEjzpRG6M3KzM0vLC5ll3Mrq2vrG/nNrboKY0lojYQ8lE0fK8qZoDXNNKfNSFIc+Jw2/OFZmjeuqVQsFFd6FFEvwH3BeoxgbdBls1Pu5AvIdsvlExdBZKOJUuMcldwD6MxI4fTjJXzfeQ6qnfxruxuSOKBCE46Vajko0l6CpWaE03GuHSsaYTLEfdoyVuCAKi+ZjDqG+4Z0YS+U5gkNJ/R7R4IDpUaBbyoDrAfqd5bCv7JWrHvHXsJEFGsqyPSjXsyhDmG6N+wySYnmI2MwkczMCskAS0y0uU7OHOFrU/i/qZds59BGF06hUgRTZcEu2ANF4AAXVMA5qIIaIKAPbsAduLe4dWs9WI/T0ow169kGP2Q9fQIxupId</latexit>

X4
2
Ts x 0 + Ts X3
<latexit sha1_base64="3FovGoRD9OlAmWRb8ZT2dDMxOrQ=">AAAB8nicbZDLSgMxFIYzXmu9Vd0IboJFKAhlRhHdWXDjskJvMB2GTJppQ3MZkoxYhj6GGxeKuHHhU/gI7nwCn0IwvSy09YfAx/+fQ845UcKoNq776SwsLi2vrObW8usbm1vbhZ3dhpapwqSOJZOqFSFNGBWkbqhhpJUognjESDPqX43y5i1RmkpRM4OEBBx1BY0pRsZa/l3oHtdCDVvhaVgoumV3LDgP3hSKl9/v8mv/lVfDwke7I3HKiTCYIa19z01MkCFlKGZkmG+nmiQI91GX+BYF4kQH2XjkITyyTgfGUtknDBy7vzsyxLUe8MhWcmR6ejYbmf9lfmriiyCjIkkNEXjyUZwyaCQc7Q87VBFs2MACworaWSHuIYWwsVfK2yN4syvPQ+Ok7J2V3RuvWCmBiXLgAByCEvDAOaiAa1AFdYCBBPfgETw5xnlwnp2XSemCM+3ZA3/kvP0A3biUsg==</latexit>

X3 = f (xk 1 + X2 , uk 1 )
2
X4 = f (xk 1 + Ts X3 , uk 1 )
X3
<latexit sha1_base64="aaaUefmlnct3FKi/ZzA9tyLK6BA=">AAAB6nicdZDLSgMxFIYzXmu9Vd0IboJF6GrItNXRlQU3LivaC7RDyaRpG5rJDElGKEMfwY0LRdy68Dl8BHc+gU8hmGkrqOgPgZ/vP4ecc/yIM6URerPm5hcWl5YzK9nVtfWNzdzWdl2FsSS0RkIeyqaPFeVM0JpmmtNmJCkOfE4b/vAszRvXVCoWiis9iqgX4L5gPUawNuiy2Sl1cnlku+XyiYsgstFEqXGOim4JOjOSP/14Cd93n4NqJ/fa7oYkDqjQhGOlWg6KtJdgqRnhdJxtx4pGmAxxn7aMFTigyksmo47hgSFd2AuleULDCf3ekeBAqVHgm8oA64H6naXwr6wV696xlzARxZoKMv2oF3OoQ5juDbtMUqL5yBhMJDOzQjLAEhNtrpM1R/jaFP5v6kXbObTRhZOvFMBUGbAH9kEBOMAFFXAOqqAGCOiDG3AH7i1u3VoP1uO0dM6a9eyAH7KePgEwNpIc</latexit>

Ts
xk = xk 1 + (X1 + 2X2 + 2X3 + X4 ) <latexit sha1_base64="iCLyuHr3DNRgguAfU5pwGmudkCE=">AAAB9HicbVDLSgNBEOz1mcRX1KOXwSAEhLgbED0GvHiMmBckyzI7mU2GzM6uM7PBuOQ7vHhQxKs/4s2D4N84eRw0saChqOqmu8uPOVPatr+tldW19Y3NTDa3tb2zu5ffP2ioKJGE1knEI9nysaKcCVrXTHPaiiXFoc9p0x9cTfzmkErFIlHTo5i6Ie4JFjCCtZHce88+rXkKtbzyWdnLF+ySPQVaJs6cFCqZh6+P22q26uU/O92IJCEVmnCsVNuxY+2mWGpGOB3nOomiMSYD3KNtQwUOqXLT6dFjdGKULgoiaUpoNFV/T6Q4VGoU+qYzxLqvFr2J+J/XTnRw6aZMxImmgswWBQlHOkKTBFCXSUo0HxmCiWTmVkT6WGKiTU45E4Kz+PIyaZRLznnJvnEKlSLMkIEjOIYiOHABFbiGKtSBwB08wjO8WEPryXq13matK9Z85hD+wHr/Ac15k7Q=</latexit>

x0 + Ts X2 /2
6 <latexit sha1_base64="E9QAZqbo1kFr0eOp+BwM+ABk00I=">AAAB6nicdZDLSgMxFIYz9VbrrepGcBMsQlfDTK2Oriy4cVnRXqAdSiZN29BkMiQZoQx9BDcuFHHrwufwEdz5BD6FYKatoKI/BH6+/xxyzgkiRpV2nDcrMze/sLiUXc6trK6tb+Q3t+pKxBKTGhZMyGaAFGE0JDVNNSPNSBLEA0YawfAszRvXRCoqwis9iojPUT+kPYqRNuiy2Sl18gXH9srlE8+Bju1MlBr3qOQdQHdGCqcfL+J955lXO/nXdlfgmJNQY4aUarlOpP0ESU0xI+NcO1YkQniI+qRlbIg4UX4yGXUM9w3pwp6Q5oUaTuj3jgRxpUY8MJUc6YH6naXwr6wV696xn9AwijUJ8fSjXsygFjDdG3apJFizkTEIS2pmhXiAJMLaXCdnjvC1Kfzf1Eu2e2g7F26hUgRTZcEu2ANF4AIPVMA5qIIawKAPbsAduLeYdWs9WI/T0ow169kGP2Q9fQIuspIb</latexit>

X2
<latexit sha1_base64="LZFvjgMe9FqLLp7gQ40Xcjk5y0g=">AAAB9HicbVDLSgNBEOz1mcRX1KOXwSAEhLgbED0GvHiMmBckyzI7mU2GzM6uM7PBuOQ7vHhQxKs/4s2D4N84eRw0saChqOqmu8uPOVPatr+tldW19Y3NTDa3tb2zu5ffP2ioKJGE1knEI9nysaKcCVrXTHPaiiXFoc9p0x9cTfzmkErFIlHTo5i6Ie4JFjCCtZHce88+rXkKtTznrOzlC3bJngItE2dOCpXMw9fHbTVb9fKfnW5EkpAKTThWqu3YsXZTLDUjnI5znUTRGJMB7tG2oQKHVLnp9OgxOjFKFwWRNCU0mqq/J1IcKjUKfdMZYt1Xi95E/M9rJzq4dFMm4kRTQWaLgoQjHaFJAqjLJCWajwzBRDJzKyJ9LDHRJqecCcFZfHmZNMol57xk3ziFShFmyMARHEMRHLiAClxDFepA4A4e4RlerKH1ZL1ab7PWFWs+cwh/YL3/AMvzk7M=</latexit>

x0 + Ts X1 /2
<latexit sha1_base64="zP8TynTvYsd/mKEizQk9XCG+Fog=">AAACRnicbVCxThtBEJ0zCZAjIQ4p06xiIaXBukNEUCKlQXJjEAYk38na2xv7VuztnnbnINbJX5eGmo5PSJOCCNFmbVwkkNGu9PRmnubNyyolHUXRXdBaefV6dW39Tbjx9t3m+/aHrTNnaitwIIwy9iLjDpXUOCBJCi8qi7zMFJ5nl9/m/fMrtE4afUrTCtOST7QcS8HJU6N2mjj0Ij2hokkIv9O1zKmYNftSz8Iw0UbqHDWx00I65h8VyE5qPcGdXk3E2dgboWLH2BwtK5EKk7MkCY1lJ729cNTuRN1oUewliJegA8vqj9q3SW5EXfqVQnHnhnFUUdpwS1IonIVJ7bDi4pJPcOih5iW6tFnEMGPbnsm9I+u/t7xg/1Y0vHRuWmZ+suRUuOe9Ofm/3rCm8UHaSF3VhFo8LRrXipFh80xZLi0KUlMPuLDSe2Wi4JYL8snPQ4ifn/wSnO1246/d6Hi3cxgt41iHT/AZvkAM+3AIR9CHAQj4AT/hHn4HN8Gv4CF4fBptBUvNR/inWvAHEQmxQg==</latexit>

This is the Runge-Kutta fourth-order method

or RK4 x0
<latexit sha1_base64="/zGzmPbTItqFGytsFrl8u+rrkts=">AAAB6nicbZC7SgNBFIbPeo3xFrURbAaDkCrsCqKdARvLiOYCSQizk9lkyFyWmVkxLHkEGwtFbC18Dh/BzifwKQQnl0ITfxj4+P9zmHNOGHNmrO9/eguLS8srq5m17PrG5tZ2bme3alSiCa0QxZWuh9hQziStWGY5rceaYhFyWgv7F6O8dku1YUre2EFMWwJ3JYsYwdZZ13dtv53L+0V/LDQPwRTy59/v6mv/TZTbuY9mR5FEUGkJx8Y0Aj+2rRRrywinw2wzMTTGpI+7tOFQYkFNKx2POkRHzumgSGn3pEVj93dHioUxAxG6SoFtz8xmI/O/rJHY6KyVMhknlkoy+ShKOLIKjfZGHaYpsXzgABPN3KyI9LDGxLrrZN0RgtmV56F6XAxOiv5VkC8VYKIMHMAhFCCAUyjBJZShAgS6cA+P8ORx78F79l4mpQvetGcP/sh7/QETpZIH</latexit>

<latexit sha1_base64="aLqk5NZhlFiNnXH6GFw6T3qMn60=">AAAB6nicdZDLSgMxFIYz9VbrrepGcBMsQldDplZHVxbcuKxoL9AOJZOmbWhmMiQZoQx9BDcuFHHrwufwEdz5BD6FYKatoKI/BH6+/xxyzvEjzpRG6M3KzM0vLC5ll3Mrq2vrG/nNrboSsSS0RgQXsuljRTkLaU0zzWkzkhQHPqcNf3iW5o1rKhUT4ZUeRdQLcD9kPUawNuiy2XE6+QKy3XL5xEUQ2Wii1DhHJfcAOjNSOP14Ee87z0G1k39tdwWJAxpqwrFSLQdF2kuw1IxwOs61Y0UjTIa4T1vGhjigyksmo47hviFd2BPSvFDDCf3ekeBAqVHgm8oA64H6naXwr6wV696xl7AwijUNyfSjXsyhFjDdG3aZpETzkTGYSGZmhWSAJSbaXCdnjvC1Kfzf1Eu2c2ijC6dQKYKpsmAX7IEicIALKuAcVEENENAHN+AO3FvcurUerMdpacaa9WyDH7KePgEtLpIa</latexit>

X1

t0 t0 + T s
<latexit sha1_base64="BWZB4SsmNApZnxFfxbbKWNUc8VY=">AAAB7nicbZDJSgNBEIZrXGPcol4EL4NBCAhhRhC9GfDiMUI2SIahp9OTNOll6O4RwpCH8OJBEa95Dh/Bm0/gUwh2loMm/tDw8f9VdFVFCaPaeN6ns7K6tr6xmdvKb+/s7u0XDg4bWqYKkzqWTKpWhDRhVJC6oYaRVqII4hEjzWhwO8mbD0RpKkXNDBMScNQTNKYYGWs1Teid10IdFope2ZvKXQZ/DsWb73f5dTzm1bDw0elKnHIiDGZI67bvJSbIkDIUMzLKd1JNEoQHqEfaFgXiRAfZdNyRe2adrhtLZZ8w7tT93ZEhrvWQR7aSI9PXi9nE/C9rpya+DjIqktQQgWcfxSlzjXQnu7tdqgg2bGgBYUXtrC7uI4WwsRfK2yP4iysvQ+Oi7F+WvXu/WCnBTDk4gVMogQ9XUIE7qEIdMAzgEZ7hxUmcJ+fVeZuVrjjzniP4I2f8A6Wvk3w=</latexit>

<latexit sha1_base64="CxH18uahsU8uOpFsMJxGPSp7SnE=">AAAB6nicbZC7SgNBFIbPxluMt6iNYDMYhFRhVxDtDNhYRjQXSJYwO5lNhsxlmZkVQsgj2FgoYmvhc/gIdj6BTyE4uRSa+MPAx/+fw5xzooQzY33/08ssLa+srmXXcxubW9s7+d29mlGpJrRKFFe6EWFDOZO0apnltJFoikXEaT3qX47z+h3Vhil5awcJDQXuShYzgq2zbmzbb+cLfsmfCC1CMIPCxfe7+jp4E5V2/qPVUSQVVFrCsTHNwE9sOMTaMsLpKNdKDU0w6eMubTqUWFATDiejjtCxczooVto9adHE/d0xxMKYgYhcpcC2Z+azsflf1kxtfB4OmUxSSyWZfhSnHFmFxnujDtOUWD5wgIlmblZEelhjYt11cu4IwfzKi1A7KQWnJf86KJSLMFUWDuEIihDAGZThCipQBQJduIdHePK49+A9ey/T0ow369mHP/JefwANjZID</latexit>

<latexit sha1_base64="FsLEFliRXkmMkFwwb8JyrYgl+rE=">AAAB8HicbZDLSgMxFIbP1Ftbb1WXboJFKAh1piC6LLhxWbEXpR2GTJq2oUlmSDJCHfoUblwo4tZXcedC8G1MLwtt/SHw8f/nkHNOGHOmjet+O5mV1bX1jWwuv7m1vbNb2Ntv6ihRhDZIxCN1G2JNOZO0YZjh9DZWFIuQ01Y4vJzkrXuqNItk3Yxi6gvcl6zHCDbWujOBe1IP9GklKBTdsjsVWgZvDsVq9uHr46aWqwWFz043Iomg0hCOtW57bmz8FCvDCKfjfCfRNMZkiPu0bVFiQbWfTgceo2PrdFEvUvZJg6bu744UC61HIrSVApuBXswm5n9ZOzG9Cz9lMk4MlWT2US/hyERosj3qMkWJ4SMLmChmZ0VkgBUmxt4ob4/gLa68DM1K2Tsru9desVqCmbJwCEdQAg/OoQpXUIMGEBDwCM/w4ijnyXl13malGWfecwB/5Lz/AJTKkn8=</latexit>

t0 + Ts /2

Adapted from HilberTraum - Own work, CC BY-SA 4.0,

https://commons.wikimedia.org/w/index.php?curid=64366870

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide26
 DYNAMICS AND RK4
IMPLEMENTATION
See example code

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide27
 Project
<latexit sha1_base64="d4tahC4J1Vwn/CkrRMuajdZlnJE=">AAAFg3icjVRbTxNBFF6kVVxvoI++TKQaiFhbxOCLCYSQqIkJRgok0DSzs2fbCbMz61wKSwP/w7/lm//GM7OtlIuJ+7Jnz5zL931zziaF4Ma2Wr9n7szW6nfvzd2PHzx89PjJ/MLTPaOcZtBhSih9kFADgkvoWG4FHBQaaJ4I2E+Ot/z5/hC04Uru2rKAbk77kmecUYuu3sLsz6ME+lyOQLocNLVwHh9xC3n8OS8E5CAtsQMgXzf3CPxwIcsQlZFceZP0+RAk4ZJsTw4vlt4122vLmKWV6w+I/1xfbhKyaQy2QD+tSnJZOGuIVeErc5KFilRD5VDI0BAqU+zlcaBdoCSQTlI8JuzszUSlJck0zaFJtgZU9oGiAqSg3mWRPzED5USK8Uy4tOqQU2NWKmvSAJsVWqWO2UASRdWW05VwUGHm+C0IUzLllRaIkwBlA2Isqoc8O6YqP9X8r0qbRQGYeXqxjYFxXElN9kDzrLxU5jahvSxMaQ3MkqTEWikf8tRRIUpiwFou+/+UTSg8DBjpKQ+KUyKVPAOtyJAKByEcOQ1RHl+oxPHCmcouIY1R8HAJWhWae7ITBvF3TATS+NIbnZ6dN3wY1n/jGwSBETu6+qVWhqmCM+zl8CqxUwL2BFAbrYQIKEp6gtLs4g2DsQEIMTx3Igiy4qletkEigQPmxYWgiGDCakIdITdEIxRuyOo9vCp2BU2qm5A87WlYKB4YxDZ9WycDDLxK25JrtP+zcHyEkzG1hqQ3v9hqtsJDbhrtsbEYjZ+d3vyvo1Qx55kzgbN92G4VtjuiOMJM+L12BgrKjmkfDtGUOJ2mOwr/kHPyEj1pGOZM4c4H73TGiObGlHmCkTm1A3P9zDtvOzt0NvvQHYVlB8mqRpkTYYnxh0RS7mcapzjllGlcKpyOAa4O85sTowjt65RvGnurzfb7Zuvb6uJGayzHXPQ8ehEtRe1oPdqIPkU7USditaj2qva21qrX66/rq/W1KvTOzDjnWXTlqX/8A74L05M=</latexit>

1. Implement the MAV equations of motion given in Equations (3.14) through

(3.17). Assume that the inputs to the function are the forces and moments
applied to the MAV in the body frame. Changeable parameters should
include the mass, the moments and products of inertia, and the initial
conditions for each state. Use the parameters given in Appendix E.

2. Verify that the equations of motion are correct by individually setting the
forces and moments along each axis to a nonzero value and convincing
yourself that the motion is appropriate.
3. Since Jxz is non-zero, there is gyroscopic coupling between roll and yaw.
To test your simulation, set Jxz to zero and place nonzero moments on l
and n and verify that there is no coupling between the roll and yaw axes.
Verify that when Jxz is not zero, there is coupling between the roll and
yaw axes.

Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012 Chapter 3: Slide28