軌道決定・推定に使いたい誤差伝播のお話

この記事は、東京大学航空宇宙工学科／専攻 Advent Calendar 2019 の17日目の記事として書かれています

adventar.org

理論的な学術記事ってなんだろうね
Jo論
条件付確率・ベイズ推定
誤差伝播とフィルタリング手法
- カルマンフィルター (KF) 、拡張カルマンフィルター (EKF)
- 無香カルマンフィルター (UKF)
誤差伝播の手法
まとめ

理論的な学術記事ってなんだろうね

「あったよ！摩擦0かつ傾斜が一定な坂が！」

「でかした！」

「まな板にしようぜ！」

～こんな方へ～

「普段離散時間システムとフィルタリングを使ってるけど、そもそもどんな根っこや枝葉があるんだ？」という方

「数式とかいいので、雰囲気かしこい記事をください」という方

暇な方

※数学的な正しさよりも、概念的な理解が進むことを考慮して書いております（QiitaやQuoraじゃないので…）

Jo論

　実在するモノをどうにかして推定・制御しようとする私たちは、「誤差」という恐怖から逃れることができません。技術屋が用いる「誤差」という言葉は、理論値――選りすぐりの天才たちが考案した数式や、CPUを利用した（誤差無し）数値計算――からの実際の物理現象のずれのことで、これが大したことない内は無視した議論ができます。一方、未来のことを予測して制御しようと考えると、大体の場合は誤差が拡大していって酷いことになります。誤差が未来でどうなっているか考える時に、最終的に確率的な考え方を導入するというのがダイナミクス界隈で主流になっているやり方です。
　この未来の誤差を考えるために使う手法が誤差伝播であり、だいたい確率的な事象を扱うときに出てきます。確率論的な考えは対称な意味合いで用いられる決定論的事象とセットで考えるとその性質がわかりやすく、

「決定論的」（Deterministic）
ある事象が必ず決まった道筋をたどると考えて現象を考えること。例えば、ある時間に＋１にある物体は次に必ず＋２にある…というように時間ごとに状態が定まる現象の考え方。摩擦0の坂を一定の重力加速度のみを受けて進んでいく箱とか。

「確率論的」（Statistic）
ある事象がその状態でいる確率から、次の時間の条件付確率を考えて事象の遷移を考察すること。例えば、ある時間に＋１に80％で、－１に20％の確率で存在するので、次の時間では＋１に60％、－１に40％の確率で存在する。という解析をすること。お酒君の力で生じる千鳥足ウォークとか。

といった比較ができます。

条件付確率・ベイズ推定

　以上を考えると、確率論的事象を考えるには条件付確率という考えが大事になってくることがわかります。

そもそも確率論的事象を考える必要があるモデルには出力と遷移の双方に確率的なぶれ（誤差）があることがほとんどで、出力と一個前の状態量を観察してその条件下で現在の状態がどうなっているのか推定する、ベイズ推定が主流の推定手法として機能しています。

　ベイズ推定については私如きが解説してもあれなので、参考文献を参照するといいと思います。[*1]

すごくざっくりまとめれば、条件付確率に基づいて尤もらしい事象を推定するということです。私がそれとなく研究したつもりになっている軌道決定においても、誤差伝播の考えを外して考えることができません。宇宙機の位置を求めるためには「観測結果に誤差が乗る」ことと「非線形な運動をモデル化した故の誤差の伝播」を考えないといけないからです。誤差が動いていく様子を考える時には、その誤差がガウス分布に沿っていることを前提に、状態量の期待値と共分散行列の伝播を考えることになります。

誤差伝播とフィルタリング手法

カルマンフィルター (KF) 、拡張カルマンフィルター (EKF)

　宇宙工学の分野で誤差伝播を取り扱えるようになった事例といえば、宇宙輸送ロケットやアポロ宇宙船の軌道推定に用いられたカルマンフィルター（KF）、拡張カルマンフィルター（EKF）が思い浮かびます。これらのフィルターには

