Successive Loop Closure

Chapter 6
Successive Loop Closure
Beard & McLain, “Small Unmanned Aircraft,” Princeton University Press, 2012, Chapter 6: Slide 1
Architecture
Destination, Map
obstacles
Path planner
Waypoints
Status
Path manager
Path Definition
Tracking error
Airspeed, Path following

Altitude,
Heading, Position error
commands
Autopilot
Servo commands
State estimator
Wind
Unmanned Vehicle
On-board sensors
Outline
• Different Options for Autopilot Design
– Successive Loop Closure
– Total Energy Control
– LQR Control
Successive Loop Closure
Open-loop system
Closed-loop system
SLC: Inner Loop Closed
At frequencies below
inner-loop bandwidth,
approximate CLTF as 1,
then design middle loop
SLC: Two Loops Closed
At frequencies below
middle-loop bandwidth,
approximate CLTF as 1,
then design outer loop
Key idea: Each successive loop must be lower in bandwidth

--- typically by a factor of 10 or more
Lateral-directional Autopilot
Course Control
Roll Control
rc = 0 <latexit sha1_base64="A3fH1v5cEaEIudosH6r19u0rI+o=">AAAB8HicbVDLSsNAFJ20Pmp8VcWVm2ARXJVEF7osuHFZwT6gDWUyuWmHTiZh5kYooT/hSlAQt/6F3yAIrvwUnT4W2nrgwuGce7n3niAVXKPrflqF4srq2nppw97c2t7ZLe/tN3WSKQYNlohEtQOqQXAJDeQooJ0qoHEgoBUMryZ+6w6U5om8xVEKfkz7kkecUTRSuxuCQNpTvXLFrbpTOMvEm5NKrfjx/Xb4BfVe+b0bJiyLQSITVOuO56bo51QhZwLGdjfTkFI2pH3oGCppDNrPp/eOnROjhE6UKFMSnan6eyKnsdajODCdMcWBXvQm4n9eJ8Po0s+5TDMEyWaLokw4mDiT552QK2AoRoZQpri51WEDqihDE5FtmxS8xZ+XSfOs6p1X3RsTh0tmKJEjckxOiUcuSI1ckzppEEYEuSeP5MlS1oP1bL3MWgvWfOaA/IH1+gNFjZQv</latexit>
r Cr (s2 + 2⇣!n s + !n2 ) r
1
<latexit sha1_base64="6OOY3nqChLZrJnTzIcHQr3tHOk4=">AAAB6XicbZBNS8NAEIYn9avWr6pHL4tF8FSSXuyx4MVjFfsBbSib7aRdutmE3Y1QQv+BFw+KePUfefPfuGlz0NYXFh7emWFn3iARXBvX/XZKW9s7u3vl/crB4dHxSfX0rKvjVDHssFjEqh9QjYJL7BhuBPYThTQKBPaC2W1e7z2h0jyWj2aeoB/RieQhZ9RY60FVRtWaW3eXIpvgFVCDQu1R9Ws4jlkaoTRMUK0HnpsYP6PKcCZwURmmGhPKZnSCA4uSRqj9bLnpglxZZ0zCWNknDVm6vycyGmk9jwLbGVEz1eu13PyvNkhN2PQzLpPUoGSrj8JUEBOT/Gwy5gqZEXMLlCludyVsShVlxoaTh+Ctn7wJ3Ubds3zv1lqNIo4yXMAlXIMHN9CCO2hDBxiE8Ayv8ObMnBfn3flYtZacYuYc/sj5/AELoIz2</latexit>
<latexit sha1_base64="7VmVkfJ34GzMAfPk2ppqwcHbw3E=">AAAB6XicbZC7SgNBFIbPeo3rLWppMxgEq7CbRhsxYGMZxVwgWcLsZDYZMju7zJwVwhLwAWwsFLH1IXwPO9/GyaXQxB8GPv7/HOacE6ZSGPS8b2dldW19Y7Ow5W7v7O7tFw8OGybJNON1lshEt0JquBSK11Gg5K1UcxqHkjfD4fUkbz5wbUSi7nGU8iCmfSUiwSha60673WLJK3tTkWXw51C6+nQvHwGg1i1+dXoJy2KukElqTNv3UgxyqlEwycduJzM8pWxI+7xtUdGYmyCfTjomp9bpkSjR9ikkU/d3R05jY0ZxaCtjigOzmE3M/7J2htFFkAuVZsgVm30UZZJgQiZrk57QnKEcWaBMCzsrYQOqKUN7nMkR/MWVl6FRKfuWb71StQIzFeAYTuAMfDiHKtxADerAIIIneIFXZ+g8O2/O+6x0xZn3HMEfOR8/ZSiOsw==</latexit>
sha1_base64="E0THBuggfHWUnJr9vp8B8bg4diM=">AAAB6XicbZDLSgMxFIZP6q2Ot6pLN8EiuCoz3ehGLLhxWcVeoB1KJs20oZnMkGSEMvQN3LhQxG0fwvdwI76NmbYLbf0h8PH/55BzTpAIro3rfqPC2vrG5lZx29nZ3ds/KB0eNXWcKsoaNBaxagdEM8ElaxhuBGsnipEoEKwVjG7yvPXIlOaxfDDjhPkRGUgeckqMte6V0yuV3Yo7E14FbwHl6w/nKpl+OfVe6bPbj2kaMWmoIFp3PDcxfkaU4VSwidNNNUsIHZEB61iUJGLaz2aTTvCZdfo4jJV90uCZ+7sjI5HW4yiwlRExQ72c5eZ/WSc14aWfcZmkhkk6/yhMBTYxztfGfa4YNWJsgVDF7ayYDoki1Njj5EfwlldehWa14lm+c8u1KsxVhBM4hXPw4AJqcAt1aACFEJ7gBV7RCD2jN/Q+Ly2gRc8x/BGa/gBWt5An</latexit>
<latexit sha1_base64="HVFfYrvNas4GrfXg18Rb57AAalI=">AAAB7nicbZBNS8NAEIYn9avWr6pHL4tF8FSSXvQiFLx4rGA/oI1ls920SzebsDsRSuiP8OJBEa/+Hm/+G7dpDtr6wsLDOzPszBskUhh03W+ntLG5tb1T3q3s7R8cHlWPTzomTjXjbRbLWPcCargUirdRoOS9RHMaBZJ3g+ntot594tqIWD3gLOF+RMdKhIJRtFZXPzJyQ9xhtebW3VxkHbwCalCoNax+DUYxSyOukElqTN9zE/QzqlEwyeeVQWp4QtmUjnnfoqIRN36WrzsnF9YZkTDW9ikkuft7IqORMbMosJ0RxYlZrS3M/2r9FMNrPxMqSZErtvwoTCXBmCxuJyOhOUM5s0CZFnZXwiZUU4Y2oYoNwVs9eR06jbpn+d6tNRtFHGU4g3O4BA+uoAl30II2MJjCM7zCm5M4L86787FsLTnFzCn8kfP5A+Z2jow=</latexit>
sha1_base64="sY/S3B+9UzeqCPkQe8fnoqpU4Zs=">AAAB7nicbZC7SgNBFIbPxltcb1FLm8EgWIXdNNoEAzaWEcwFkhhmJ7PJkNnZZeasEJaAr2BjoYitb+B72Pk2Ti6FJv4w8PH/5zDnnCCRwqDnfTu5tfWNza38truzu7d/UDg8apg41YzXWSxj3Qqo4VIoXkeBkrcSzWkUSN4MRtfTvPnAtRGxusNxwrsRHSgRCkbRWk19z0iFeL1C0St5M5FV8BdQvPp0K48AUOsVvjr9mKURV8gkNabtewl2M6pRMMknbic1PKFsRAe8bVHRiJtuNht3Qs6s0ydhrO1TSGbu746MRsaMo8BWRhSHZjmbmv9l7RTDy24mVJIiV2z+UZhKgjGZ7k76QnOGcmyBMi3srIQNqaYM7YVcewR/eeVVaJRLvuVbr1gtw1x5OIFTOAcfLqAKN1CDOjAYwRO8wKuTOM/Om/M+L805i55j+CPn4wdADZBJ</latexit>
sha1_base64="z/2dypsx4m/Lm2xSi8WWnVAGI8I=">AAAB7nicbZDLSgMxFIbP1Fsdb1WXboJFcFVmurGbYsGNywr2Au1YMmmmDc1kQpIRytCHcONCERdufAPfw434NqaXhbb+EPj4/3PIOSeUnGnjed9Obm19Y3Mrv+3u7O7tHxQOj5o6SRWhDZLwRLVDrClngjYMM5y2paI4DjlthaOrad66p0qzRNyasaRBjAeCRYxgY62WuiOoirxeoeiVvJnQKvgLKF5+uFX59uXWe4XPbj8haUyFIRxr3fE9aYIMK8MIpxO3m2oqMRnhAe1YFDimOshm407QmXX6KEqUfcKgmfu7I8Ox1uM4tJUxNkO9nE3N/7JOaqJKkDEhU0MFmX8UpRyZBE13R32mKDF8bAETxeysiAyxwsTYC7n2CP7yyqvQLJd8yzdesVaGufJwAqdwDj5cQA2uoQ4NIDCCB3iCZ0c6j86L8zovzTmLnmP4I+f9BzGckb0=</latexit>
(s + psp )(s2 + 2⇣dr !ndr s + !n2 dr )

