カルマンフィルタ

\[ \newcommand{dn}[3]{\frac{\mathrm{d}^{#3} #1}{\mathrm{d} #2^{#3}}} \newcommand{\d}[2]{\frac{\mathrm{d} #1}{\mathrm{d} #2}} \newcommand{\dd}[2]{\frac{\mathrm{d}^2 #1}{\mathrm{d} {#2}^2}} \newcommand{\ddd}[2]{\frac{\mathrm{d}^3 #1}{\mathrm{d} {#2}^3}} \newcommand{\pdn}[3]{\frac{\partial^{#3} #1}{\partial {#2}^{#3}}} \newcommand{\pd}[2]{\frac{\partial #1}{\partial #2}} \newcommand{\pdd}[2]{\frac{\partial^2 #1}{\partial {#2}^2}} \newcommand{\pddd}[2]{\frac{\partial^3 #1}{\partial {#2}^3}} \newcommand{\p}{\partial} \newcommand{\D}[2]{\frac{\mathrm{D} #1}{\mathrm{D} #2}} \newcommand{\Re}{\mathrm{Re}} \newcommand{\Im}{\mathrm{Im}} \newcommand{\bra}[1]{\left\langle #1 \right|} \newcommand{\ket}[1]{\left|#1 \right\rangle} \newcommand{\braket}[2]{\left\langle #1 \middle|#2 \right\rangle} \newcommand{\inner}[2]{\left\langle #1 ,#2 \right\rangle} \newcommand{\l}{\left} \newcommand{\m}{\middle} \newcommand{\r}{\right} \newcommand{\f}[2]{\frac{#1}{#2}} \newcommand{\eps}{\varepsilon} \newcommand{\ra}{\rightarrow} \newcommand{\F}{\mathcal{F}} \newcommand{\L}{\mathcal{L}} \newcommand{\t}{\quad} \newcommand{\intinf}{\int_{-\infty}^{+\infty}} \newcommand{\R}{\mathcal{R}} \newcommand{\C}{\mathcal{C}} \newcommand{\Z}{\mathcal{Z}} \newcommand{\bm}[1]{\boldsymbol{#1}} \]

Joan Solà の論文を参考に、ドローンの自己位置推定を実装する。

Joan Solà, Quaternion kinematics for the error-state Kalman filter

東京大学 航空宇宙工学専攻 土屋研究室の方々による和訳版もある。

Joan Solá著”Quaternion kinematics for the error-state Kalman filter”の日本語翻訳の公開について | 土屋研究室 -東京大学

論文では加速度と角速度を操作量として扱っているが、本稿では状態量として扱っている点に注意されたい。

\[ \newcommand{\rot}{\mathrm{Rot}} \newcommand{\w}{\omega} \]

カルマンフィルタの基礎

予測

現在 \(t\) の推定値から次の時刻 \(t+1\) の推定値は、状態方程式から単純に推定される。

\[ x_{t+1|t} = F x_{t|t} \]

しかし、推定値には真値との誤差があり、推定によって誤差は増大する。

\[ P_{t+1|t} = F P_{t|t} F^T + G Q G^T \]

第一項は現在の推定値の誤差による分散で、第二項はプロセスノイズによる分散である。

直感的にわかるように、予測だけのよって状態を推定していては、分散は次々と増大する。

\[ x_{t|t} \rightarrow x_{t+1|t} \rightarrow x_{t+2|t} \]

\[ P_{t|t} \preceq P_{t+1|t} \preceq P_{t+2|t} \]

そこで、観測によって推定状態を補正する。

観測

センサ値 \(z\) が計測された。この値を基に、状態の推定値を補正する。

観測残差を求める。 \[ e_t = z_t - H x_{t|t-1} \] 観測値と、予測されていた観測値の差を表している。

観測残差の分散を求める。 \[ S_t = R + HP_{t|t-1}H^T \] 第一項はセンサのノイズに起因する分散で、第二項は予測誤差に起因する分散である。

カルマンゲインを求める。 \[ K_t = P_{t|t-1}H^TS_t \] 観測値がどの程度信頼できるかを表している。

更新

カルマンゲインを使って、状態の推定値と、推定値の分散を更新する。 \[ x_{t|t} = x_{k|k-1} + K_t e_t \] 予測値と観測値の「加重平均」をとることで、最も確からしい状態の推定値が得られる。

\[ P_{t|t} = (I-K_tH)P_{t|t-1} \]

誤差状態カルマンフィルタ

状態方程式

真の状態 \(x_t\) に対する状態方程式を \(f\) とする。

\[ \dot{x_t} = f(x_t) \]

誤差状態カルマンフィルタでは、真の状態 \(x_t\) をノミナル状態 \(x\) と誤差状態 \(\delta x\) に分解する。

\[ x_t = x \oplus \delta x \]

ここで演算子 \(\oplus\) は、状態の「自然な足し算」を表す。具体的には、位置に対しては並進（ベクトルの加算）を、姿勢に対しては回転（回転群の積）を意味する。

状態方程式も、ノミナル状態方程式と誤差状態方程式に分解する。

\[ f(x_t) = f(x \oplus \delta x)= f(x) \oplus f_e(x,\delta x) \]

ノミナル状態方程式は、真の状態方程式と同じである。

\[ \dot{x} = f(x) \]

誤差状態方程式は、ノミナル状態と真の状態の差分である。

\[ \dot{\delta x} = f_e(x,\delta x) \]

誤差状態方程式は、\(O(\delta x^2) \rightarrow 0\) とみなして線形化できる。

観測方程式

\[ z = h(x_t) = h(x \oplus \delta x) \]

誤差状態カルマンフィルタでは、観測をもとに誤差状態を補正する。

\[ H := \left. \frac{\partial h}{\partial \delta x}\right|_x \]

これは連鎖律から求めることができる。

\[ \left. \frac{\partial h}{\partial \delta x}\right|_x = \left. \frac{\partial h}{\partial x_t}\right|_x \left. \frac{\partial x_t}{\partial \delta x}\right|_x =: H_x X_{\delta x} \]

\(H_x\) は観測方程式次第、つまりセンサ次第だが、\(X_{\delta x}\) はモデルの状態ベクトルから自動的に定まる。

状態方程式

状態 \(x\) \(\dot{x}\) \(\delta x\) \(\dot{\delta x}\)
位置 \(p\) \(\dot{p} = v\) \(\delta p\) \(\dot{\delta p} = \delta v\)
姿勢 \(q\) \(\dot{q} = \frac{1}{2}q \w\) \(\delta \theta\) \(\dot{\delta \theta} = \rot(\delta \theta) \, \rot(q) \, (\w + \delta \w)\)
速度 \(v\) \(\dot{v} = \rot(q) a\) \(\delta v\) \(\dot{\delta v} = \rot(\delta \theta) \, \rot(q) \, (a + \delta a)\)
加速度 \(a\) \(\delta a\)
加速度センサバイアス \(a_b\) \(\delta a_b\)
角速度 \(\w\) \(\delta \w\)
角速度センサバイアス \(\w_b\) \(\delta \w_b\)

ヤコビアン

観測方程式

各種センサをカルマンフィルタで使う方法

IMU

加速度の計測値 ＝ 機体の加速度 + 重力加速度 + バイアス + ノイズ

角速度の計測値 = 機体の角速度 + バイアス + ノイズ

GPS

位置：緯度・経度・高度

速度：NED座標系での速度

姿勢：

カメラオドメトリ

速度・角速度：前のフレームから現在のフレームへの移動量を、機体座標系で出してきます