確率的な部分にガウシアン仮定が置かれており、マルコフ過程であるという前提でシステムが敷かれている。
非線形な挙動を局所的に線形近似し、追従できるように工夫している

といった特徴があります。この2点目の工夫が曲者で、非線形性が強い過程にはついていけずに変な収束をする・そもそも推定が収束しないといったことが多いです。しかし出てくる数字がとにかく大雑把な天体間軌道には有効なことが多く、アポロ宇宙船のナビゲーションに使われたのを皮切りに、今も宇宙機航法の論文中によく現役で登場するフィルターです。

～EKFのやりかた～[*2]

にゅーめんくらちゃあ

$\boldsymbol{X}:状態量, \boldsymbol{\hat{X}}:推定値, \boldsymbol{Z}:観測量（出力）, P:推定共分散誤差, Q:力学的誤差, R:観測誤差, A:\frac{\partial f(\boldsymbol{X})}{\partial \boldsymbol{X}}, C:\frac{\partial \boldsymbol{Z}}{\partial \boldsymbol{X}}$

$\boldsymbol{X}(t=i+1)=f(\boldsymbol{X}(t=i)),\boldsymbol{Z}(t=i)=h(\boldsymbol{X}(t=i))$

1.真値の更新

$\boldsymbol{X}(t=i+1)=f(\boldsymbol{X}(t=i))$

2.状態量・観測量・誤差共分散の事前推定

$\boldsymbol{\hat{X}^{-}}(t=i+1)=f(\hat{X}(t=i))\\ \boldsymbol{\hat{Z}^{-}}(t=i+1)=h(\boldsymbol{\hat{X}^{-}}(t=i+1))\\ P^{-}(t=i+1)=A(t=i)P(t=i)A^{T}(t=i)$

3.実観測による修正・フィルタリング

$\tilde{\boldsymbol{Z}}=\boldsymbol{Z}(t=i+1)-\boldsymbol{\hat{Z}^{-}}(t=i+1)\\K=C(\boldsymbol{\hat{X}^{-}})P^{-}(t=i+1)(C(\boldsymbol{\hat{X}^{-}})P^{-}{t=i+1}C^{T}(\boldsymbol{\hat{X}^{-}})+R)^{-1}\\ \boldsymbol{\hat{X}}(t=i+1)=\boldsymbol{\hat{X}^{-}}(t=i+1)+K\tilde{\boldsymbol{Z}}\\ P(t=i+1)=P^{-}(t=i+1)-KC(\boldsymbol{\hat{X}^{-}})P^{-}(t=i+1)$

無香カルマンフィルター (UKF)

このカルマンフィルターの登場を皮切りに、様々な逐次フィルタリングの理論が登場します。とくに有名なものは無香カルマンフィルター（Uncented KF）とその仲間たちで、代表点をモデルに従って伝播させることで非線形性に対抗しようとしているのがポイントです。

UKFのアルゴリズムは次のようになります。（画像で失礼します…）[*3], [*4]

f:id:dkhate425425:20191217185854p:plain

ところで、「Uncented」という言葉を名付けた発案者の知人は「未だ手付かずな領域で考案された、無垢な新しい理論である」ことをニュアンスに込めたくて提案したと推測されています。ちょっと日本人には伝わりにくい由来な気はします。

誤差伝播の手法

ここまでガウシアンプロセスに対する代表的な推定手法であるKF達を紹介しました。しかし本記事の趣旨は誤差伝播手法の紹介なので、ガウシアンプロセスやそれ以外の誤差伝播手法について紹介したいと思います。いくつかの手法をサーベイ[*5]から抜き出して調査する会があったので、同期・先輩・後輩方のリサーチを引用しながらさわり程度の解説をしていきます。

[おしながき]

Uncented Transform

CADFT