<latexit sha1_base64="nCNoHcQNYdPwoNpMmEigAYg9mcE=">AAAB6XicbZBNT8JAEIan+IX4hXr0spGYeJG0XPRI4sUjGgsk0JDtsoUN222zOzUhDf/AiweN8eo/8ua/cYEeFHyTTZ68M5OdecNUCoOu++2UNja3tnfKu5W9/YPDo+rxSdskmWbcZ4lMdDekhkuhuI8CJe+mmtM4lLwTTm7n9c4T10Yk6hGnKQ9iOlIiEoyitR6uvEG15tbdhcg6eAXUoFBrUP3qDxOWxVwhk9SYnuemGORUo2CSzyr9zPCUsgkd8Z5FRWNugnyx6YxcWGdIokTbp5As3N8TOY2Nmcah7Ywpjs1qbW7+V+tlGN0EuVBphlyx5UdRJgkmZH42GQrNGcqpBcq0sLsSNqaaMrThVGwI3urJ69Bu1D3L926t2SjiKMMZnMMleHANTbiDFvjAIIJneIU3Z+K8OO/Ox7K15BQzp/BHzucP3dSM2A==</latexit>
sha1_base64="7BARNB3AEjf56WHCl1/2ub4je0g=">AAAB6XicbZC7SgNBFIbPxluMt6ilzWAQbAy7abQzYGMZxVwgWcLs5GwyZHZ2mZkVwpI3EMFCEVvfyM7O17Bzcik08YeBj/8/hznnBIng2rjup5NbWV1b38hvFra2d3b3ivsHDR2nimGdxSJWrYBqFFxi3XAjsJUopFEgsBkMryZ58x6V5rG8M6ME/Yj2JQ85o8Zat2det1hyy+5UZBm8OZQuv5PHLwCodYsfnV7M0gilYYJq3fbcxPgZVYYzgeNCJ9WYUDakfWxblDRC7WfTScfkxDo9EsbKPmnI1P3dkdFI61EU2MqImoFezCbmf1k7NeGFn3GZpAYlm30UpoKYmEzWJj2ukBkxskCZ4nZWwgZUUWbscQr2CN7iysvQqJQ9yzduqVqBmfJwBMdwCh6cQxWuoQZ1YBDCAzzDizN0npxX521WmnPmPYfwR877DwkEj+4=</latexit>
sha1_base64="IJU2sXXlCh+hIwHh8Vb0L3ln+Mg=">AAAB6XicbZC7SgNBFIbPeo3xFrW0GQyCjWE3jXYGbCyj5AZJCLOTs8mQ2dllZlYIS95ABQtFbH0Fn8TOxvewc3IpNPGHgY//P4c55/ix4Nq47qeztLyyurae2chubm3v7Ob29ms6ShTDKotEpBo+1Si4xKrhRmAjVkhDX2DdH1yO8/otKs0jWTHDGNsh7UkecEaNtW5OvU4u7xbcicgieDPIX3zHD1+V9/tyJ/fR6kYsCVEaJqjWTc+NTTulynAmcJRtJRpjyga0h02Lkoao2+lk0hE5tk6XBJGyTxoycX93pDTUehj6tjKkpq/ns7H5X9ZMTHDeTrmME4OSTT8KEkFMRMZrky5XyIwYWqBMcTsrYX2qKDP2OFl7BG9+5UWoFQue5Ws3XyrCVBk4hCM4AQ/OoARXUIYqMAjgDp7g2Rk4j86L8zotXXJmPQfwR87bDyNHkYE=</latexit>
<latexit sha1_base64="HfiHpXm+daNHKCHEUixBAvsqjhw=">AAACcnicbVHRTtswFHXCGNDBKEy8bNJmqJCKCijJC3tE4oVHkCggNSVy3JvWwnEi+wZRonwAv8fbvmIv+4A5JRoFdiVLx+f4XNvnxrkUBj3vl+MufFj8uLS80vq0uvZ5vb2xeWmyQnPo80xm+jpmBqRQ0EeBEq5zDSyNJVzFtye1fnUH2ohMXeA0h2HKxkokgjO0VNR+DBPNeHlwEmnaNTdBLwgfAFmYpTBmUakqQ3v0ZXcT7FVl1/TyKES4x9Lk1d6crWFHuvpneaFet5rj66a0FbU73qE3K/oe+A3okKbOovZTOMp4kYJCLpkxA9/LcVgyjYJLqFphYSBn/JaNYWChYimYYTmLrKK7lhnRJNN2KaQzdt5RstSYaRrbkynDiXmr1eT/tEGByc9hKVReICj+fFFSSIoZrfOnI6GBo5xawLgW9q2UT5idAdop1SH4b7/8HlwGh77F517nOGjiWCbfyA7pEp8ckWNySs5In3Dy29lyvjs/nD/uV3fbbbJzncbzhbwqd/8vgyO+FQ==</latexit>
sha1_base64="j5D8D/+EaeWxfLPHBFL9qI42GRY=">AAACcnicbVFNT9tAEF27lEKgkBZxKRIsjSoFBZDtSzkicekRJAJIcbDWm3FYsV5bu+OqqWWJK/+NEzd+BZf+gK6TCMLHSCu9eTNvdvdNnEth0PMeHPfD3Mf5TwuLjaXlzyurzS9fz0xWaA5dnslMX8TMgBQKuihQwkWugaWxhPP4+qiun/8GbUSmTnGUQz9lQyUSwRlaKmreholmvNw7ijRtm8ugE4R/AVmYpTBkUakqQzv0ObsMdqqybTp5FCL8wdLk1c6MbMoOdPUkeaZejprh66G0ETVb3r43DvoW+FPQOgzulm4IIcdR8z4cZLxIQSGXzJie7+XYL5lGwSVUjbAwkDN+zYbQs1CxFEy/HFtW0R+WGdAk0/YopGN2VlGy1JhRGtvOlOGVeV2ryfdqvQKTg34pVF4gKD65KCkkxYzW/tOB0MBRjixgXAv7VsqvmN0B2i3VJvivv/wWnAX7vsUntRtkEgtkg3wnbeKTn+SQ/CLHpEs4eXTWnU1ny/nnfnO33dak1XWmmjXyItzd/+ySv94=</latexit>
sha1_base64="B9iIZqcWVF0RhjclwGCybakLn/g=">AAACcnicbVHBTttAEF07baGhLWlRL63UbhtVCgog2xc4IiGhHqnUAFIcrPVmnKxYr63dcZXU8gfwb5y4IfEPXPgA1klUAnSkld68mTe7+ybOpTDoedeO23jx8tXK6uvm2pu379Zb7z8cm6zQHHo8k5k+jZkBKRT0UKCE01wDS2MJJ/H5QV0/+QPaiEz9xmkOg5SNlEgEZ2ipqHURJprxcvsg0rRjzoJuEP4FZGGWwohFpaoM7dKH7CzYrMqO6eZRiDDB0uTV5pJswQ519U/yQD0etcTXQ2kzarW9HW8W9DnwF6C9H1yuTRqHN0dR6yocZrxIQSGXzJi+7+U4KJlGwSVUzbAwkDN+zkbQt1CxFMygnFlW0R+WGdIk0/YopDN2WVGy1JhpGtvOlOHYPK3V5P9q/QKTvUEpVF4gKD6/KCkkxYzW/tOh0MBRTi1gXAv7VsrHzO4A7ZZqE/ynX34OjoMd3+JftRtkHqvkM/lOOsQnu2Sf/CRHpEc4uXU+Ol+cr86d+8n95rbnra6z0GyQR+Fu3QNohcD7</latexit>
⌧r s
kr
<latexit sha1_base64="U0a30+4YvkzOVSFcqtGZHPhgSIw=">AAAB7HicbZBNS8NAEIYn9avWr6pHL4tF8FSSXvRY8OKxgmkLbSib7aZdutmE3YlQQn+DFw+KePUHefPfuGlz0NYXFh7emWFn3jCVwqDrfjuVre2d3b3qfu3g8Oj4pH561jVJphn3WSIT3Q+p4VIo7qNAyfup5jQOJe+Fs7ui3nvi2ohEPeI85UFMJ0pEglG0lj8b5XoxqjfcprsU2QSvhAaU6ozqX8NxwrKYK2SSGjPw3BSDnGoUTPJFbZgZnlI2oxM+sKhozE2QL5ddkCvrjEmUaPsUkqX7eyKnsTHzOLSdMcWpWa8V5n+1QYbRbZALlWbIFVt9FGWSYEKKy8lYaM5Qzi1QpoXdlbAp1ZShzadmQ/DWT96EbqvpWX5wG+1WGUcVLuASrsGDG2jDPXTABwYCnuEV3hzlvDjvzseqteKUM+fwR87nDxkkjsw=</latexit>
sha1_base64="gDdB5A2KY4anfCNAK37VFc74rgA=">AAAB7HicbZDPSgMxEMZnq9Za/1U9egkWwVPZ7UWPBS8eW3DbQruUbDrbhmazS5IVytJn8OJBEa8+h8/gTXwZ0z8Hbf0g8OObGTLzhang2rjul1PY2t4p7pb2yvsHh0fHlZPTtk4yxdBniUhUN6QaBZfoG24EdlOFNA4FdsLJ7bzeeUCleSLvzTTFIKYjySPOqLGWPxnkajaoVN2auxDZBG8F1Uax9f0BAM1B5bM/TFgWozRMUK17npuaIKfKcCZwVu5nGlPKJnSEPYuSxqiDfLHsjFxaZ0iiRNknDVm4vydyGms9jUPbGVMz1uu1uflfrZeZ6CbIuUwzg5ItP4oyQUxC5peTIVfIjJhaoExxuythY6ooMzafsg3BWz95E9r1mme5ZdOow1IlOIcLuAIPrqEBd9AEHxhweIRneHGk8+S8Om/L1oKzmjmDP3LefwAPEJD+</latexit>
sha1_base64="RGvNkIiSrkKJODxAw8/FonS73aU=">AAAB7HicbZBNSwMxEIZn/ai1flU9egkWwVPZ7UWPBS8eW3DbQltKNp1tQ7PZJckKZelvEEFEEa/6Xzx6E/+M6cdBW18IPLwzQ2beIBFcG9f9ctbWNzZzW/ntws7u3v5B8fCooeNUMfRZLGLVCqhGwSX6hhuBrUQhjQKBzWB0Na03b1FpHssbM06wG9GB5CFn1FjLH/UyNekVS27ZnYmsgreAUjVX//54vH+v9YqfnX7M0gilYYJq3fbcxHQzqgxnAieFTqoxoWxEB9i2KGmEupvNlp2QM+v0SRgr+6QhM/f3REYjrcdRYDsjaoZ6uTY1/6u1UxNedjMuk9SgZPOPwlQQE5Pp5aTPFTIjxhYoU9zuStiQKsqMzadgQ/CWT16FRqXsWa7bNCowVx5O4BTOwYMLqMI11MAHBhzu4AmeHek8OC/O67x1zVnMHMMfOW8/hcGS1g==</latexit>
<latexit sha1_base64="lJlMqHzIwMl/9rWXK/4QEudtzoI=">AAACBnicbZDLSsNAFIZPvNZ6i7oUYbAIglCSbnRZcOOygr1AE8JkOmmHTiZhZiKUkJUbX8WNC0Xc+gzufBunbRBt/WHg4z/ncOb8YcqZ0o7zZa2srq1vbFa2qts7u3v79sFhRyWZJLRNEp7IXogV5UzQtmaa014qKY5DTrvh+Hpa795TqVgi7vQkpX6Mh4JFjGBtrMA+8SKJSe5pnAUSqeKH0AVyi8CuOXVnJrQMbgk1KNUK7E9vkJAspkITjpXqu06q/RxLzQinRdXLFE0xGeMh7RsUOKbKz2dnFOjMOAMUJdI8odHM/T2R41ipSRyazhjrkVqsTc3/av1MR1d+zkSaaSrIfFGUcaQTNM0EDZikRPOJAUwkM39FZIRNLtokVzUhuIsnL0OnUXcN3zq1ZqOMowLHcArn4MIlNOEGWtAGAg/wBC/waj1az9ab9T5vXbHKmSP4I+vjG17gmFE=</latexit>
sha1_base64="nDdTjYfytUOD/ZXe2e1MvdjBApo=">AAACBnicbZDLSsNAFIZP6q3WW9SlCINFEISSFEGXBUFcVrAXaEOYTCft0MkkzEyEErJy42O4deNCEbc+gzvfxukF0dYfBj7+cw5nzh8knCntOF9WYWl5ZXWtuF7a2Nza3rF395oqTiWhDRLzWLYDrChngjY005y2E0lxFHDaCoaX43rrjkrFYnGrRwn1ItwXLGQEa2P59mE3lJhkXY1TXyKV/xA6RW7u22Wn4kyEFsGdQblWuTp7BIC6b392ezFJIyo04Vipjusk2suw1Ixwmpe6qaIJJkPcpx2DAkdUednkjBwdG6eHwliaJzSauL8nMhwpNYoC0xlhPVDztbH5X62T6vDCy5hIUk0FmS4KU450jMaZoB6TlGg+MoCJZOaviAywyUWb5EomBHf+5EVoViuu4RuTRhWmKsIBHMEJuHAONbiGOjSAwD08wQu8Wg/Ws/VmvU9bC9ZsZh/+yPr4BnAImdk=</latexit>
sha1_base64="p7FlzB8RlrkinhcGs17cUmdW4Lw=">AAACBnicbZDLSsNAFIZPvNZ6i7oUYbAIglCSIuiyIoi4qmAv0IQwmU7aoZMLMxOhhKzc+AY+gxsXinTrM7jzbZxeEG39YeDjP+dw5vx+wplUlvVlLCwuLa+sFtaK6xubW9vmzm5DxqkgtE5iHouWjyXlLKJ1xRSnrURQHPqcNv3+5ajevKdCsji6U4OEuiHuRixgBCtteeaBEwhMMkfh1BNI5j+ETpCde2bJKltjoXmwp1Cqlq9On4YXNzXP/HQ6MUlDGinCsZRt20qUm2GhGOE0LzqppAkmfdylbY0RDql0s/EZOTrSTgcFsdAvUmjs/p7IcCjlIPR1Z4hVT87WRuZ/tXaqgnM3Y1GSKhqRyaIg5UjFaJQJ6jBBieIDDZgIpv+KSA/rXJROrqhDsGdPnodGpWxrvtVpVGCiAuzDIRyDDWdQhWuoQR0IPMAzvMKb8Wi8GO/GcNK6YExn9uCPjI9vEqebEg==</latexit>
⌧r s + 1
Yaw Damper
Roll Autopilot
Roll Autopilot
• The book suggests using an integrator on roll in the roll loop to correct
for any steady-state error due to disturbances
• Our current suggestion is to not have an integrator on inner loops,

