GNSSとINSのセンサフュージョンによる自己位置推定

単なるリポジトリの紹介です。
https://github.com/zwire/gnss_ins_fusion

やりたいこと

ザコGNSSとかザコINSとか，安価なセンサでナビゲーションしたいときとかありますよね。
そこで，不確かな情報をフィルタリングして尤もらしい状態を推定する手法をとりました。
カルマンフィルタなど自体は確立された一般的な手法ですが，実装は「これを使っておけば間違いない！」というものがわからなかったので，イイ感じに作ってみようと思いました。フレームワークやモデリングとは疎な単体ライブラリを。ということです。

リポジトリ解説

ROS2のパッケージにしていますが，main文はデモプログラムなのでちゃんと使う場合はsequential_bayesian_filter.hだけをコピーして各人の環境で使ってください。このヘッダライブラリが本稿の肝になります。

本ライブラリの役割はフィルタアルゴリズムのソルバーで，初期化後に行列(変数)や式(コールバック)を登録してもらうことで自由なモデル設計に対応できるように以下のクラスを提供しています。

• Extended Kalman Filter (EKF)
• Unscented Kalman Filter (UKF)
• Particle Filter (PF)

理論的なわかりやすい説明は巷にたくさんありますので割愛します。

実装として，これらはいずれも統計量から「予測」された内部状態を観測値で「更新」するという本質的に同じことをしていますので，同一のインターフェース(SequentialBayesianFilter)を継承して振る舞いを統一しています。

デモプログラムでは以下に述べるモデルでLocalizationをしてみます。
内部状態(位置・方位)をINSの入力(速度・角速度)で予測し，GNSSの観測(位置)で更新するという流れですね。

(例) モデル

位置と方位だけで記述する一番単純なやつです。

$$状態ベクトル: \bm{x}=\begin{bmatrix}x&y&\theta\end{bmatrix}^T$$

$$観測ベクトル: \bm{y}=\begin{bmatrix}x&y\end{bmatrix}^T$$

$$制御ベクトル: \bm{u}=\begin{bmatrix}v_x&v_y&\omega\end{bmatrix}^T$$

$$遷移方程式: \bm{x}_t=f(\bm{x}_{t-1}, \bm{u}_t)=\bm{x}_{t-1}+\begin{bmatrix}v_{xt}\cos\theta_{t-1}-v_{yt}\sin\theta_{t-1}\\v_{xt}\sin\theta_{t-1}+v_{yt}\cos\theta_{t-1}\\\omega_t\end{bmatrix}\Delta t$$

$$観測方程式: \bm{y}_t=h(\bm{x}_t)=\begin{bmatrix}1&0&0\\0&1&0\end{bmatrix}\bm{x}_t$$

この状態遷移が三角関数を伴い非線形なので，仰々しいフィルタの出番なわけです。
UKFとPFはこれらの式を登録しておくだけで解いてくれます。
EKFの場合は遷移方程式をそのときの状態ベクトルで微分したヤコビアンを作っておきます。

$$\left. \frac{\partial f}{\partial \bm{x}} \right|_{\hat{\bm{x}}_{\left. t-1 \right|t-1}, \bm{u}_t}=\begin{bmatrix}1&0&(-v_{xt}\sin\theta_{t-1}-v_{yt}\cos\theta_{t-1})\Delta t\\0&1&(+v_{xt}\cos\theta_{t-1}+v_{yt}\sin\theta_{t-1})\Delta t\\0&0&1\end{bmatrix}$$

これを遷移行列Aに入れておけばOKです。

ちなみにEKFで非線形な遷移/観測方程式とヤコビアンを作らない場合は普通の線形カルマンフィルタと全く同じ式になります。線形でやりたい場合もEKFクラスを使ってください。

実験

適当なbagデータに適当なノイズとバイアスを意図的に加え，フィルタがロバストに機能するのかを確かめました(時計回りに一周走行)。図はUKFの結果で，黒線が観測に適当なノイズを加えたもの，赤線がアルゴリズムの出力です。