State Transition Tensor

（DA - Differential algebla）

Polynomial Chaos

Uncented Transform

Uncented Transform は、UKFのアルゴリズム途中に出てきた誤差伝播の手法で、

１．推定誤差共分散行列からシグマポイントと呼ばれる「分布の代表点」を、次元数nに対して2n+1個取る

２．それぞれの点を状態遷移モデルに従って遷移させ，これらの点から重み付け和により状態量と観測量を推定する

という手法です．UTでは2次までのモーメントを求められます。（つまり、シグマポイントを取る際の分布にガウシアン仮定をしています）

CADFT

Covariance Analysis Describing Function Techniqueの略
統計的線形化という手法を用いて期待値と共分散行列の微分方程式を作り、それを伝搬していく方法です。利点と欠点は
メリット：伝搬時間が短い間は線形化共分散分析より正確であり、またヤコビアンが定義できないシステムにも用いることができる。
デメリット：非線形性に弱く、伝搬時間が長い場合は正確性が失われること、また3回微分まで計算する必要がある。
といった点を挙げられます。
統計的線形化は、その名の通り非線形ベクトル関数 $f(\boldsymbol{X})$ を統計的に $\boldsymbol{f(x)}\approx\boldsymbol{\alpha}+N_f(\boldsymbol{x}-\boldsymbol{m})$ のように線形化することです。まず $\boldsymbol{X}$ の期待値と共分散行列を $m,P$ として線形化誤差 $\boldsymbol{e}=\boldsymbol{f(x)}-\boldsymbol{\alpha}-N_f(\boldsymbol{x}-\boldsymbol{m})$ を定義し、線形化誤差の二次形式の期待値 $J=\mathbb{E}[\boldsymbol{e}^TA\boldsymbol{e}]$ を最小にする2パラメータ $\boldsymbol{a},N_f$ を求めます。（Aは任意の半正定値行列です）
$\frac{\partial J}{\partial a}=\mathbb{E}(2A\boldsymbol{e})=0$ より、 $a=\mathbb{E}[f(X)]$ であり、 $J$ に代入して,
$J=\mathbb{E}[(N_f(m-x)+f(x)-\mathbb{E}[f(\boldsymbol{X})])^TA(N_f(\boldsymbol{x}-\boldsymbol{m})+f(x)-\mathbb{E}[f(x)]]$

従って $N_f=\mathbb{E}[\boldsymbol{f(x)}(\boldsymbol{x}-\boldsymbol{m})^T]P^{-1}$ となり、最終的に

$\boldsymbol{f(x)}\approx\mathbb[\boldsymbol{f(x)}]+\mathbb{E}[\boldsymbol{f(x)}(\boldsymbol{x}-\boldsymbol{m})^T]P^{-1}(\boldsymbol{x}-\boldsymbol{m})$

となります。

これを状態方程式 $\boldsymbol{f(x)}\approx\mathbb{E}[\boldsymbol{f(x)}]+N_f(\boldsymbol{x}-\boldsymbol{m})$

期待値を取ることと時間微分は可換なので、

$\dot{\boldsymbol{m}}=\mathbb{E}[\boldsymbol{f(x)}]$

また、共分散行列を考えると、

$\frac{d}{dt}(\boldsymbol{x}-\boldsymbol{m})(\boldsymbol{x}-\boldsymbol{m})^T=(\dot{\boldsymbol{x}}-\dot{\boldsymbol{m}})(\boldsymbol{x}-\boldsymbol{m})^T+(\boldsymbol{x}-\boldsymbol{m})(\dot{\boldsymbol{x}}-\dot{\boldsymbol{m}})^T=N_f(\dot{\boldsymbol{x}}-\dot{\boldsymbol{m}})(\boldsymbol{x}-\boldsymbol{m})^T+(\dot{\boldsymbol{x}}-\dot{\boldsymbol{m}})(\boldsymbol{x}-\boldsymbol{m})^TN_f^T$

より、この期待値を取ることで

$\dot{P}=N_fP+PN_f^T$

がわかります。つまり、 $\mathbb{E}[\boldsymbol{X}],N_f$ がわかれば $\boldsymbol{m},P$ を伝播できます。

$\mathbb{E}[\boldsymbol{X}]$ について、多変量の場合、期待値を解析的に求めるのは難しいので、 $\boldsymbol{f(x)}$ を $\boldsymbol{m}$ 周りでテイラー展開し、二次近似を行います（これは、少なくとも $x$ がガウシアンに従うときは妥当な仮定のように思えます)。計算は省略して、