including the roll loop
• Integrators add delay and instability -> not a good idea for inner-
most loops
• An integrator will be used on the course loop to correct for steady-

state errors
Lateral-directional Autopilot
Course Hold Loop
For the course loop, note the presence of the input disturbance
<latexit sha1_base64="vSjH9yWoS/3faPsHFbqhkWF43js=">AAAE4HiclVPfb9MwEM7aAiMbsMEjLxYdEtJGaSvxQ0iTJiEheEEDrdukpqsc55KaOXZmO5s6K++8IV75z5D4Y7ikYe02JMBSlPP5u+/O353DTHBju90fS41m68bNW8u3/ZXVO3fvra3f3zcq1wwGTAmlD0NqQHAJA8utgMNMA01DAQfh8Zvy/OAUtOFK7tlpBqOUJpLHnFGLrvF6Iw+k4jICaf23ShM7AcKQ3AARSmVbRCoLlRdpDUgGRMXVnssstyTCEnMdUjzwg1OTUQau2+n1n3NZ+P6ce2C4TAglu++RXlqthABNYsw4y7ZVcWKKTEnMbdViJUylKZURKb8gWMxJuCEJPwVJwqkfDP2ATTjZJiSINWXueOyysSt9RUGSZ/vjhBiySdDNf7t9crEqQOHMUX+G+VtoHUDK3RGriTbJBeOshjr2P3hr2qje+8FoUcgP2I/XfhBCwqXjFlJ+DogpLbI3AV1JQsk5aIUt+hdVN2YX2JiTXOt1xUZtZSvNMTUBaXJEIflnYOUslVihzp7GGk5yDJ4u9sn4AchoXu94rd3tdKtFrhu92mh79dodry99CiLF8hQ1YIIaM+x1MztyVFvORKlAbgCn75gmMERT0hTMyFUPpCCP0RNV0xbj8JHKuxjhaGrMNA0RmVI7MVfPSuefzoa5jV+NXPUU8MqzRHEuKqXxtaEEGtURpRaUaY61EjahOBcW3yR2Fc7qLrggi6AY9kduNjZBVt6MinavmNt9nNdLMRHEcIIAY7EeDcIFVnMqExTEbV8Fp6haUf7wErFLi6oLvauaXzf2+53ei073Y7+906/7sew99B55T7ye99Lb8d55u97AY42fzUZzpbnaCltfWl9b32bQxlId88C7tFrffwF8qZni</latexit>
Using a PI controller for course, the response to the course command and
disturbance is given by
kp g/Vg s + ki g/Vg c g/Vg s

= + d
s2 + kp g/Vg s + ki g/Vg s2 + kp g/Vg s + ki g/Vg
Note:
c
• There is a zero in the response to the course command
• The presence of the zero at the origin ensures rejection of low-frequency
disturbances
TF Zero Affects Response
10 1.4
−10 1.2
magnitude (dB)
−20
−30 1
−40
−50 0.8
−60
0
0.6
phase (deg)
−30
0.4
−60 0.2
−90 0
−2 −1 0 1 2 3
10 10 10 10 10 10 0 2 4 6 8 10 12
frequency (rad/sec) (sec)
With a zero, the canonical second-order TF is given by

<latexit sha1_base64="GDm9IN7Kg7HEIyPX6WH4oGF67QM=">AAAEhHicpVPbbtNAEHVDKMXcWnhCvIxIkFq1hMSIy0tRJSTUJ1RQb1KcRuv1OF7V3jW765Z0ZYmv4RV+h79hnAR6A4FgJMujmXN2ZufMRkUmjO12v801rjSvzl9buO7fuHnr9p3Fpbu7RpWa4w5XmdL7ETOYCYk7VtgM9wuNLI8y3IsOX9f5vSPURii5bccFDnI2kiIRnFkKDZca9/eETYHBCWq1BjZF4EwqSYAMDHIl48dKx6hh+w0IAyNxhBKisR/2/c1lswLrECaacReEJ2gZhCrHERs6WYGBVR9O7TRzEFTOHASwClPSX3IoQcUyTOzy2ZKVm6GIHmoxSu0KhKWkliMCoZtiz54E5p/qV0MXWvxoXTt+Ets2qATawcEsJuOqav+cVFU3u/qHNv6zid+WBj8c+P5bZZHkZBbak/PbkDJDOsciSVCjtIDkcAvHKQla615vQC0x7Y+p88PFVrfTnRhcdnozp+XNbGu4NPc+jBUvc2LzjBnT73ULO3BMW8EzrPywNFgwfshG2CdXshzNwE2WuIJHFIkhUZo+qj6JnmU4lhszziNC5sym5mKuDv4q1y9t8nLghCxKi5JPCyVlBlZB/SJoIprmkI3JYVwL6hV4ykgtS+/GDyUec5XnTMYuLGKs+sHgh5hFfTOWtXrVqU8SnefEmOAHAhhL/WjMSDUtmBzRQNz6RXBOU6vqH10icTmlSYXexZlfdnaDTu95p/suaG0EMz0WvAfeQ2/Z63kvvA1v09vydjze+NT43PjS+Nqcb641nzafTaGNuRnnnnfOmq++A8ivc6I=</latexit>
✓ ◆
2⇣!n s + !n2 2⇣ !n2 s !n2
H(s) = 2 = +
s + 2⇣!n s + !n2 !n s2 + 2⇣!n s + !n2 s2 + 2⇣!n s + !n2
| {z } | {z }
d/dt of 2nd -order TF 2nd -order TF
Note that ⇣ has a di↵erent e↵ect when the zero is present
Course Hold Loop
<latexit sha1_base64="gYxNI+u6KMP8z3VtL0BAzaktKnE=">AAAFpniclVTbbtQwEE27LJRwa+GRF4stUlGldncfAIFWqsRFPKFSddtKm21wnEnWauKksdMLVn6Gr+EV3vgbxrlUe0MISyvPzpwZnzkex0sjLlW3+3tltXWrffvO2l373v0HDx+tbzw+kkmeMRiyJEqyE49KiLiAoeIqgpM0Axp7ERx7Z+9M/PgCMskTcaiuUxjHNBQ84IwqdLkbrbfOyHbYhJMBIU4ufMi8jDLQToCb3jpzdepqAygKEu4eueELIsk2MQHeBGxysypIoeVpv0b9O71JIubvKSvQr+BK6QOQaSIkEJUQhg2jxZI4psLHnG17CdvmhP87f5E+8ZvYMi4+XkueeVQwwGRnbH84z1FOESI9CFBbDkLJkjUViUCtI3L4kYT8AuQbYhvBkxhC6mrhlqcg1wGZZlTSIM451vVJRcB03Xj6zjdQmF5lzxWrSqWzpQzLJFtyNKIr7XqFxgwzL9qLciiOmwLT9VOT0dAqN9vxkivw9YzQA9KfqTVDd54v0tsN/16VT1VdlK1KNt3Z70HyUJCUZjQGhSNPaAYEb8m/5L6aEAkmZKaezJDbbDrdLAiCiU/j1NxliZ2D1o3UcNtd73R3uuUii0avNjpWvfbdjZUDx09YHuOAsIhKOep1UzXWNFOcRYCN5IYmO6MhjNAU2Ioc65JCQZ6jxydBkuFPKFJ6pzM0jaW8jj1ExlRN5HzMOJfFRrkKXo81F2muQLDqoCCPzASbbwYOfAZMRddoUJZx5ErYBMVkRmbbEXBZv0vtpD4Uo/64fpBOajqjUQen68bum0czneNDAOcIkAr5ZBDhk8s4FSEKogfz4BhVK8yGTQQ6Lspb6M1rvmgc9Xd6L3e6X/qdvW59H2vWU+uZtWX1rFfWnvXJ2reGFmt9b/1o/Wz9am+1P7eH7eMKurpS5zyxZlb76x/60+vF</latexit>
(kp g/Vg )s + (ki g/Vg ) c (g/Vg )s

= + d
s2 + (kp g/Vg )s + (ki g/Vg ) s2 + (kp g/Vg )s + (ki g/Vg )
| {z } | {z }
Response to course command Response to disturbance
Equating coefficients to canonical TF gives:
!n2 = ki g/Vg and 2⇣ !n = kp g/Vg
or
1
!n = !n kp = 2⇣ !n Vg /g ki = !n2 Vg /g
W
Design parameters are bandwidth separation W and damping ratio ⇣
Yaw Damper
r Cr (s2 + 2⇣!n s + !n2 ) r
1
(s + psp )(s2 + 2⇣dr !ndr s + !n2 dr )

⌧r s
kr
⌧r s + 1
• The rudder is used to counter act the yaw rate caused by adverse yaw
<latexit sha1_base64="tbhPhnX4Hvp7VfEqkABSllGbF1Y=">AAADhnicfZJLb9NAEMfthkcxrxaOvaxIkbgQOZGgXJAqceFYUF9SHFXj9ThedXft7o5bGcsXPg1X+DZ8G8ZJ6BOxkuXx/Gfn8RunlVae4vh3uDa4d//Bw/VH0eMnT58939h8cejL2kk8kKUu3XEKHrWyeECKNB5XDsGkGo/S00+9fnSOzqvS7lNT4czA3KpcSSB2nWyGW1GS4lzZVhEa9Q27KOktsV+gcHWWoRPKi9pjJqgUsqwtsQskCeKIBi6EA0IhYRGSNgKyvt5Siq4luwBflDWJXOk+g4FT9EKR8CVngqt0GZiK9dLq5m85ruajQs2Lt7nDsxqtbC4r/6cE9+2VURpc3zqITDlqBE+kznn6c45XVIg5KCu2E4L6xG1HCdrsEsXJxjAexYsj7hrjlTEMVmePcX5NslLWBi1JDd5Px3FFsxYcKal7tkypAnkKc5yyacGgn7WLNXbi9QJhXjp+LImF9/qNFoz3jUk50gAV/rbWO/+lTWvKP8xaZauamN2yUF7rnkr/T/RgUBLzzhRIp7hXIQvosfMmo8TihSyNASaTVBl208msTXKW+bOfDPRw3F3Zk667eSfDHM84wBP341C3CTkFds5A2o+3gw1T6/oXD5G3hmXewvg287vG4WQ0fj+Kv0yGu/FqH+vBVvAqeBOMg51gN/gc7AUHgQy/hz/Cn+GvwfpgNHg32FmGroWrOy+DG2ew+wd1gSU/</latexit>
• The washout filter makes it so that the yaw damper only counteracts
high-frequency yaw rate
• The washout filter is similar to a dirty derivative with gain ⌧r
Lateral Autopilot - Summary
If model is known, the the design parameters are

