Kanade Electronicskanaelecom
🧭

カルマンフィルタ

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

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

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

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

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

newcommand{upright("rot")}{upright(upright("Rot"))} newcommand{w}{omega}

カルマンフィルタの基礎

  • 状態方程式: x = Fx + Gw, 𝑤∼𝑁(0,𝑄)
    • 𝑥: 状態
    • 𝑃: 状態の共分散行列
    • 𝐹: 状態遷移モデル
    • 𝑤: ホワイトノイズ
    • 𝑄: ホワイトノイズの共分散行列
    • 𝐺: ノイズモデル
  • 観測方程式: 𝑧=𝐻𝑥+𝑣, 𝑣∼𝑁(0,𝑅)
    • 𝑧: 観測値
    • 𝐻: 観測行列
    • 𝑣: 観測ノイズ
    • 𝑅: 観測ノイズの共分散行列

予測

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

𝑥𝑡+1|𝑡=𝐹𝑥𝑡|𝑡

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

𝑃𝑡+1|𝑡=𝐹𝑃𝑡|𝑡𝐹𝑇+𝐺𝑄𝐺𝑇

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

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

𝑥𝑡|𝑡→𝑥𝑡+1|𝑡→𝑥𝑡+2|𝑡
𝑃𝑡|𝑡⪯𝑃𝑡+1|𝑡⪯𝑃𝑡+2|𝑡

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

観測

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

観測残差を求める。

𝑒𝑡=𝑧𝑡−𝐻𝑥𝑡|𝑡−1

観測値と、予測されていた観測値の差を表している。

観測残差の分散を求める。

S_t = R + HP_(t|t-1)H^T

第一項はセンサのノイズに起因する分散で、第二項は予測誤差に起因する分散である。

カルマンゲインを求める。

K_t = P_(t|t-1)H^TS_t

観測値がどの程度信頼できるかを表している。

更新

カルマンゲインを使って、状態の推定値と、推定値の分散を更新する。

𝑥𝑡|𝑡=𝑥𝑘|𝑘−1+𝐾𝑡𝑒𝑡

予測値と観測値の「加重平均」をとることで、最も確からしい状態の推定値が得られる。

P_(t|t) = (I-K_tH)P_(t|t-1)

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

状態方程式

真の状態 𝑥𝑡 に対する状態方程式を 𝑓 とする。

̇𝑥𝑡=𝑓(𝑥𝑡)

誤差状態カルマンフィルタでは、真の状態 𝑥𝑡 をノミナル状態 𝑥 と誤差状態 𝛿𝑥 に分解する。

𝑥𝑡=𝑥⊕𝛿𝑥

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

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

𝑓(𝑥𝑡)=𝑓(𝑥⊕𝛿𝑥)=𝑓(𝑥)⊕𝑓𝑒(𝑥,𝛿𝑥)

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

̇𝑥=𝑓(𝑥)

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

̇𝛿𝑥=𝑓𝑒(𝑥,𝛿𝑥)

誤差状態方程式は、𝑂(𝛿𝑥2)→0 とみなして線形化できる。

観測方程式

𝑧=ℎ(𝑥𝑡)=ℎ(𝑥⊕𝛿𝑥)

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

𝐻≔𝜕ℎ𝜕𝛿𝑥|𝑥

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

𝜕ℎ𝜕𝛿𝑥|𝑥=𝜕ℎ𝜕𝑥𝑡|𝑥𝜕𝑥𝑡𝜕𝛿𝑥|𝑥≕𝐻𝑥𝑋𝛿𝑥

𝐻𝑥 は観測方程式次第、つまりセンサ次第だが、𝑋𝛿𝑥 はモデルの状態ベクトルから自動的に定まる。

状態方程式

状態𝑥̇𝑥𝛿𝑥̇𝛿𝑥
位置𝑝̇𝑝=𝑣𝛿𝑝̇𝛿𝑝=𝛿𝑣
姿勢𝑞̇𝑞=12𝑞𝑤𝛿𝜃̇𝛿𝜃=rot(𝛿𝜃)rot(𝑞)(𝑤+𝛿𝑤)
速度𝑣̇𝑣=rot(𝑞)𝑎𝛿𝑣̇𝛿𝑣=rot(𝛿𝜃)rot(𝑞)(𝑎+𝛿𝑎)
加速度𝑎𝛿𝑎
加速度センサバイアス𝑎𝑏𝛿𝑎𝑏
角速度𝑤𝛿𝑤
角速度センサバイアス𝑤𝑏𝛿𝑤𝑏

ヤコビアン

観測方程式

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

IMU

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

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

GPS

位置：緯度・経度・高度

速度：NED座標系での速度

姿勢：

カメラオドメトリ

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