$\displaystyle \mathbb{E}[\boldsymbol{f_n(x)}]\approx\boldsymbol{f_n(m)}+1/2\sum_{i,j} \frac{\partial^2 \boldsymbol{f_n(m)}}{\partial \boldsymbol{x}_i \partial \boldsymbol{x}_j}P_{i,j}$

また、 $N_f$ は、 $p(\boldsymbol{x})$ が正規分布の時、 $\frac{d}{d \boldsymbol{m}}p(\boldsymbol{x})=p(\boldsymbol{x})(P^{-1}(\boldsymbol{x}-\boldsymbol{m}))^T$ となり、期待値の積分を考慮すれば

$\displaystyle \frac{d}{d \boldsymbol{m}}=\mathbb{E}[\boldsymbol{f(x)}(P^{-1}(\boldsymbol{x}-\boldsymbol{m}))^T]=\mathbb{E}[\boldsymbol{f(x)}(\boldsymbol{x}-\boldsymbol{m})^T]P^{-1}=N_f$

となります。二次元二体問題について（ガウシアンを仮定して）やってみると結構よい計算練習になると思います。

State Transition Tensor

STM（State Transition Matrix）の高次拡張版という感覚的な説明ができます。
特徴としては、ダイナミクスをテイラー展開して近似しており、統計量を近似する手法とは異なるアプローチである
ダイナミクスの高次項を考慮してSTMと比べて高精度な不確定性伝搬が可能である
というものを備えています。一方でダイナミクスは微分できる必要があり、高事項であるほどSTTの計算は煩雑になっていきます。
①ノミナル軌道生成
②ノミナル軌道から各時間のダイナミクスf_iを求め，微分方程式より欲しい次数のSTTを伝播
③STTが求まれば，初期値の平均，共分散行列より任意時刻での平均，共分散行列が求まる（ただし，初期分布がガウス分布の場合）
ただ、このテンソル計算は相応のモジュールを用意しなければかなりの重さになり、実装するにあたっての壁が高そうな手法だなぁというのが、解説を聞いたり実装の様子を見た感想であったりします。

Polynominal Chaos

多項式カオス展開手法　と訳すことができます。ランダム変数空間を直行多項式基底を用いて確率解へ射影する手法であり、特徴を列挙すると、

確率微分方程式を解いて得られる確率解の確率密度分布を直接近似してくれる。
確率微分方程式の解なので、初期状態量の不確定性だけでなくダイナミクス自体の不確定性も扱える。（太陽輻射圧, 重力場, 大気密度の不確定性など）

GMMやSTTと比較して、多くの項を更新する煩雑な手続きがなく精密な確率密度分布を近似できる。

といった特徴を挙げられます。

～ざっくりしたやり方～

定式化は

$\displaystyle \boldsymbol{Y}=\sum_{\boldsymbol{\alpha} \in \mathbb{N}^M} y_\boldsymbol{\alpha}\Psi_\boldsymbol{\alpha}(\boldsymbol{X})$

とできます。ここでそれぞれの成分は、

$\boldsymbol{Y}$ ：時刻tにおけるランダムな出力 (n×1)

$\boldsymbol{X}$ ：時刻によらない互いに独立な成分を持つランダムベクトル (m×1)