<latexit sha1_base64="VAnHfOOS+vZG0GxnfG4XVB62JlY=">AAAFVXiclVRta9RAEE57rdbzpa1+9MvgVahgy+U+qF+U4gtYEGzFvsjlODabyWW9zW66u+lxDQH9leJf8D8ITtKjL9dT6kLIZGfm2eeZmU2YSWFdu/1zbr6xsHjj5tKt5u07d+8tr6ze37c6Nxz3uJbaHIbMohQK95xwEg8zgywNJR6EwzeV/+AYjRVafXbjDHspGygRC84cbfVX538FSgsVoXLN7RhSHaEEYWGo9Eg9BZdg/URoxUBBxgxL0REeMIPN4NhmjGPR3vQ7QpXNgPxncEUQxrCtFBr4oHUG60ZLCcwRyTxCSLSMnlBKiAOhCuEwFSdI35UFa0VQSytCmWMZ6BQHrF+ofpAloizXYAPeGaMNCAU16ihBBUxINFrB19w6sMzlhjm0sxFP0BHgOdxblmZCDcBUZYGYoC+zlaSgGaCKzpnOUPsxd2dqOXXI/pfMg37AE/HKrwm9ZioaicglYLGqetUtCNGNECeaKQAmh1Ts/i2U/13oBaLXlPmFjSAiHNK6LmIweRSRSUPDjqkHjEbvuo11LO+bmhfPHeg4htjgUY6Kj2tuI2aTDU2uWEgq7UyQ4QRhwGgaqqTxGb0pKf2VVnuzXS+4avgTo+VN1k5/de5TEGmep6ScS2Zt129nrlcw4wSXlai86g4fsgF2yVR0OWyvqMmV8Jh2oppRrJWDevdiRsFSa8dpSJEpc4md9lWbs3zd3MUveoVQGY2b4qcHxbkEp6G64hAJg9zJMRmMG0FcgSc0Q7y6uM1A4YjrNKX5oQsQYdnt9IogJndRtdoJJlt+eW53yvJyToQxHlGAdcTHoCwCZwRTAypI8XI6OKWqldWLRMRFWtZd8KdrftXY72z6zzbbu53WVmfSjyXvoffIW/d877m35b33drw9jzd2G6PGt8b3hR8LvxfpL3kaOj83yXngXVqLy38AZOXTbw==</latexit>
Inner Loop (roll attitude hold)
• !n - Error in roll when aileron just saturates

• ⇣ - Damping ratio for roll attitude loop
Outer Loop (course hold)
• W > 1 - Bandwidth separation between roll and course loops

• ⇣ - Damping ratio for course hold loop
Yaw damper (if rudder is available)
• ⌧r - cut o↵ frequency for wash-out filter
• kr - gain for yaw damper
Lateral Autopilot – In Flight Tuning
If model is not known, and autopilot must be tuned in flight, then the following
<latexit sha1_base64="oZz9dc9wBB+N9c8BEbREN4Y08hI=">AAAGF3icnVTdbhNHFN6AXaihLSGXvTlqghSkNLJ9UWglpKjcgIRUWgggZS1rduasfeTZmWFmNpE72gfpO/QduKt628u+DWc3gSTGSMBI9h6f3+/79owLpynE4fD/jStXe/2vrl3/enDj5jfffndr8/aLYGsv8VBabf2rQgTUZPAwUtT4ynkUVaHxZbF42MZfHqMPZM3zuHQ4qcTMUElSRHZNN6/+nRtLRqGJg8clVFahBgpgbISFsSdmD4RRIOpoHWl2VnWIUCDE2qACMlBqms3jHsQ5mvYLSqu1PSEzg5kgE0D4d9nWIIgIAiJVuNcWxznPCg5lCwmsV+h/GeTHwQmJabg/GpNpBrkT/j3KlBclPDYGPTyx1sGu53HclbnXCmFutbrLJQXOyCSKWNGfyL9bC3ZS3imWCl1js5gmNU25m1PTNDvwI3eVLF1A2LkY2oHaRNIMPmAEWzLsEEVBmuLyVJyOeSHkgqMlFEsYD/M7HxvpLo18zrp049z5uGhhxpOElOh4kEYIER14DK7FMMjRqHNma9T5rY7v1ZG8KOFzZWnByPUY5SdiXN+Y1jemS409Vva4ayjUkh8iIqD31q8yX0P9GSkMmlxHGHapBF8rXqp2pcWxIN1i/UQh8ijqafIXsb5zfbEAfoW5/zzK01vbw/1hd+BDY3RmbGdn5+l0c+OPXFlZVyyQ1CKEo9HQxUkSPpLULfE6IF+2hZjhEZtGVBgmqQPdwB32KL7Pnj8mQue9WJFEFcKyKjizEnEeVmOtc13sqI7l/Uki43hTjTwdVNa6laL9kwJFHmXUSzaE9MRYQc6FF5IXmzsZPJG2qvju8aVR2ByNJykvOZzajYgk9PaoObfHTXO5RmGJrzmBhZYLjzrl0ZMwMxYkPVhNrli1pn0wiTJVTfcWRquaf2i8GO+Pftof/j7ePhifvY/r2ffZD9luNsruZQfZo+xpdpjJ3lbv596vvYf9v/pv+v/0/z1NvbJxVrOVXTr9/94C86YZww==</latexit>
gains are tuned one at a time, in this specific order:
Inner Loop (roll attitude hold)

• kd - Increase kd until onset of instability, and then back o↵ by 20%
• kp - Tune kp to get acceptable step response
Outer Loop (course hold)

• kp - Tune kp to get acceptable step response
• ki - Tune ki to remove steady state error
Sideslip hold (if rudder is available)
• ⌧r - Tune ⌧r to get acceptable step response

• kr - Tune kr to remove steady state error
Longitudinal Flight Regimes
hc
<latexit sha1_base64="kFjzyuELaTJYJfIYt4PU25WYhWA=">AAADX3icbVLLbtQwFHVngJYByhRWiE3ECIlFNZqMEGVTUQkJsSyPPqQkVI5zk1jjR7Ad0MjkE9iC4E/4FJb8CTczVUtSrhT55J57j32PnVaCWzeb/d4YDK9dv7G5dXN06/ad7bvjnXvHVteGwRHTQpvTlFoQXMGR407AaWWAylTASbp42fInn8BYrtV7t6wgkbRQPOeMOky9Kz+ws/FkNp2tIrgKwnMwefHrRxs/D892Bq/iTLNagnJMUGujcFa5xFPjOBPQjOLaQkXZghYQCXAOTEUrMLuBpKbgan8+3eMq8QVoCc4suw0IFZVgE78argkeYyYLcm3wUy5YZf/t8FRau5QpVkrqStvn2uT/uKh2+fPEc1XVDhRbb5TXInA6aJ0KMm6AObFEQJnhOFzASmoow4FQScFnpqWkKvNxlUETzRMf50jjb2sFFZOwucTzpun2ZJDDRyywDs9jQPjYGU5VgQ76/X6xRJubdsEhci/7tMGeNZum/m3Tp5U2solCPJ+A3MVfJmFseFEiarpuiULjoKXkbPcC97Roai+lLpS6QpHSChJfLqsSL9N1N0m1XrhWZYQvL+y/s6vgeD4Nn03nb2aTg6dkHVvkIXlEnpCQ7JED8pockiPCSEG+km/k++DPcHO4PRyvSwcb5z33SSeGD/4C5NEj0A==</latexit>
h + hhold
<latexit sha1_base64="+ZR8cVay8PCP3sYlI0FsAWlwlKs=">AAADb3icbVLbbhMxEHUTLiFcmpQHHpDQqhESiCrKRojyUqkSCPFYEGkr7a4ir3d214ovi+2lRMafwmv5Cv6AH+Az+AO8SdWyKSNZPp4zc+wZT1oxqs1k8nur071x89bt3p3+3Xv3H2wPhjvHWtaKwIxIJtVpijUwKmBmqGFwWinAPGVwki7eNPzJF1CaSvHJLCtIOC4EzSnBxrvmg2H5opzb2MBXY0vJMufmg9FkPFlZcB2EF2B02Pv1c+ft+e7RfNh5F2eS1ByEIQxrHYWTyiQWK0MJA9ePaw0VJgtcQMTAGFAVrkDtBRyrgoqD6XifisQWIDkYtWwneCgwB53YVbEueOo9WZBL5Zcwwcr7b4bFXOslT30kx6bUm1zj/B8X1SZ/nVgqqtqAIOuL8poFRgZN54KMKiCGLT3ARFFfXEBKrDDxBXklAWdEco5FZuMqAxdNExvnnvbHphWYjUJ3hafOtXMyyOGzD9DGv0cB85+iKBaF76A92Azmvs2u2XwRueWbtPI5azZN7Ue3SQupuItC/z4GuYm/jcJY0aL0yLW7xQrpCy05JXuXeEMLp/pK6lKpLRQJKSCx5bIq/Wea9iWplAvTqPT95IWbc3YdHE/H4avx9IMfwZdobT30GO2iZyhE++gQvUdHaIYIOkPf0Tn60fnTfdR90g3WoZ2ti5yHqGXd538B3Ywo1g==</latexit>
hc = sat(hc , h ± hhold )
<latexit sha1_base64="vUatTVZK1EIrl6+QTwaaaBEmqVY=">AAACWXicbVHLattAFB2paeuqL6fOrpuhwZBCMZIpTTeFQDZdJlAnAcs1o9GVNWQeYuYqqRH6i35Nt81PlPxMxlIWzePAwOGce7n3nskqKRzG8b8gfLL19NnzwYvo5avXb94Ot9+dOFNbDjNupLFnGXMghYYZCpRwVllgKpNwmp0fbvzTC7BOGP0D1xUsFFtpUQjO0EvL4aT8yek3miL8wsYxbPe88ImWNK0ULZdNb5RG5m37MYqWw914EnegD0lyS3YPdooOR8vtYJTmhtcKNHLJnJsncYWLhlkUXEIbpbWDivFztoK5p5opcIumO6ylY6/ktDDWP420U//vaJhybq0yX6kYlu6+txEf8+Y1Fl8XjdBVjaB5P6ioJUVDNynRXFjgKNeeMG6F35XyklnG0Wd5Z8rmj4ReuZbScbcpNzlE49SChktulGI671O8FLnfppkK3foYk/uhPSQn00nyZTI99nl+Jj0G5D35QPZIQvbJAflOjsiMcPKb/CF/yVVwHQbhIIz60jC47RmROwhHN5Fftxw=</latexit>
h
<latexit sha1_base64="qvoIRGJlJsQK08IyTSJun/z7MQ0=">AAADXXicbVJNb9QwEHV3KZSllBYOHLhErJA4VKvNClEuFZWQEMcWsW2lJKocZ5JY649gT0Ark1/AFQ7wT/gpnPgrOLtVS1JGivwyb+bZ8+y0EtzidPp7YzC8tXn7ztbd0b3t+zsPdvcenlpdGwZzpoU25ym1ILiCOXIUcF4ZoDIVcJYu3rT82Scwlmv1AZcVJJIWiuecUfSpk/JidzydTFcR3AThJRi//vWjjZ/HF3uDt3GmWS1BIRPU2iicVpg4apAzAc0ori1UlC1oAZEARDAVrcDsB5KagqvD2eSAq8QVoCWgWXYbPFRUgk3carQmeOYzWZBr4z+FwSr7b4ej0tqlTH2lpFjaPtcm/8dFNeavEsdVVSMott4or0WAOmh9CjJugKFYekCZ4X64gJXUUOYH8koKPjMtJVWZi6sMmmiWuDj3tP9traBiHDbXeNY03Z4McvjoCyz68xgQLkbDqSq8g+6wXyy9zU27+CFyJ/u08T1rNk3d+6ZPK21kE4X+fAJyjL+Mw9jwovSo6bolCu0HLSVn+1e4p0VTey11pdQVipRWkLhyWZX+MrG7Sar1AluVkX95Yf+d3QSns0n4cjI7mY6PXpB1bJEn5Cl5TkJyQI7IO3JM5oQRIF/JN/J98Ge4Odwe7qxLBxuXPY9IJ4aP/wJZGCL7</latexit>
• Regulate altitude by commanding pitch

