!!高さ方向の誘導
//
現在の機体位置から次のウェイポイントまでを見上げた角度を目標経路角とし，現在の経路角との差をPIDコントローラに入力し，安定化ループへの上昇コマンドを生成する．現在の機体位置を(x,y)，高度をh，次のウェイポイントの位置を(xc,yc)，高度をhcとする．目標経路角γcは，
{{ref_image GAMC.png}}
である．
{{ref_image GAM.png}}
'''目標経路角の計算'''

{{ref_image LONG.png}}
'''高さ方向の誘導則'''
//
!PIDゲインについて
誘導コントローラのパラメータであるPIDゲインは飛行実験や数値シミュレーションで決定する．その数値シミュレーションに使う縦ダイナミクスは，一次遅れで近似したものを用いる．
{{ref_image LONGD.png}}
ここでδthcomは安定化ループに入力する上昇コマンドである．ゲインKと時定数Tは，飛行試験でステップ状の上昇コマンドを入力し，その応答から決定する．
----
[[UAVの作り方へ戻る|HowtoMakeUAV]]