$y_\boldsymbol{\alpha}$ ：時刻tにおける係数 (n×1)

$\Psi_\boldsymbol{\alpha}$ ：時刻によらないスカラー出力の基底関数

$\boldsymbol{\alpha}$ ：は既定関数の次元を定めるインデックスベクトル $\boldsymbol{\alpha}={\alpha_1,\alpha_2,...,\alpha_M}$

となっています。以下では簡単にn=1の $Y$ がスカラである場合を考えます。

ランダムベクトルの成分は互いに独立なので、同時確率密度関数 $f_\boldsymbol{X}(\boldsymbol{x})=\prod_{i=1}^M f_{X_i}(x_i)$ を定義できます。

基底関数 $\Psi_\boldsymbol{\alpha}$ は、インデックスベクトル $\boldsymbol{\alpha}$ に対して一つ定まる基底関数です。 $x_i$ に対応した正規化直交多項式 $P^{i}_{\alpha_i}$ により、

$\Psi_\boldsymbol{\alpha}(\boldsymbol{x})=\prod_{i=1}^M P^{(i)}_{\alpha_i}(x_i)$

と定義できます。

直交多項式は漸化式で逐次的に求まり、帰納的に $p_n(x)$ は必ずn次の多項式になります。

このように変換した $\boldsymbol{Y}$ は、異なる基底関数 $\Psi_\boldsymbol{\alpha},\Psi_\boldsymbol{\beta}$ の積の期待値が持つ性質から、

$\mu_Y=\mathbb{E}_\boldsymbol{X}[Y]=y_0$

$\sigma^2_Y=\mathbb{E}_\boldsymbol{X}[Y^2]-{\mu_Y}^2= \sum_{\alpha,\alpha\neq0} {y_\alpha}^2$

（興味のある方は計算してみてください）

実際の計算では、インデックスベクトルが無限にあっても意味がないので、truncation schemeというものを用いて有限個のaを求め、任意のオーダーpまでの近似を取る手法が一般的なようです。モデルによってエルミート多項式、ルジャンドル多項式等を用いるようです。

$y_\alpha$ は、モデルに踏み込んで直接微分方程式を解く手法・モデルをBlock Boxと考えて解く手法により求められ、前者にはGalerkin Projection method, 後者にはSpectral Projection, Ordinary Least squaresといった手法が存在します。

まとめ

・誤差伝播という、誤差とダイナミクスに向き合う手段にどんなものがあるのかざっくり見てみました。
・王道の誤差伝播に対するフィルタリングとしてはカルマンフィルターがあります。
・その他、いろいろなフィルタリングや誤差伝播手法があるので紹介しました。

ざっくりこんな感じになったと思います。推定・制御界隈は猛者の巣窟なので、この貧弱記事では無限に背中から刺されそうですが…手法の名称間違いや前提・数式的な誤りがあれば是非ご指摘ください。
力尽きた中途半端な記事で失礼しました…見たまま編集とかいう見えてる地雷を踏んだ結果がこれだよ

「あったよ！質点として扱っても力学的誤差が観測できないぐらい小さい球が！」
「おい、それ俺たちを吸い込もうとしてな

　　終

制作・著作
━━━━━
　ⓃⒽⓀ

*1:確率ロボティクス pp.024 - 031

*2:電子情報通信学知識ベース 6章カルマンフィルタ

*3:UKF (Unscented Kalman Filter) って何？山北　昌毅 2006

*4:S. J. Julier, J. K. Uhlmann, and H. F. Durrant-Whyte, “New approach for filtering nonlinear systems,” Proc. Am. Control Conf., vol. 3, no. June, pp. 1628–1632, 1995.

*5:Y. Luo and Z. Yang, “A review of uncertainty propagation in orbital mechanics,” Prog. Aerosp. Sci., vol. 89, no. December 2016, pp. 23–39, 2017.