<latexit sha1_base64="IAvJXdgm2In7P5cPoYiFngj1NOw=">AAACu3icbVLLbhMxFPUMrzY8mtIlLCyiIDZEM0ECNpUqsWFZKtJWykSRx76ZmPoxsq9b0lH+gd/jR1jjSUeCpr2SpaP7OPfoXJe1kh6z7HeSPnj46PGTnd3e02fPX+z191+eehschwm3yrrzknlQ0sAEJSo4rx0wXSo4Ky++tPWzS3BeWvMdVzXMNKuMXEjOMKbm/V9FCZU0jUTQ8hrWPUqLFtMTqIJiCJSpSBsE0HJFudWaGSFNRWuJfNl2X/qacWjeZ6P8gzT3EUjnawCxRYBLZzHq7RVgxL/98/4gG2WboHdB3oEB6eJ4vp8cFMLyoMEgV8z7aZ7VOGuYQ8lVZCyChyjxglUwjdAwDX7WbKxb02HMCLqwLj6DdJP9f6Jh2vuVLmOnZrj027U2eV9tGnDxedZIUwcEw28WLYKiaGl7ByqkA45qFQHjTkatlC+ZYxzjtW5taX9BNMyvKR1ulHIroDcsHBi46gxtCoSfeCVFVNOM4xmijfm2aXfB6XiUfxyNv40HR1ln6A55Rd6QdyQnn8gR+UqOyYRw8id5nQyTt+lhytMfqbppTZNu5oDcijT8BS2N2Eg=</latexit>
• Regulate airspeed by commanding throttle

h hhold
<latexit sha1_base64="JYBw0xI04C1uQryN7Ha/mHLujqg=">AAADb3icbVLbbhMxEHUTLiFcmpQHHpDQqhESSCXKRojyUqkSCPFYEGkr7a4ir3d214ovi+2lRMafwmv5Cv6AH+Az+AO8SdWyKSNZPp4zc+wZT1oxqs1k8nur071x89bt3p3+3Xv3H2wPhjvHWtaKwIxIJtVpijUwKmBmqGFwWinAPGVwki7eNPzJF1CaSvHJLCtIOC4EzSnBxrvmg2H5opzb2MBXY0vJMufmg9FkPFlZcB2EF2B02Pv1c+ft+e7RfNh5F2eS1ByEIQxrHYWTyiQWK0MJA9ePaw0VJgtcQMTAGFAVrkDtBRyrgoqD6XifisQWIDkYtWwneCgwB53YVbEueOo9WZBL5Zcwwcr7b4bFXOslT30kx6bUm1zj/B8X1SZ/nVgqqtqAIOuL8poFRgZN54KMKiCGLT3ARFFfXEBKrDDxBXklAWdEco5FZuMqAxdNExvnnvbHphWYjUJ3hafOtXMyyOGzD9DGv0cB85+iKBaF76A92Azmvs2u2XwRueWbtPI5azZN7Ue3SQupuItC/z4GuYm/jcJY0aL0yLW7xQrpCy05JXuXeEMLp/pK6lKpLRQJKSCx5bIq/Wea9iWplAvTqPT95IWbc3YdHE/H4avx9IMfwZdobT30GO2iZyhE++gQvUdHaIYIOkPf0Tn60fnTfdR90g3WoZ2ti5yHqGXd538B45wo2A==</latexit>
htake o↵
<latexit sha1_base64="nFPxHNzDdA+Iqa3TuKX2quCAjTc=">AAADcXicbVLNbtQwEHZ3+SnL35YeEOISdYWERLXarBDlUqkSEoJbQWxbKYkWx5kk1von2A4QGT8LV3gCnoUX4MiFF8DpVi1JGcny5/lmPnvGk1aMajOb/dwYDK9cvXZ988bo5q3bd+6Ot+4daVkrAgsimVQnKdbAqICFoYbBSaUA85TBcbp60fLHH0FpKsU701SQcFwImlOCjXctx9vl0sYGPhtr8AoCmefOLceT2XR2asFlEJ6BycH9179+/xi8P1xuDV7GmSQ1B2EIw1pH4awyicXKUMLAjeJaQ4XJChcQMTAGVIUrULsBx6qgYn8+3aMisQVIDkY13QQPBeagE3targseeU8W5FL5JUxw6v03w2KudcNTH8mxKXWfa53/46La5M8TS0VVGxBkfVFes8DIoO1dkFEFxLDGA0wU9cUFpMQKE1+QVxLwiUjOschsXGXgonli49zT/ti2ArNJ6C7w3LluTgY5fPAB2vj3KGD+YxTFovAdtPv9YO7b7NrNF5Fb3qeVz1mzaWrfuj4tpOIuCv37GOQm/jIJY0WL0iPX7RYrpC+05JTsnuOeFk71hdS5UlcoElJAYsumKv1nmu4lqZQr06qM/OSF/Tm7DI7m0/DZdP7Gj+BTtLZN9BDtoMcoRHvoAL1Ch2iBCGrQV/QNfR/8GT4YBsOddehg4yxnG3Vs+OQvQvQqTg==</latexit>
• Regulate pitch to a fixed ✓c

<latexit sha1_base64="n7KAZoiZRp4E13DtN6B4b4l9N5Y=">AAACn3icbZHbbtNAEIbXBkrrQknpZW8W0kjcENlBAi4rIQFXVYpID4pDtF6P7VX3YO2O2wYrDwpPwzqN1BMjrfRrDju/vslqKRzG8Z8gfPL02cbzza1o+8XLnVe93dcnzjSWw4QbaexZxhxIoWGCAiWc1RaYyiScZhdfuvrpJVgnjP6JixpmipVaFIIz9Kl5D9MMSqFbgaDEb1hGlKadpj+gbCRDoLVAXlE0lNFCXENOD1KsANkvftA1X7qacWjfx8Pkg9C3818bKSlW1qD3FKWg89sd814/HsaroI9FshZ9so7xfDfYS3PDGwUauWTOTZO4xlnLLAou/Y9p48D7uGAlTL3UTIGbtSs8SzrwmZwWxvqnka6ydydappxbqMx3KoaVe1jrkv+rTRssPs9aoesGQfObRUUjO1gda5oLCxzlwgvGrfBeKa+YZRz9Re5t6S4tdOmWlA5WTrnJIRqkFjRccaMU8/xShGu8Erl30448a48xeQjtsTgZDZOPw9HxqH8Yr4Fukn3ylrwjCflEDsl3MiYTwsnfgARbQRS+Cb+FR+H4pjUM1jN75F6E5/8A4EPLgA==</latexit>
Take-off zone
• Full throttle
Altitude Hold Using Commanded Pitch
Pitch Attitude Hold
max
e
Altitude Hold Using Commanded Pitch
Provided pitch loop functions as intended, we can simplify the inner-loop

<latexit sha1_base64="iuq08rOzPjnu+9KVPguND2vZlzk=">AAAC/HicbVHLbtNAFB2bVzGvFJZIaESK6AJSOwtgg1SpLJDYBETaSnGIJjPXyajzMDPjtpFlPoJvYIfY8i/8CUuuk0jQlCtZPj7nXPnec6elkj6k6a8ovnL12vUbWzeTW7fv3L3X2b5/6G3lOAy5VdYdT5kHJQ0MgwwKjksHTE8VHE1PDlr96BScl9Z8DIsSxprNjCwkZwGpSedrbqw0AkxIBs6eSgGCljLwOVXWlrSoDG+NnjJPpQmAVvGMngHlzFAvNc5YLGiYQyKNAfd82ZXnVCwM05J7GizdyVEP7BPfWwGas7J09py+m9QrZpIHOA/1m4Om2UkmnW7aS5dFL4NsDbpkXYPJdvQhF5ZXGrfgink/ytIyjGvmguQKmiSvPJSMn7AZjBDiZODH9TK8hj5BRtDCOnxMoEv2346aae8XeopOzcLcb2ot+T9tVIXi1biWpqwwNr76UVGpNpH2ElRIBzyoBQLGncRZKZ8zx3jAeyW5gTNutWZG1HkpoBn1x3VeoIyf7WZMdbPmL+43zcUeAQV8RoMPOI8DhVE7ycwMA6lfb5o1pta0L1yiqDXKeIVsM/PL4LDfy1700vf97n5/fY8t8pA8JrskIy/JPnlLBmRIOPkdPYqeRrvxl/hb/D3+sbLG0brnAblQ8c8/obz1jA==</latexit>
dynamics to ✓c /✓ ⇡ K✓DC
Altitude from Pitch – Simplified
<latexit sha1_base64="m8fINTT8tSSuEqS7b0wrCN5iaKk=">AAAEKnicrVLNbtNAEHYTfor5a+HIZUUKahUROTkAl0pF5VAEh4JIWilOrc16HC9Zr83uuCWs/Ea8A+/AreLKgzBOA/SHcmIuHs98szPzzTculLQYBMdLjeaVq9euL9/wb966fefuyuq9gc1LI6AvcpWb/TG3oKSGPkpUsF8Y4NlYwd54ul3n9w7BWJnr9zgrYJTxiZaJFBwpFK0ufQ11LnUMGv1w6O+s2w226YcKElwPE8OFex25EFNAHoUIn9C93K6qQcSnkSuitLLtS/KMAJIAlbMHPb/N/gGbv8Muhfi/XgqNnKS4wdIDUY/ZZqfHtPNGrO3/x0ZxlFIjPxz5fnhoCy7ABZ2u1BX9/2btBdt9xUSu0eSK5ZpxRWcoY2CgbWnAMkw5srV0jSHNObU11iLXdexArLEjiSn7DCZnFoHHsyeURKo2JjeM65gZ+AACT9XFJIvSjLkWYP1opRV0grmxi0534bS8he3Syd+FcS7KjIYXils77AYFjhw3KIWCyg9LC7TrlE9gSK7mGdiRm0utYo8oErOERktoZTaPnq5wPLN2lo0JmXFM7flcHfxbblhi8nzkpC5KBC1OGiWlYpizWre0tCEW1IwcLoykWZlIOTGKpG46BxyJPMuILxcWMVTD3sidCCMs6s24anWrP36vqs7WxJDARwIQw2JqQJFAjOR6QoS4zfPgjFir6g8tkbiM0nSF7nnOLzqDXqf7tBO87bW2gsU9lr0H3kNv3et6z7wtb8fb9fqeaDxuvGn0G4Pml+a35nHz+wm0sbSoue+dseaPn70QY24=</latexit>
✓ ◆ ✓ ◆
K✓DC Va kph s + K✓DC Va kih s
H(s) = hc (s)+ dh (s)
s2 + K✓DC Va kph s + K✓DC Va kih s2 + K✓DC Va kph s + K✓DC Va kih
A PI control on altitude ensures that h tracks constant hc with zero steady-state

error and rejects constant disturbances
Altitude from Pitch Gain Calculations
Equating the transfer functions

