この章では上下方向、つまり高度を制御します。姿勢と違って 軸の連成がない素直な1次元——だからこそ、制御設計の王道の手順 「モデルを立てる → トリム点を見つける → 線形化する → 制御器を設計する」を、 省略なしのフルコースで歩くのに最適な題材です。 そして後半では、実機がこの王道の外で戦っている2つの「現実」—— 高度は 30 Hz でしか測れないこと、 トリム推力は動くこと——を、実際の飛行ログ解析の物語つきで見ます。
8.1 モデル — 鉛直1次元の運動方程式
上向きを正にとると、機体の鉛直運動はこれだけです:
\(T\) は4つのロータの合計推力、\(d_v v\) は鉛直方向の空気抵抗 （ホバ付近ではロータの誘導流による項が主。理論推定で \(d_v/m \approx 1.4\ \mathrm{1/s}\)、 未実測なので設計では小さめに見込みます）。推力は第6章と同じく、 指令からモータの一次遅れ \(\tau_m \approx 20\ \mathrm{ms}\) を経て発生します。 使う数字はすべて第4章の実測値: \(m = 36.8\ \mathrm{g}\)。
8.2 トリム点 — まず「釣り合い」を見つける
制御の前に、系がじっとしていられる点＝トリム点（平衡点）を探します。 ホバリングの釣り合いは \(T_0 = mg = 0.361\ \mathrm{N}\)。 たった1行ですが、実はここに実機の最初の難所があります。 「Duty をいくつにすれば 0.361 N 出るのか」は、 電池電圧で変わってしまうのです。
StampFly はこれを第6章で見た逆モータ曲線＋電圧補償 （必要電圧を生きた電池電圧で割って duty にする: duty = V/Vbat）で フィードフォワード的に解決しようとします。理論的には 「同じ推力指令 → 同じ推力」が保証されるはず——しかし実際には ぴったりにはなりません。ずれを小さくとどめる工夫（補償）と、 残りをフィードバックで埋める仕組みの分担。これがこの章の裏テーマです。
8.3 線形化 — トリまわりの小さな世界
トリム点まわりの変化分 \(\Delta T = T - T_0\)、\(z\)、\(v\) で書き直すと、 重力の定数項が消えて線形なプラントが現れます:
第5章の姿勢と同じ「積分が重なる系」です。ここまでが王道の前半—— あとは道具箱（第5〜7章）の出番です。
8.4 まず PID をつないでみる — ゲイン調整の限界を体で知る
道具は第6章で揃っています。さっそく高度誤差から推力補正への PID （時定数形、D は測定値側）をつないで、自分の手で調整してみてください。 目標高度は 0.2〜3.0 m の全範囲で動かせます。 なお、このシミュレーションの中身は最初から実機仕様です—— 高度は ToF 30 Hz でしか測れず、制御は相補フィルタの推定値 \(\hat{z}, \hat{v}\) を使い、推力補正には ±0.15 N の飽和があります （計測の中身は 8.6 節で開けます）。
条件: 単段 PID（\(u = K_p(e + \frac{1}{T_i}\int e\,dt) - K_p T_d \hat{v}\)）、 推力補正 ±0.15 N・モータ遅れ 20 ms・計測は ToF 30 Hz＋相補フィルタ。 上段=高度、下段=推力補正。
やってみると、こんな壁に当たるはずです:
- 目標 0.5 m で調整する → 数 cm の行き過ぎまで追い込めます （例: \(K_p=0.3, T_i=5, T_d=0.8\) で約 6 cm）。
- そのまま目標を 3 m にする → 同じゲインなのに行き過ぎは 20 cm 超に化けます。
- では 3 m に合わせて調整し直すと？ → ゲインを弱めても 行き過ぎは大きいまま、全部が遅くなるだけ。強めれば振動。 どのゲインの組も、小さい目標の機敏さと大きい目標の素直さを 両立できません。
これはゲインの選び方が下手なのではありません。線形の世界ならステップの大きさは 応答の「形」を変えないはず——それが変わるのは、飽和という非線形が いるからです。遠い目標ほど接近速度が育ち、±0.15 N のブレーキでは 止まりきれない。ゲインをいくらいじっても、この構造は直らないのです。
8.5 目標の渡し方を変える — そしてカスケードを再発明する
発想を変えます。ゲインではなく目標の渡し方を変えるのです。 第一手: 目標を一度に渡さず、速度制限つきでゆっくり動かす （目標整形。0.4 m/s のランプで本当の目標まで滑らかに移動させる）。 「目標の近傍までは速度で近づく」という発想の、これが最小実装です。 ——ところが、下の実験②を見てください。それだけでは直りません。 意外なことに、行き過ぎはむしろ少し悪化します。
犯人は D 項です。一定速度で登っている間、測定値微分の D は 「動くな」と逆らい続け（この例で約 −0.1 N）、 その分を積分が抱え込みます。目標に着いた瞬間、抱えた分が吐き出されて 機体を押し上げる——第6章で学んだ「D は重り」の高度版です。 でも、ここで気づきます。整形した目標は滑らかなのだから、 その速度（ランプの傾き）も一緒に指令できるはず。 D を「測定値の速度」ではなく「目標速度との差」に掛け、 上昇の抵抗分（\(d_v m \dot{r}\)）をフィードフォワードで先渡しすると—— 実験③: どの目標でも行き過ぎ 4〜5 cm・整定は倍速になります。
条件: ゲインは 8.4 の代表値（\(K_p=0.3, T_i=5, T_d=0.8\)）で固定。 破線=整形後の目標。上段=高度、下段=鉛直速度。 ③の抵抗 FF は理論推定値 \(d_v\) の見込み——ずれても残りは I が埋める（8.2 の「FF で小さく、FB で埋める」の再演）。
さて、実験③をよく見ると、私たちはカスケード制御を再発明して いました。「位置の誤差から欲しい速度を決め（整形ランプ＝速度指令）、 速度の誤差を別の項が追いかける（D=速度誤差）」——これは第5章の 位置ループ＋速度ループそのものです。実機 StampFly も最初からこの形で、 速度指令の上限（クランプ）が整形の役割を果たします:
| ループ | \(K_p\) | \(T_i\) [s] | 出力制限 |
|---|---|---|---|
| 位置（外側） | 0.45 [1/s] | 7.0 | 速度指令 ±0.5 m/s |
| 速度（内側） | 0.1 [N/(m/s)] | 2.5（ホバ時は切替可） | 推力補正 ±0.15 N |
帯域は速度ループ約 2.7 rad/s・位置 0.45 rad/s（比 1/6、第5章の 1/3 則より保守的）。 なお実機にも「上昇の抵抗分を積分が抱えて捕捉時に吐き出す」問題は残っていて、 開発ログで「捕捉オーバーシュート」として格闘の跡があります—— 8.7 節の「積分の強さをフェーズで切り替える」工夫はその対策でもあります。
8.6 現実① — 30 Hz の計測をどう埋めるか: 相補フィルタからカルマンフィルタへ
姿勢のジャイロは数kHzで読めますが、高度の絶対値を教えてくれる ToF センサ（レーザーの往復時間で距離を測る素子）は 33 ms に1回＝30 Hz しか答えをくれません。 400 Hz の制御ループから見ると、13周期に1回しか高度が更新されない世界です。
実機のファームウェアには、これを埋める推定器が2つ積まれていて、 パラメータ1つで差し替えられます。ひとつは相補フィルタ: 速い動きは加速度計の積分（400 Hz）が担い、ToF の更新が来るたびに高度と速度を そっと引き戻す（更新1回あたり高度に30%・速度に5%の固定ゲイン）。 役割分担は第5章のカスケードと同じ発想——速いが流されやすい信号と、 遅いが確かな信号を組み合わせる。仕組みが目に見えるので、 8.4 からのシミュレーションの中で動かしてきたのはこちらです。 もうひとつがカルマンフィルタの発展形（15状態 ESKF）で、 実は実機の既定はこちらです。なぜか——それがこの節の後半です。
ただし相補フィルタの補正ゲイン（30% / 5%）は人が決めた固定値で、 そして加速度計には必ずバイアス（振動・取り付け・温度による 一定のずれ）が乗ります。固定の弱い引き戻しでは、積分され続けるバイアスを 消しきれません。カルマンフィルタはここで二枚上手です—— ①補正ゲインをノイズの統計から自動で計算し、 ②「バイアス」そのものを状態に加えて推定し、差し引いてしまえる。 下の実験でバイアスを動かして、両者の差を見てください。
条件: 0.8 m へ上昇して保持する同一飛行（制御は実機カスケード）に対し、 同じ計測（ToF 30 Hz ±3 mm・加速度計にノイズ σ=0.3 とバイアス）から、 相補フィルタと3状態カルマンフィルタ（状態は \(z, v\) と加速度バイアス \(b\)。 補正ゲインは共分散の更新から毎回自動計算）で高度を推定し、 推定誤差 \(\hat{z}-z\) を比較。多センサへの拡張と理論の中身は第9章で。
固定ゲインの相補フィルタは、バイアスという「統計の外の敵」に弱い。 カルマンフィルタは状態を増やしてバイアスごと推定してしまう—— だから実機 StampFly の既定の推定器はカルマン族（ESKF）なのです （相補フィルタはパラメータ estimator.type=1 で選べる、仕組みの見通しが良い もう一つの選択肢として残されています）。
8.7 現実② — トリム推力は動く
8.2 の電圧補償で「同じ指令なら同じ推力」になったはず——でした。 実際の3分間ホバリングのログ解析（2026-06-27）はこう言っています: 電圧補償は正しく働いている（電圧と duty の相関 −1.00、 duty は 0.648→0.722 と自動で増えて総推力を保っている）。 それでも、電池が 3.6 V を切ったあたりから高度のふらつきが 2〜3倍に育ち、電圧と強く相関した（相関 −0.78）。 閉ループ再生シミュレーションで正体を追い込むと、 犯人は低電圧域で残る電圧依存の推力誤差—— 0.1 Hz 前後のゆっくりした外乱として推力に入り込んでいました。 静的な補償では、負荷で刻々と下がっていく電圧の動きを補いきれないのです。
さて、この「内側に入るゆっくりした外乱」への定石は何だったでしょうか。 第5章 5.5 節の答え——外乱が入る場所の内側の積分を強くするです。 外側（位置）の積分は届きません。実際の解析でも、速度ループの \(T_i\) を 2.5→1.5 s に縮めるとふらつきが 20%減、1.0 s で 37%減。 しかも \(K_p\) を上げる案と違って交差周波数を上げない（遅れに突っ込まない）ので 安全側のレバーです。下の実験でこのレバーを触ってみてください。
条件: 高度0.8mを保持中、t=10s から電池の電圧低下に相当する低周波外乱 （一定分＋0.1Hz、実飛行同定で 4.5〜8.5 mN 級）が推力に加わる。 灰色の破線は \(T_i\)=2.5 s（既定値）の参照。実飛行ログの再生解析では \(T_i\)=1.5 で −20%、1.0 で −37% ——この教材のモデルでもほぼ同じ比率になります。
実機はさらに2つの工夫を積んでいます。 ①積分の強さを飛行フェーズで切り替える——ホバ中だけ強い積分 （上の \(T_i\) レバー）を使い、離陸中は元に戻す。強い積分を離陸まで効かせると、 目標を捕まえる瞬間の行き過ぎが悪化するからです（8.4の実験①と同じ理屈）。 ②トリム推力を学習する——定常ホバリング中の速度ループの 定常出力は「フィードフォワードが外した分」そのものなので、これを時定数20秒で フィードフォワード側へ移し替え、着陸時に保存します。飛ぶたびに 「この機体・この電池の今日のトリム」を覚え直すわけです。 トリムは一度計算して終わりの数字ではなく、測って学び続ける量—— これが2つ目の現実への答えです。 なお、外乱そのものを推定して先回りで打ち消す外乱オブザーバ（DOB） という進んだ工夫もあり、StampFly でも試行中です—— こういう手もある、うまくいくかもしれない、という段階の話題として 紹介にとどめます。
8.8 実機の値で運転する — 高度ホールド
仕上げです。実機のゲイン・実機の制限・30 Hz の計測・電池の電圧低下による外乱—— 全部入りの高度ホールドを運転してください。第6章6.7と同じく、 プラントは実測値、初期値は実機の飛行実績ゲインです。
条件: t=1s に +0.8 m のステップ（③方式: 速度0.4m/sで接近→捕捉）、 t=12s から電圧低下の外乱 5 mN。計測は ToF 30 Hz＋相補フィルタ簡略版 （加速度積分＋ToF更新で高度30%/速度5%引き戻し、ToFに±3mmノイズ。 加速度バイアスは無し——本物の推定器は第9章で）。 上段=高度（推定値も薄く重ね描き）、下段=推力補正 [mN]（±150mN 制限）。
試してほしい実験:
- 電圧低下の外乱を入れて \(T_i\) を下げる → 8.7 のレバーがここでも効きます。
- 30Hz計測を外して理想計測にしても、ふらつきはほぼ変わりません ——「ブレは計測の遅さが原因ではない」という実飛行解析の結論を、 自分の手で確認できます。
- 速度 \(K_p\) を上げる → このモデルでは改善に見えます。ところが……（下のコラム）
まとめ
- 高度は「モデル → トリム → 線形化 → 設計」という制御設計の王道を 1次元で通しで練習できる軸。プラントは \(27.2/(s(s+1.4)(0.02s+1))\)（全て実測値）。
- トリム点 \(T_0 = mg\) は1行だが、「同じ Duty で同じ推力」は電池電圧で崩れる。 補償でずれを小さくとどめ、残りをフィードバックで埋めるのが実機の分担。
- 単段 PID はどうゲインを調整しても「小さい目標の機敏さ」と 「大きい目標の素直さ」を両立できない（飽和という非線形のため）。 解決はゲインでなく目標の渡し方——整形して速度ごと指令すると、 どの目標でも行き過ぎ4〜5cm。そしてそれはカスケードの再発明だった。
- 高度計測は 30 Hz。速い加速度計と確かな ToF を組み合わせて埋める （教材のシミュレーションでは相補フィルタが最初から動いていた）。 固定ゲインの相補はバイアスに弱く、カルマンフィルタは バイアスごと状態にして推定する——実機の既定の推定器も カルマン族（15状態 ESKF）。基準の違う気圧計は「あえて使わない」。
- それでも残る電池の電圧低下による低周波外乱には、内側（速度）の積分を強くするのが 正しいレバー（外側は届かない・交差を上げないので安全）。実機はフェーズ切替と トリム学習まで積んでいる。
30 Hz の計測を「埋める」と言いましたが、その中身——加速度計とジャイロと ToF を どう信じてどう混ぜるか——はそれ自体が一つの分野です。 次章では、いよいよセンサと状態推定の世界に入ります。
本章の対話記録: notes/qa_log.md の Q-ALT-1〜6。実機実装の出典: stampfly_ecosystem firmware/vehicle（params.cpp・actuator.cpp・ complementary_estimator.cpp）、飛行ログ解析と対策の経緯は同リポジトリの 開発ログ（2026-06-14〜07-18 のコミット群）、整理ノート analysis/notes/altitude_model_20260718.md。プラント数値は第4章の同定値。