<latexit sha1_base64="f/yy0lG1DV20Cs+UleHbFPLGDTs=">AAAFgnicjVTbbtNAEHWbBoq5tfDAAy8rUqSWSiUxEvBApUotUiVeCqIXKU6t9Xpsr2qvXe+6F1bL1/AK/8PfML60TVKC2BevZs7MnHN2Ej9PuFT9/u+5+c5C987dxXv2/QcPHz1eWn5yILOyYLDPsiQrjnwqIeEC9hVXCRzlBdDUT+DQP9mu8odnUEieia/qModRSiPBQ86owpC33HnmioyLAISyP56WGBURUTEQVVAhQyhIWApWYaXtDu1dT8ev42NmVuXapu2GBWX6k6ddrFDUcxVcKL2zbcyBR088nXuxcRMI1apcb7AY5Bg0us0at+BRrNaMlseOvU5m9CItXJKZEPuqNXFRRkCaJBXBVWDXY0iabJKGieN+wx7EzVKIqKeFIdjcJjfnJnPs1PRwdlP0nzW2O7Ijfgay9pNlEKLtHI2unXT97AKCxhBd0b4idt0DxVSTZ+ithTZK689Yw3yyoaPdek+0n5RgagU1YHzQP8ZUMs5jKID46OY5D1RMJOS0qDeIcEkoizmcQUD8S8wkwKolqjWOTbimMzB6gtChN8GlZdHMtW+WcwckjwSp5qagcKMJnUVpov0K9l8xBHEkoGlerXcNm0K1trTDV4ztLfX6G/36kNuXQXvpWe3Z85bnvrhBxsoU2bKESjkc9HM10rRQnCWAesqKIzuhEQzxKlCHHOmahCEvMRKQMMPfWyYUqaPjFZqmUl6mPiJTqmI5nauCf8sNSxW+H2ku8lKBYM2gsEyIykj1f0ACXuCDJZd4oazgyJWwGJ1klcfoP5yzLE3RPu3mAZihM9LtnuaVMpr08EGv7071buM1AYRwigCpkE8BCTpccCoiNERvToNTdM1UHxQR6tTUrzCY9vz25cDZGLzd6H92eltO+x6L1nPrhbVqDax31pa1a+1Z+xbrfO/86Pzs/OoudF91B903DXR+rq15ak2c7oc/KpLjYA==</latexit>
⇣ ⌘
k ih
K✓DC Va kph s + kp 2⇣!n s + !n2
h
Hh/hc (s) = 2 and Hc (s) = 2
s + K✓DC Va kph s + K✓DC Va kih s + 2⇣!n s + !n2
gives the coefficients
!n2 h 2⇣h !nh

k ih = kph =
K✓DC Va K✓DC Va
where bandwidth separation is achieved by selecting

1
! nh = !n
Wh ✓
Design parameters are bandwidth separation Wh and damping ratio ⇣✓
Airspeed Hold Using Throttle
kiV
s
V̄ac ¯t V̄a
kpV
<latexit sha1_base64="VGVxrD0haPQSdhNy8w8+uKd0GWw=">AAAD3HichVJLb9NAEHYSoMW8WjhyGZEipYoa2TkAl0pFXOBWEE0rxYm1WY+TJetdd3fdKli+cUNc+W3wZxDjJFL6QGIly59mvnl9M5NcCuuC4Hej2bpz997W9n3/wcNHj5/s7D4dWF0YjidcS23OJsyiFApPnHASz3KDLJtIPJ3M39X+0ws0Vmj12S1yHGVsqkQqOHNkincb59HQH8QMDv1IYuo6UWoYL1lcDuJ+1ZnHZR4PKtslIAjsV6Ud97udpT+sumvemrZv/S5sTHVABZER05nbpxpjDt1rVex/ssGtbP46GyTxwI9Gvh9d2JxxLINeKBS5lRYqQeX8t3D8AbhWzmgJWoGbIX1GO5LowOkDJozNEROQWueAyhYGLTGYgz3qdQ8cdTi3wOok1jG1so/5HlwKN4OvaDRYhyxZHJDbIaAx2gBTCRj8gtzZTWRCiyzMhCmO1o932kEvWD64DcI1aHvrd0xL+hQlmhcZzcUls3YYBrkblcw4wSXS1IVFUmHOpjgkqFiGdlQuj6OCl2RJIKXWUlIDltarESXLrF1kE2JmzM3sTV9t/JdvWLj0zagUKi8cKr4qlBYSnIb60mhoQyrIBQHGjaBegc8YqeroHmlTeMl1lpFeZZQnWA37o3J1FlFeT8ZkO6w2uF8v/2pMgimeE4EU5nODsoycEUxNSZDy8CY5I9Wq+kdDpGVGbtpCeFPz22DQ74WvesHHfvsoWO9j23vuvfA6Xui99o68996xd+Lxxq/Gn+ZWc7s1bn1rfW/9WFGbjXXMM+/aa/38C+2wQr8=</latexit>
✓ ◆ ✓ ◆
aV2 (kpV s + kiV ) s
Va = Vac + dV
s2 + (aV1 + aV2 kpV )s + aV2 kiV s2 + (aV1 + aV2 kpV )s + aV2 kiV
A PI control on the throttle-to-airspeed loop ensures that Va tracks a constant

Vac with zero steady-state error and rejects constant disturbances
Airspeed from Throttle Gain Calculations
Equating the transfer functions and their coefficients

<latexit sha1_base64="LgiDGkKDd2INcyFcfIkyR8h9HGQ=">AAAE6XicjVPNbtNAEHabAMXlp+XnxGVFipQoIsQ+AJdKlQCpx4JoipRNo8167Kxir53ddSGslnfghrjyVDwA78Guk5KfIsRKlscz3+zM9814VKRMqm7359Z2rX7t+o2dm/7urdt37u7t3+vJvBQUTmme5uLDiEhIGYdTxVQKHwoBJBulcDaavHLxswsQkuX8vZoVMMhIwlnMKFHWNdzf/oJ5zngEXPlvpqX18gSpMSAlCJcxCBSXnDqsRIRHLsQEojnE9g5ms6SP+/7xUPeG5Jl9zqlpyhY6RDiFWDVxLAjVxIVDMxnqYtgzsr38Zva7ZbQ8D9vNyhuY9jq6Jf02Wk8wCAuWjFUL+bZOVSHEn0ERhPMMEgvmBknU9tHyLCPnYVUPtdE86T9zfDzwE3YBslInIYxXzPEo/wSRrjrTrrXLjjSuhqNHaQnmz00VpmphQcmRmVrdo8Vr5cJi/cJw/UbXegX4V6GnC01XyjkePr6QBaGgu52Aceu6XAH0GiRLOCqIIBkouzeICECcqFKQFMUCpiVwOkNrRQ+WVXvmwFR7EpGscKsk3J5twi97PzD+cK/R7XSrg64awcJoeItzMtzfeoejnJaZbZemRMp+0C3UQBOhGE3BkiklWHITkkDfmtwSkQNd1TfoifVEKM7tWueWbuVdzdAkk3KWjSwyI2osN2PO+bdYv1Txy4FmvCiVFWheKC5TpHLkfjsUMQFUpTNrECqY7RXRsVWZOpHtAOAjzbPMKqdxEYHphwM9HzsuHDOSNgKztEM3x9WcCGKYWoBUth8BqcZKMMITK4g+3ARnVjXjXpZErDNTTSHY1Pyq0Qs7wfNO923YOAoX89jxHnmPvaYXeC+8I+/YO/FOPbr9q7Zbe1B7WJ/Uv9a/1b/Podtbi5z73tqp//gNFyOngA==</latexit>
✓ ◆
a V 2 k p V s + a V 2 k iV ) 2⇣!n s + !n2
HVa /Vac (s) = = 2
s2 + (aV1 + aV2 kpV )s + aV2 kiV s + 2⇣!n s + !n2
gives the gains
! nV 2 2⇣V !nV aV1

k iV = kpV =
aV2 aV2
Design parameters are natural frequency !nV and damping ratio ⇣V
The control signal is
t = ⇤
t + ¯t
⇤ k iV
= t + kpV (Vac Va ) + (Vac Va )
s
Python Implementation
class autopilot :
<latexit sha1_base64="nedzH/NyEbUhV9GuUN+sQjJRbZw=">AAAH7niclVVbixs3FJ5NGzt1L8k2j30RMQteMjH2FtpSCKQESt6yLdlLWZtB1pzxiNVIqnQmrmv8N/pW+trfkX/Rf9MzV1/WvUQwIJ3Ldz59R9LMrJIeR6O/ju598OH9TvfBR72PP/n0s4ePjj+/9CZ3Ai6EUcZdz7gHJTVcoEQF19YBz2YKrma3Lwv/1VtwXhr9BpcWphmfa5lIwZFM0fH9d5MZzKVeKY9FOann6xvF9Tznc3h+vsTU6GlPKO494zkaK5XBb3uMRgwJiyKpJUbRgBgkIUMfCaPRGXVaxRSjcA3JpKLEmSziUoEzmj1nNn5ZBV9JTH/kCINbG7LbOGRKZhJPdxEEbdlDhVGgFQCyBqgSZcje+MPJS76IYp5ZcJSGjmufgPs+16JQYaDzLKTt6CJ/L9FKFGlVFBS85WjcezPnihqTxzX3EvG9yHPpvAWIq3xMnUHq839BtD3KbVwwrDokMiLpkQxbHeozRQbH1abFrU+kMhJUa+G4HVB20wdhsozrBmxIYRvWts4p2XuOuSsItO4SlpCKsIT2lRi34C5mTw83elhvoGSyXS/cQXym7dDxWFJvB1+OTkO2u96wi0Ehj3jD787JbOrZ7Xq0aKf7UK6B2pyyBqPKcHU3aq2NnhfnQeqDgvdZoxjDFFhzeFgteBuXHpC4ULU9bW2HdmTaGRW9lD1j351vEn81GsLW9/SOb7N/Ikj7b3kcOOiNEOlGynRfP2jyD1y2Jr+u1GCUy2bx8z4gtoQO3pz2QBVqNRH/Q61/GxWTS/6PVNoe1faQjYajkI2Ho63D4YCCdJUaNreh5EUMyxK93gR0vP1a93rRoz5hlYPdnYzrST+ox3l0fPR4EhuRZ6CxfNpvxiOL0xV3KIWCdW+Se7Bc3NI/4Iammmfgp6vyb7NmJ2SJGV1a+jSy0rqdseKZ98tsRpEZx9Tv+wrjId9Njsk305XUNkfQoiqU5IqhYcWvi8XSgUC1pAkXThJXep2444Lert0qtTZ+zdhJyVSYGHonEwcaFrWiqwnCL7iQMbFZnUm9JhnH+6LdnVyeDcdfDc9+OOu/GNSCPgi+CJ4Eg2AcfB28CF4F58FFIDqnnded685PXdv9rft7948q9N5RnfM42BndP/8G2/h2Qg==</latexit>
def init ( self , ts control ):

s e l f . r o l l f r o m a i l e r o n = pdControlWithRate ( kp , kd , l i m i t )
s e l f . c o u r s e f r o m r o l l = p i C o n t r o l ( kp , ki , Ts , l i m i t )
s e l f . yaw damper = t r a n s f e r F u n c t i o n (num , den , Ts )
s e l f . p i t c h f r o m e l e v a t o r = pdControlWithRate ( kp , kd , l i m i t )
s e l f . a l t i t u d e f r o m p i t c h = p i C o n t r o l ( kp , ki , Ts , l i m i t )
s e l f . a i r s p e e d f r o m t h r o t t l e = p i C o n t r o l ( kp , ki , Ts , l i m i t )
def update ( s e l f , cmd , s t a t e ) :

# lateral autopilot
c h i c = wrap (cmd . course command , s t a t e . c h i )
phi c = s e l f . saturate (
cmd . p h i f e e d f o r w a r d + s e l f . c o u r s e f r o m r o l l . update ( c h i c , s t a t e . c h i ) ,
np . r a d i a n s ( 3 0 ) , np . r a d i a n s ( 3 0 ) )
d e l t a a = s e l f . r o l l f r o m a i l e r o n . update ( p h i c , s t a t e . phi , s t a t e . p )
d e l t a r = s e l f . yaw damper . update ( s t a t e . r )
# longitudinal autopilot
# s a t u r a t e t h e a l t i t u d e command
h c = s e l f . s a t u r a t e (cmd . altitude command ,
state .h AP. a l t i t u d e z o n e , s t a t e . h + AP. a l t i t u d e z o n e )
t h e t a c = s e l f . a l t i t u d e f r o m p i t c h . update ( h c , s t a t e . h )
d e l t a e = s e l f . p i t c h f r o m e l e v a t o r . update ( t h e t a c , s t a t e . t h e t a , s t a t e . q )
d e l t a t = s e l f . a i r s p e e d f r o m t h r o t t l e . update (cmd . airspeed command ,
s t a t e . Va)
delta t = s e l f . saturate ( delta t , 0.0 , 1.0)
return d e l t a , s e l f . commanded state
Longitudinal Autopilot - Summary
If model is known, the the design parameters are
Inner Loop (pitch attitude hold)
• emax
✓ - Error in pitch when elevator just saturates.
• ⇣✓ - Damping ratio for pitch attitude loop.
Altitude Hold Outer Loop

• Wh > 1 - Bandwidth separation between pitch and altitude loops.
• ⇣h - Damping ratio for altitude hold loop.
Airspeed Hold Outer Loop

• WV2 > 1 - Bandwidth separation between pitch and airspeed loops.
• ⇣V2 - Damping ratio for airspeed hold loop.
Throttle hold (inner loop)
• !nV - Natural frequency for throttle loop.

• ⇣V - Damping ratio for throttle loop.
Longitudinal Autopilot – In Flight Tuning
If model is not known, and autopilot must be tuned in flight, then the
following gains are tuned one at a time, in this specific order:
Inner Loop (pitch attitude hold)
• kd✓ - Increase kd✓ until onset of instability, and then back o↵ by 20%.
• kp✓ - Tune kp✓ to get acceptable step response.

Altitude Hold Outer Loop
• kph - Tune kph to get acceptable step response.
• kih - Tune kih to remove steady state error.
Airspeed Hold Outer Loop

• kpV2 - Tune kpV2 to get acceptable step response.
• kiV2 - Tune kiV2 to remove steady state error.
Throttle hold (inner loop)

• kpV - Tune kpV to get acceptable step response.
• kiV - Tune kiV to remove steady state error.
PID Loop Implementation
PID continuous time
Taking Laplace transform…
Use bandwidth-limited differentiator to reduce noise
PID Loop Implementation
Tustin’s rule or trapezoidal rule
Integrator term
Differentiator term
Integrator Anti-wind-up
control before anti-wind-up update
control after anti-wind-up update
is anti-wind-up update
subtracting top two equations
Let value of control after saturation is applied
solving…
2τ − Ts 2
D[n] = D[n − 1] + (E [n] − E [n − 1]) . (6.30)
2τ + Ts 2τ + Ts
PID Implementation
Matlab code that implements a general PID loop is shown below. update with Python
1 function u = pidloop(y_c, y, flag, kp, ki, kd, limit, Ts, tau)

2 persistent integrator;
3 persistent differentiator;
4 persistent error_d1;
5 if flag==1, % reset (initialize) persistent variables
6 % when flag==1
7 integrator = 0;
8 differentiator = 0;
9 error_d1 = 0; % _d1 means delayed by one time step
10 end
11 error = y_c − y; % compute the current error
12 integrator = integrator + (Ts/2)*(error + error_d1);
13 % update integrator
14 differentiator = (2*tau−Ts)/(2*tau+Ts)*differentiator...
15 + 2/(2*tau+Ts)*(error − error_d1);
16 % update differentiator
17 error_d1 = error; % update the error for next time through
18 % the loop
19 u = sat(... % implement PID control
20 kp * error +... % proportional term
21 ki * integrator +... % integral term
22 kd * differentiator,... % derivative term
23 limit... % ensure abs(u)<=limit
24 );
25 % implement integrator anti−windup
26 if ki~=0
27 u_unsat = kp*error + ki*integrator + kd*differentiator;
28 integrator = integrator + Ts/ki * (u − u_unsat);
29 end
30
31 function out = sat(in, limit)

32 if in > limit, out = limit;
33 elseif in < −limit; out = −limit;
34 else out = in;
35 end
Yaw Damper Implementation
<latexit sha1_base64="Ma97JJ6cBBX8m2/UpDxcJM2oIFQ=">AAAF7XicfVRdb9s2FFWaeOu0jzbrS4u9XCze5ix1IPlhLQYEKDZg6GM3xG0HyxMo6somTJEKSTWwBb30P+xt2Ot+037C/sUuZddOk2wEbF/eD+nwnENnlRTWRdHfe3f2D3offHj3o/DjTz797N79w89fWl0bjmOupTavM2ZRCoVjJ5zE15VBVmYSX2WLH3391Rs0Vmh17pYVTks2U6IQnDlKpYf7bxOlhcpRufAnbYDBDBUaJh9DIYx1Q21yNMC1ckLVurZDJ0oEZ5iyBRWKWnH/JNAFuDlCoU0ZJpMQaD0f2GM4g6QwjDdj2rTNr/6bcotNVqUxWDgBlUZtk282OW3CZBpeIlwy5cBpyBAYHcmHnEleS+Yw716IF7V4wyThh1xYbtDhf0DcwVpdhbXqYK06WOtclkaEIiM0q9+aYdy2TUx7ttt7cEVHlq2QezbBsrIifIZwQf88tf1TGFuhZnBeW2LuGwslurnOYUC4KlxpkTMJppZ4DNkSkgRsnVGnq52fCvsWEiNmc8eM0ZcbYKO2oUe3icTCDdapeLjFeLKJ1nPH/cdABHKmdox1hHGNBSEWxJj1qvU9H31gNkwynAnVMClm6tu248pT8Y6WBQxG4AU7AULhJSPmRpBvM143SC4uapZ37H19BsNro8P/HU26d7Kbo/l2NL91NExQ5VvgPyzBsYWn0Z9XKO9/UmXV33iCLLrjRvuyA7pPW//c4m7yHTW/b7BcFNSDincm7O7T9+88VqcL4m3oj1KnzYJk6RwVwZIKa28t13nyUnr/KDqNugU3g3gTHAWb9SI93PslyTWvS9KQS2btJI4qN22YcYJLpGfWFivGF2yGEwoVK9FOm+7fooWvKJP7i0ofujVd9upEw0prl2VGnSVzc3u95pO31Sa1K55OG6Gq2hEr6xcVtfTk+b8eIswgd3JJAeNGEFbgc0YiOxIoTBRecl2WjJRMqhzbyWjarD2QVP5kTB7F7S4e+Xt4dSbHAi+owTrCY1A2iTOCqRkR0pxdby6Jtdb/0CGKpqQyqRBf5/xm8HJ0Gn93Gv08OnoWbfS4G3wRfBkMgjh4EjwLngcvgnHA9/85uHfw8OBRT/d+7/3R+3PdemdvM/MgeG/1/voXaYvnLA==</latexit>
For a general, first-order continuous-time transfer function of the form
U (s) n1 s + n0
H(s) = =k
Y (s) d1 s + d0
we want to be able to calculated the equivalent discrete-time transfer function

1
U (z) b 0 + b1 z
H(z) = = 1
Y (z) 1 + a1 z
for a specific sample⇣rate Ts⌘. Using Tustin’s method (trapezoidal rule) by

1
substituting s ! T2s 11+zz 1 , we can calculate the coefficients of H(z) as
k(2n1 + Ts n0 ) k(2n1 Ts n0 )
b0 = b1 =
2d1 + Ts d0 2d1 + Ts d0
k(2d1 Ts d0 )
a1 =
2d1 + Ts d0
By taking the inverse z transform, we can convert our discrete transfer function
to an discrete-time di↵erence equation:
uk = a 1 uk 1 + b0 y k + b1 y k 1
Yaw Damper Implementation
r Cr (s2 + 2⇣!n s + !n2 ) r Bode Diagram
1
(s + psp )(s2 + 2⇣dr !ndr s + !n2 dr )

10 -1
Magnitude (abs)
⌧r s
kr
⌧r s + 1
10 -2
For our yaw damper,
<latexit sha1_base64="Lc8yBZUoKDw1gbzPmx2MO+WtwTE=">AAAFqXicbVRfb+NEEHcvBI7w7wqPvIxo0PXUNMQRPehD0R1I6B5A6qH0riIO1mY9jlexd32764R05Y/Dp+EVHvg2zDpOr9d2Zdnj+bfz+83szstcGDsa/bf3oPNe9/0PHn7Y++jjTz797NH+56+MqjTHC65ypS/nzGAuJF5YYXO8LDWyYp7j6/nyJ29/vUJthJITuylxVrCFFKngzJIq3u/8EEklZILS9n5WGigvbNgaElaUqAe9aNoDWi8OzRM4gyjVjLsowdyyWJOuds2bTMtYQ5Rjag9bJ8sqUpn6WoIjCOtIi0Vmn9wTQJ7mqIzdWtU7rwH0ollvnaFG6Hv/MxgNx/0B9Ld+zf+3J33QLPnGDIDJBPqT2DT6UdgH0+tFK1Myjm40DMcnQtakuQY8yYSBhVihAQaJMFyjxWMrCgSrmTQpakgryT1VoFLYsUF0XN1Dx1VDx1VDx9ZEm56ePoVjaIVoAFd/uOOwrl3YaE9PT05uaBu4wmZgMwSutEZTKpkIuaDyUioHJUfAN1XTvV05bQFOD5ZbSnZZbxgoO/H/tgwdL98pS8fO+/gCJs3eSDMiiCTjgfc94D5wJmFORpbzKmcWEyiYrFieb4AZWCNkbIWQKEmS7xnNEwHZXMepoqx8lJDw6/PJL89/hMp4bB5tZPFPa63j46T2noVv5jUXrRXcY1sZK+TjGlS5bYv26c43NqOfe9IpabXKhwWzOZsPm+y7lg6hGQB6iGbkogFCASuUHjmklDujQUR9rHRCs3BnKMwwfnRAo9YsuCuErXAQtOs83t/7LUoUrwragefMmGk4Ku3MMW0Fz5E6UBmkiV2yBU5JlKxAM3PNSa/ha9IkTWEpAYNGezPCscKYTTEnT0Kcmds2r7zPNq1s+v3MCekbJPl2o7TKwSrw1waNn0ZuiZ9EMK4F1Qo8YzTjli4XOlK4bnvmojLBejqeufZ0lB4Zyw/C+q089qN+MybBFN+Qg7FUj8acLg0tmFwQIe7stnNBrNX+QyBSV5CZuhDe5vyu8Go8DJ8ORy/HB89GbT8eBl8GXwWHQRh8FzwLXgTnwUXAO391/u780/m3e9R92b3s/r51fbDXxnwRvLO6/H8N7dO+</latexit>
✓ ◆ ✓ ◆
r (s) ⌧r s s 90
H(s) = = kr = kr ,
r(s) ⌧r s + 1 s + pwo
Phase (deg)
where kr = 0.2, pwo = 0.45 rad/s, and Ts = 0.01 s 45
This gives a discrete-time transfer function of

0
1
0.1996 0.1996 z r (z) 10 -2 10 -1 10 0 10 1
H(z) = = Frequency (rad/s)
r(z) 1 0.9955 z 1
with the corresponding di↵erence equation
r,k = 0.9955 r,k 1 + 0.1996 rk 0.1996 rk 1
The coeficients of H(z) can be calculated manually as we have done here or

they can be computed in MATLAB using the c2d command with the ’tustin’
option or in Python using the control.matlab.c2d function. This is especially
convenient for higher-order transfer functions.
Simulation Project
This slide needs updating
• Roll attitude loop. For Aerosonde model use Va = 17 m/s. Note that
max
is a design parameter. Put aircraft in trim, and command steps on
roll.
• Course attitude loop. Command steps in . Can by-pass simplified
simulink files.
• For sideslip, assume no rudder, i.e., set r = 0.

• Pitch attitude loop. Note that emax
✓ is a design parameter. Don’t use
simplified simulink file. Command steps in pitch angle.
• Altitude using pitch, airspeed using pitch, and airspeed using throttle:
implement directly on full Simulink model.
• Implement full autopilot using state machine for longitudinal control. Sim-
ulation should be from take-o↵ to altitude hold.
Outline
• Successive Loop Closure
• Total Energy Control
• LQR Control
Total Energy Control
• Developed in the 1980’s by Antonius Lambregts
• Based on energy manipulation techniques from the 1950’s
• Control the energy of the system instead of the altitude and airspeed
Thrust
Total
Energy
Kinetic Potential
Energy Energy
Elevator
Drag
• Kinetic Energy: EK , 12 mVa2
• Potential Energy: EP , mgh
• Total Energy: E T , EP + EK
• Energy Di↵erence: E D , EP EK
Original TECS proposed by Lambregts is based on energy rates:
c
Rt ˙
• T = TD + kp,t Ėt + ki,t t0 Ẽt ⌧
– TD is thrust needed to counteract drag

– PI controller based on total energy rate
c
Rt ˙
• ✓ = kp,✓ Ėd + ki,✓ t0 Ẽd ⌧
– PI controller based on energy distribution rate
• Stability shown for linear systems
We will show that the performance of this scheme is less than desirable.
If the trust needed to counteract drag is unknown, then one possibility is to

use an integrator to find TD :
c
R
• T = kp,t Ẽt + ki,t Ẽt d⌧ + kd,t Ėt
– PID controller based on total energy (not energy rate)
c
R
• ✓ = kp,✓ Ẽd + ki,✓ Ẽd d⌧ + kd,✓ Ėd
– PID controller based on energy distribution (not rate)
Nonlinear re-derivation:
• Error Definitions
1 ⇣ d 2 ⌘
ẼK = m Va Va2
2
ẼP = mg hd h
• Lyapunov Function
1 2 1 2
V = ẼT + ẼD
2 2
• Controller
c ẼTd ẼT
T =D+ + kT
Va Va
!
d
ḣ 1 ⇣ ⌘
c 1
= sin + k1 ẼK + k2 ẼP
Va 2mgVa
c ĖT ẼT
• Original: T = D + kp,t + ki,t
mgVa mgVa
d
c Ė T ẼT
• Nonlinear: T =D+ + kT
Va Va
Similar if kp,T = mg and ki,T = mgkT .
The nonlinear controller uses the desired energy rate.
• Modified Original (Ardupilot):
c kp,✓ ⇣ ⌘ ki,✓
✓ = (2 k) ĖP k ĖK + ẼD
Va mg Va mg
k 2 [0, 2]
• Nonlinear:
!
ḣd
1 ⇣ ⌘
c 1
= sin + k1 ẼK + k2 ẼP
Va 2mgVa
k1 , |kT kD |
k2 , kT + kD
0 < kT  k D
Lyapunov derivation suggests potential energy error should be weighted more

than kinetic energy
If the drag is unknown, then we can add an adaptive estimate:

d
c ˆ> Ė T ẼT
T = D̂ + + + kT
Va Va
!
ḣ d
1 ⇣ ⌘
c 1
= sin + k1 ẼK + k2 ẼP
Va 2mgVa
⇣ ⌘
˙ˆ
= T ẼT D ẼD Va
Step in Altitude, Constant Airspeed
10
Altitude (m)
0
0 10 20 30 40 50
13.5
Airspeed (m/s)
13
12.5
12
0 10 20 30 40 50
Time (s)
Original TECS Nonlinear TECS Adaptive TECS SLC TECS PID ArduPilot PID
Step in Airspeed, Constant Altitude
0.2
Altitude (m) 0
−0.2
−0.4
0 10 20 30 40 50
13
Airspeed (m/s)
12
11
10
0 10 20 30 40 50
Time (s)
Step in Altitude and Airspeed
10
Altitude (m)
5
0
0 10 20 30 40 50
Airspeed (m/s)
16
14
12
0 10 20 30 40 50
Time (s)
• Observations
– TECS seems to work better than successive
loop closure.
– Removes needs for different flight modes.
– Nonlinear TECS seems to better, but the
Ardupilot controller works very well.
Outline
• Successive Loop Closure
• Total Energy Control
• LQR Control
LQR Control
Augment the States with an Integrator.
Given the state space system
ẋ = Ax + Bu
z = Hx
where z represents the controlled output. Suppose that the objective is to drive
z to a reference signal zr and further suppose that zr is a step, i.e., żr = 0. The
first step is to augment the state with the integrator
Z t
xI = (z(⌧ ) zr ) d⌧.
1
LQR Control
Augment the States with an Integrator. (cont)

Defining the augmented state as ⇠ = (x> , x>
I ) >
, results in the augmented
state space equations
⇠˙ = Ā⇠ + B̄u,
where ✓ ◆ ✓ ◆
A 0 B
Ā = B̄ = .
H 0 0
LQR Control
Linear Quadratic Regulator Theory
Given the state space equation
ẋ = Ax + Bu
and the symmetric positive semi-definite matrix Q, and the symmetric positive
definite matrix R, the LQR problem is to minis the cost index
Z 1
J(x0 ) = min x> (⌧ )Qx(⌧ ) + u> (⌧ )Ru(⌧ )d⌧.
u(t),t 0 0
If (A, B) is controllable, and (A, Q1/2 ) is observable, then a unique optimal

control exists and is given in linear feedback form as
u⇤ (t) = Klqr x(t).
LQR Control
Linear Quadratic Regulator Theory (cont)
The LQR gain is given by
Klqr = R 1
B > P,
where P is the symmetric positive definite solution of the Algebraic Riccati

Equation
P A + A> P + Q P BR 1 B > P = 0.
LQR Control
Linear Quadratic Regulator Theory (cont)
It should be noted that Klqr is the optimal feedback gains given Q and R.
The controller is tuned by changing Q and R.
Typically we choose Q and R to be diagonal matrices
0 1 0 1
q1 0 . . . 0 r1 0 . . . 0
B 0 q2 . . . 0 C B 0 r2 . . . 0C
B C B C
Q=B. .. .. C R = B .. .. .. C ,
@ .. . . A @ . . . A
0 0 . . . qn 0 0 . . . rm
where n is the number of states, m is the number of inputs, and qi 0 ensures

Q is positive semi-definite, and ri > 0 ensure R is positive definite.
LQR Control
Lateral Autopilot
As derived in Chapter 5, the state space equations for the lateral equation
of motion are given by
ẋlat = Alat xlat + Blat ulat ,
where xlat = (v, p, r, , )> and ulat = ( a , r )> .

The objective of the lateral autopilot is to drive course to commanded
course c . Therefore, we augment the state with
Z
xI = ( c )dt.
Since ⇡ , we approximate xI as
Z
xI = (Hlat xlat c )dt,
where Hlat = (0, 0, 0, 0, 1).

LQR Control
Lateral Autopilot (cont)
The augmented lateral state equations are therefore
⇠˙lat = Ālat ⇠lat + B̄lat ulat ,
where ✓ ◆ ✓ ◆
Alat 0 Blat
Ālat = B̄lat =
Hlat 0 0
The LQR controller designed using
Q = diag ([qv , qp , qr , q , q , qI ])
R = diag ([r a , r r ]) .
LQR Control
Longitudinal Autopilot
As derived in Chapter 5, the state space equations for the longitudinal equa-
tions of motion are given by
ẋlon = Alon xlon + Blon ulon ,
where xlon = (u, w, q, ✓, h)> and ulat = ( e , t )> .

The objective of the longitudinal autopilot is to drive altitude h to com-
manded altitude hc , and airspeed Va to commanded airspeed Vac . Therefore,
we augment the state with
✓ R ◆
(h hc )dt
xI = R
(Va Vac )dt
Z ✓ ✓ ◆◆
hc
= Hlon xlon dt,
Vac
where ✓ ◆
0 0 0 0 1
Hlon = 1 1 .
Va Va 0 0 0
LQR Control
Longitudinal Autopilot (cont)
The augmented longitudinal state equations are therefore
⇠˙lon = Ālon ⇠lon + B̄lon ulon ,
where ✓ ◆ ✓ ◆
Alon 0 Blon
Ālon = B̄lon =
Hlon 0 0
The LQR controller designed using
Q = diag ([qu , qw , qq , q✓ , qh , qI ])
R = diag ([r e , r t ]) .

Successive Loop Closure

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Successive Loop Closure

Uploaded by

Copyright:

Available Formats

Chapter 6

Successive Loop Closure

Airspeed, Path following

Key idea: Each successive loop must be lower in bandwidth

(s + psp )(s2 + 2⇣dr !ndr s + !n2 dr )

• Our current suggestion is to not have an integrator on inner loops,

• An integrator will be used on the course loop to correct for steady-

kp g/Vg s + ki g/Vg c g/Vg s

With a zero, the canonical second-order TF is given by

Note that ⇣ has a di↵erent e↵ect when the zero is present

(kp g/Vg )s + (ki g/Vg ) c (g/Vg )s

Equating coefficients to canonical TF gives:

!n2 = ki g/Vg and 2⇣ !n = kp g/Vg

Design parameters are bandwidth separation W and damping ratio ⇣

(s + psp )(s2 + 2⇣dr !ndr s + !n2 dr )

If model is known, the the design parameters are

Inner Loop (roll attitude hold)

• !n - Error in roll when aileron just saturates

• W > 1 - Bandwidth separation between roll and course loops

• kr - gain for yaw damper

gains are tuned one at a time, in this specific order:

Inner Loop (roll attitude hold)

Outer Loop (course hold)

• ⌧r - Tune ⌧r to get acceptable step response

• Regulate altitude by commanding pitch

• Regulate airspeed by commanding throttle

• Regulate pitch to a fixed ✓c

Provided pitch loop functions as intended, we can simplify the inner-loop

A PI control on altitude ensures that h tracks constant hc with zero steady-state

Equating the transfer functions

gives the coefficients

!n2 h 2⇣h !nh

where bandwidth separation is achieved by selecting

A PI control on the throttle-to-airspeed loop ensures that Va tracks a constant

Equating the transfer functions and their coefficients

gives the gains

! nV 2 2⇣V !nV aV1

Design parameters are natural frequency !nV and damping ratio ⇣V

The control signal is

def init ( self , ts control ):

def update ( s e l f , cmd , s t a t e ) :

return d e l t a , s e l f . commanded state

Altitude Hold Outer Loop

Airspeed Hold Outer Loop

• !nV - Natural frequency for throttle loop.

• kp✓ - Tune kp✓ to get acceptable step response.

Airspeed Hold Outer Loop

Throttle hold (inner loop)

PID continuous time

Taking Laplace transform…

Use bandwidth-limited differentiator to reduce noise

control after anti-wind-up update

subtracting top two equations

Let value of control after saturation is applied

1 function u = pidloop(y_c, y, flag, kp, ki, kd, limit, Ts, tau)

31 function out = sat(in, limit)

For a general, first-order continuous-time transfer function of the form

we want to be able to calculated the equivalent discrete-time transfer function

for a specific sample⇣rate Ts⌘. Using Tustin’s method (trapezoidal rule) by

(s + psp )(s2 + 2⇣dr !ndr s + !n2 dr )

This gives a discrete-time transfer function of

with the corresponding di↵erence equation

r,k = 0.9955 r,k 1 + 0.1996 rk 0.1996 rk 1