マルチコプターの拡張カルマンフィルタによる9DOFセンサを用いたジャイロバイアス推定と姿勢推定

アイキャッチ

はじめに

マルチコプターの飛行制御プログラムを一から構築することを目指しています。その中で、ジャイロ、加速度計、地磁気センサが入っている9DOFセンサを用いてマルチコプターの姿勢（方向）を推定する方法についてまとめ、ソースコードに落とし込むために有用な資料となることを目指しています。

本記事の内容は、以下のリンクに示されている論文「動加速度環境下における姿勢推定アルゴリズムの研究」に記載されている事を実際のマルチコプターに適用するために、読みくだした解説文となっています。原著論文が圧倒的に詳しく、わかりやすいと思いますのでそちらを一度ご覧になることをお勧めいたします。

素晴らしい論文にこの場を借りて、感謝いたします。

www.jstage.jst.go.jp

なお、姿勢という言葉はロボット工学等では位置と方向を合わせたものとして扱います。したがって、以下ではマルチコプターの姿勢のことを方向と言う用語で統一します。

はじめに
方向の表し方
オイラー角とクォータニオン
方向推定に関するセンサ
拡張カルマンフィルタによる統合
クォータニオンに関する最低限のこと
座標空間
- 慣性空間
- 機体座標空間
回転運動に関するキネマティクス
ジャイロ
- ジャイロの誤差
- ジャイロによる計測
バイアスのダイナミクス
加速度計と地磁気センサ
拡張カルマンフィルタアルゴリズム
マルチコプターの方向推定への拡張カルマンフィルタの適用
そして、コーディングへ・・・
おわりに

方向の表し方

物体の方向を表す主な手段は

回転行列（方向余弦行列）表現
オイラー角表現
クォータニオン表現

情報量に関しては

回転行列が９
オイラー角が３
クォータニオンが4

さらに、回転行列（方向余弦行列）はオイラー角、クォータニオンどちらからでも計算で求めることができるのでマルチコプターの方向の推定はオイラー角とクォータニオンの推定の二つに分かれます。

オイラー角とクォータニオン

以上から、マルチコプターの方向を推定するということは、オイラー角又は、クォータニオンを推定することとなります。

オイラー角は座標軸を回転軸とする回転角度で方向を表す方法なので、直感的でわかりやすく情報量も３と３つの方法の中では最も少なく良いのですが、キネマティクスの計算において0割が起きる特異点が現れてしまう欠点があります。

クォータニオンは複素数の拡張で、４つの量で方向を表します。当初は飛翔体業界で多用されていました。

両者を比較してみます。

オイラー角については

オイラー角は直感的に理解しやすい
オイラー角は情報量３と最も少ない
オイラー角は方向計算過程に特異点が存在する

クォータニオンについては

クォータニオンは軸周りの回転で表す
クォータニオンの情報量は４
クォータニオンは方向計算過程に特異点を持たない

主な理由として特異点を持たない理由で、クォータニオンを用いることが多くなってきています。

方向推定に関するセンサ

マルチコプターやロボットの方向推定に用いられるセンサは主に次のものです

ジャイロ
加速度計
地磁気センサ

さらに、Lider、カメラ、距離計などさまざまな手段を用いて、位置と方向の推定を行うことができます。

本記事では、上記の３つのセンサ情報を用いた方向推定について取り扱います。

ジャイロによる角度の推定

ジャイロは角速度を取得するセンサです。従って方向を表すには角速度を積分して求める必要があります。

積分をするため、角速度情報にバイアスが含まれていると時間とともに積分結果が増大することになり角度推定に使う際には何らかの方法で積分をリセットする必要があります。

加速度計による角度の推定

加速度計はその名の通り、加速度を取得するセンサーです。しかし、地球上では常に下向きの重力を検知することができるので重力の方向を基準として角度を推定することができます。

ただし、加速運動をしている際は、加速方向に加速度を検知しますから方向センサとしては、そのままでは正しく機能しません。

地磁気による角度の推定

地磁気センサがあると、磁場の方向を検知できますので、原理的には磁北の方向が３次元的にわかるので角度を推定できます。磁石や金属があると場所によって影響を受けるので、厳密にはその様なことも考慮した上で方向がわかります。

拡張カルマンフィルタによる統合

この様に複数の手段でマルチコプターの方向に関する情報が得られるわけですが、これらの情報を統合して、方向を推定するアルゴリズムが拡張カルマンフィルタになります。

拡張カルマンフィルタはカルマンフィルタが線形な状態方程式、観測方程式によって表されたシステムにしか適用できないため、非線形なモデルで表されたシステムに対しては、動作点周りで線形化を施してカルマンフィルタを適用するというものです。

カルマンフィルタは観測していない状態も推定でき、かつ、ノイズに汚された信号も陽に扱うことができ、その情報の確からしさを共分散として出力されます。

クォータニオンに関する最低限のこと

クォータニオンに関する最低限の知識について触れます

二つのクォータニオンの表記方法

最初の要素がスカラー部の表記

我々が、推定したい状態量はマルチコプターの方向を表すクォータニオンです。クォータニオンは４つの数値を持つ量です。４次元ベクトルで次の様にチルダを付けて表記されます。

$\begin{eqnarray} \tilde{\boldsymbol{q}}= \begin{bmatrix} q_0&q_1&q_2&q_3 \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

ここで $q_0$ は回転角に関する（回転角そのものではない）スカラー値で、残りの $q_1$ 、 $q_2$ 、 $q_3$ は回転軸を表す単位ベクトルの成分です。

添字に番号を使わなない表記方法を用いたのが以下です。

$\begin{eqnarray} \tilde{\boldsymbol{q}}= \begin{bmatrix} q_w&q_x&q_y&q_z \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

最後の要素がスカラー部の表記

実は、以上の取り方の他に

$\begin{eqnarray} \tilde{\boldsymbol{q}}= \begin{bmatrix} q_1&q_2&q_3&q_4 \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

と記述し、 $q_1$ 、 $q_2$ 、 $q_3$ は先ほどの回転軸を表す単位ベクトルの成分です。 $q_4$ は回転角に関する（回転角そのものではない）スカラー値とする表記方法があります。

こちらも、添字に番号を使わない方法で表記すると以下の様になります。

$\begin{eqnarray} \tilde{\boldsymbol{q}}= \begin{bmatrix} q_x&q_y&q_z&q_w \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

どちらかで記述するかは使用者が決めます。表記方法により計算結果の現れ方が違ってきますので決めた表記方法で統一して使って行くことが重要です。

本記事では、前者の方法で計算を行いたいと思います。

さらに、ベクトルの部分を

$\begin{eqnarray} {\boldsymbol{q}}= \begin{bmatrix} q_1&q_2&q_3 \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

と表すと。

クォータニオンは

$\begin{eqnarray} \tilde{\boldsymbol{q}}=q_0 + \boldsymbol{q} \\ \\ \end{eqnarray}$

と表記されます。

また、クォータニオンは複素数を拡張したものでもあり、基底ベクトル ${\boldsymbol{i}}$ 、 ${\boldsymbol{j}}$ 、 ${\boldsymbol{k}}$ を用いて

$\begin{eqnarray} \tilde{\boldsymbol{q}}=q_0 + q_1 \boldsymbol{i} + q_2 \boldsymbol{j} + q_3 \boldsymbol{k} \\ \\ \end{eqnarray}$

の様に表すことができます。

ここで、基底ベクトルは虚数単位と似た性質を持っているとして以下の様な式が成り立つものとします。

$\begin{eqnarray} \boldsymbol{i}^2 =\boldsymbol{j}^2=\boldsymbol{k}^2=-1\\ \boldsymbol{i}\boldsymbol{j}=-\boldsymbol{j}\boldsymbol{i}=\boldsymbol{k} \\ \boldsymbol{j}\boldsymbol{k}=-\boldsymbol{k}\boldsymbol{j}=\boldsymbol{i} \\ \boldsymbol{k}\boldsymbol{i}=-\boldsymbol{i}\boldsymbol{k}=\boldsymbol{j} \\ \\ \\ \end{eqnarray}$

共役クォータニオン

複素数と同様にベクトル部分の符号が反転したものを共役クォータニオンと呼びます。

$\begin{eqnarray} \tilde{\boldsymbol{q}}^*=q_0 - q_1 \boldsymbol{i} - q_2 \boldsymbol{j} - q_3 \boldsymbol{k} \\ \\ \end{eqnarray}$

ノルム

以下をクォータニオンのノルムとします

$\begin{eqnarray} \| \tilde{\boldsymbol{q}} \|= \sqrt{q_0^2+q_1^2+q_2^2+q_3^2}=\sqrt{\tilde{\boldsymbol{q}}\tilde{\boldsymbol{q}}^*}=\sqrt{\tilde{\boldsymbol{q}}^*\tilde{\boldsymbol{q}}} \\ \\ \end{eqnarray}$

逆クォータニオン

以下を逆クォータニオンと呼びます

$\begin{eqnarray} \tilde{\boldsymbol{q}}^{-1}=\frac{\tilde{\boldsymbol{q}}^*}{\| \tilde{\boldsymbol{q}} \|^2} \\ \\ \end{eqnarray}$

クォータニオンの積

クォータニオンについての計算を展開する上で、知っておかなければならない最低限は積についての知識だと思います。これを知っていなければ、以後の計算を展開することができないので説明します。

ベクトルの二つの積

クォータニオンの積の前にベクトルの積である内積（スカラー積）（Dot product）と外積（ベクトル積）(Cross product)について説明します。クォータニオンの積はこれらの組み合わせになります。

内積（スカラー積）（Dot product）

n次元のベクトル ${\boldsymbol{a}}$ と ${\boldsymbol{b}}$ の内積は以下の様にドット（・）を用いて表します。結果はスカラーになります。ベクトルをn行１列の行列だとみなし行列の積として捉えると、転置したものとの積となりスカラー（１x１の行列）になるのもわかります。

$\begin{eqnarray} {\boldsymbol{a}}\cdot {\boldsymbol{b}}= {\boldsymbol{a}}^\mathsf{T}{\boldsymbol{b}} \\ \end{eqnarray}$

外積（ベクトル積）(Cross product)

n次元のベクトル ${\boldsymbol{a}}$ と ${\boldsymbol{b}}$ の外積は以下の様にクロス（×）を用いて表します。結果はベクトルになります。各ベクトルに共通するそれらの基底ベクトルを ${\boldsymbol{i}}$ 、 ${\boldsymbol{j}}$ 、 ${\boldsymbol{k}}$ とすると、外積の計算は行列式の表記で計算ルールを書くこともできます。 $a_x$ 等はそれぞれのベクトルの成分です。

$\begin{eqnarray} {\boldsymbol{a}}\times{\boldsymbol{b}}= \begin{vmatrix} {\boldsymbol{i}}&{\boldsymbol{j}}&{\boldsymbol{k}}\\ a_x&a_y&a_z\\ b_x&b_y&b_z \end{vmatrix} \\ \end{eqnarray}$

ベクトル同士の３つ目の積〜ハミルトン積

ここでベクトル同士の３つ目の積を新たに導入します。ここではドットもクロスも使わずに積を表現したものをハミルトン積として次の様に定義します。

$\begin{eqnarray} {\boldsymbol{a}}{\boldsymbol{b}}:={\boldsymbol{a}}\cdot {\boldsymbol{b}} +{\boldsymbol{a}}\times {\boldsymbol{b}} \end{eqnarray}$

これは、積の結果の第一項が内積でスカラー、第２項が外積でベクトルとなり、これまで述べてきたクォータニオンの形をしていることがわかります。

回転を表すクォータニオン

以下の特別なクォータニオンは回転を表します

$\begin{eqnarray} \tilde{\boldsymbol{q}}=\cos \frac{\theta}{2} + \boldsymbol{n} \sin \frac{\theta}{2} \\ \\ \end{eqnarray}$

${\boldsymbol{n}}$ は回転軸を表す単位ベクトルになります。また $\theta$ が回転角となり、方向は右ねじの法則に従います。

また、ノルムは１となります。

そして、重要な性質にノルムが１の場合は共役クォータニオンが逆クォータニオンになります。

これを用いて、ベクトルの回転と座標変換を行えます。

ベクトルの回転

$\begin{eqnarray} \boldsymbol{r}^\prime=\tilde{\boldsymbol{q}}\boldsymbol{r}\tilde{\boldsymbol{q}}^* \\ \\ \end{eqnarray}$

行列演算形式で表すと

$\begin{eqnarray} {\boldsymbol{r}}^\prime= \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1 q_2 -q_0 q_3) & 2(q_1 q_3 + q_0 q_2) \\ 2(q_1 q_2+q_0 q_3) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2 q_3 - q_0 q_1)\\ 2(q_1 q_3-q_0 q_2) & 2(q_2 q_3+q_0 q_1) & q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix}{\boldsymbol{r}} \\ \\ \end{eqnarray}$

座標の回転（座標変換）

$\begin{eqnarray} \boldsymbol{r}^\prime=\tilde{\boldsymbol{q}}^* \boldsymbol{r}\tilde{\boldsymbol{q}} \\ \\ \end{eqnarray}$

行列演算形式で表すと

$\begin{eqnarray} {\boldsymbol{r}}^\prime= \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1 q_2+q_0 q_3) & 2(q_1 q_3-q_0 q_2)\\ 2(q_1 q_2 -q_0 q_3) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2 q_3+q_0 q_1)\\ 2(q_1 q_3 + q_0 q_2) & 2(q_2 q_3 - q_0 q_1) & q_0^2 - q_1^2 - q_2^2 + q_3^2\\ \end{bmatrix}{\boldsymbol{r}} \\ \\ \end{eqnarray}$

この行列は慣性空間から機体座標空間への座標変換行列として使用するので、次の様に定義しておきます。

$\begin{eqnarray} {\boldsymbol{E}}= \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1 q_2+q_0 q_3) & 2(q_1 q_3-q_0 q_2)\\ 2(q_1 q_2 -q_0 q_3) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2 q_3+q_0 q_1)\\ 2(q_1 q_3 + q_0 q_2) & 2(q_2 q_3 - q_0 q_1) & q_0^2 - q_1^2 - q_2^2 + q_3^2\\ \end{bmatrix} \\ \\ \end{eqnarray}$

座標空間

マルチコプターのクォータニオンを推定する際に考慮する座標空間について触れておきます。

本記事では座標空間は右手系を採用します。

慣性空間

地球とか地上や部屋に固定された基準になる座標空間をここでは慣性空間と呼称することにします。

機体座標空間の座標軸は

x軸が磁北の方向
y軸がx軸とy軸に垂直（東の向き）
z軸が重力方向（下向き）

をそれぞれ向くものとします。

この軸の取り方をNED座標（North-East-Down）と呼ぶこともあります。

慣性空間を表す添字として $i$ を用います。

機体座標空間

マルチコプターに固定された座標空間を機体座標空間と呼称します。

機体座標空間の座標軸は

x軸が機体の前方向
y軸が機体の右方向
z軸が機体の下方向

をそれぞれ向くものとします。

機体座標を表す添字として $b$ を用います。

回転運動に関するキネマティクス

回転運動に関するキネマティクス（位置の微分は速度の様なこと）は、クォータニオンの微分により、以下の様に表されます。

まず、機体座標の角速度 ${\boldsymbol{\omega}}_b$ を次の様に定義しておきます。

$\begin{eqnarray} {\boldsymbol{\omega}}_b= \begin{bmatrix} p&q&r \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

すると、キネマティクスは以下の様に表すことができます。

$\begin{eqnarray} \begin{bmatrix} \dot{q_0}\\ \dot{q_1}\\ \dot{q_2}\\ \dot{q_3} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r\\ p & 0 & r & -q\\ q & -r & 0 & p\\ r & q & -p & 0 \end{bmatrix} \begin{bmatrix} q_0\\ q_1\\ q_2\\ q_3 \end{bmatrix} \\ \\ \end{eqnarray}$

ジャイロ

ジャイロの誤差

ジャイロの計測結果に含まれる誤差は

バイアス誤差
スケールファクタ誤差
ミスアライメント誤差
ノイズ

の４つとなります

バイアス誤差

バイアス誤差とは測定値にのる直流成分のことで、このため積分するとドリフトすることになり、推定して取り除くことが望ましいのですが、動作中に変動してしまい事前の取り除くことは困難です。本記事では、バイアス誤差の推定を試みています。

スケールファクタ誤差

スケールファクタ誤差はゲインに誤差があるということで１のものが0.5になって出てしまうということで、校正作業で取り除くことができます。

ミスアライメント誤差

ミスアライメント誤差とは、取り付けによる誤差を言う。位置誤差や傾き等の分だけ測定値に誤差が生じる。これも事前に校正作業で取り除くことができます。

ノイズ

ノイズに関しては予測不可能なため事前に取り除くことはできません。

したがって計測誤差はバイアスとノイズが問題となり、特にバイアスの推定が重要となってきます。

ジャイロによる計測

ジャイロで計測した角速度 ${\boldsymbol{\omega}}_m$ を次の様に定義します。

$\begin{eqnarray} {\boldsymbol{\omega}}_m= \begin{bmatrix} p_m&q_m&r_m \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

これは、真の角速度 ${\boldsymbol{\omega}}_b$ にバイアス ${\delta \boldsymbol{\omega}}$ とノイズ ${\boldsymbol{w}}_m$ が加わったものと考えるとすると、以下の様になります。

$\begin{eqnarray} {\boldsymbol{\omega}}_m={\boldsymbol{\omega}}_b + \delta {\boldsymbol{\omega}}+ {\boldsymbol{w}}_m \\ \\ \end{eqnarray}$

バイアス成分を次の様に表します。

$\begin{eqnarray} \delta{\boldsymbol{\omega}}= \begin{bmatrix} \delta p&\delta q&\delta r \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

${\boldsymbol{w}}$ を白色ノイズで成分は以下とします。

$\begin{eqnarray} {\boldsymbol{w}}_m= \begin{bmatrix} {w_m}_p&{w_m}_q&{w_m}_r \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

書き下すと以下になります。

$\begin{eqnarray} \begin{bmatrix} p_m\\q_m\\r_m \end{bmatrix} = \begin{bmatrix} p\\ q\\ r \end{bmatrix} + \begin{bmatrix} \delta p\\ \delta q\\ \delta r \end{bmatrix} + \begin{bmatrix} {w_m}_p\\ {w_m}_q\\ {w_m}_r \end{bmatrix} \\ \\ \end{eqnarray}$

逆に真の角速度は計測値から計測誤差を引いたものとして、以下の様に表すことができます。

$\begin{eqnarray} \begin{bmatrix} p\\q\\r \end{bmatrix} = \begin{bmatrix} p_m\\ q_m\\ r_m \end{bmatrix} - \begin{bmatrix} \delta p\\ \delta q\\ \delta r \end{bmatrix} - \begin{bmatrix} {w_m}_p\\ {w_m}_q\\ {w_m}_r \end{bmatrix} \\ \\ \end{eqnarray}$

バイアスのダイナミクス

パラメータ行列として次の様なものを作っておきます。

$\begin{eqnarray} {\boldsymbol{\beta}}= \begin{bmatrix} -\beta_x & 0 & 0 \\ 0 & -\beta_y & 0 \\ 0 & 0 & -\beta_z \end{bmatrix} \\ \\ \end{eqnarray}$

すると、バイアス成分のダイナミクスは次の様に記述できることが知られています。

$\begin{eqnarray} \dot{{\delta} {\boldsymbol{\omega}}}={\boldsymbol{\beta}} {\boldsymbol{\delta \omega}} + {\boldsymbol{w}} \\ \\ \end{eqnarray}$

バイアスノイズを新たに次の様に定義します。（計測ノイズと異なるので注意してください。）

$\begin{eqnarray} {\boldsymbol{w}}= \begin{bmatrix} w_p & w_q & w_r \end{bmatrix} \end{eqnarray}$

バイアスモデルを書き下すと

$\begin{eqnarray} \begin{bmatrix} \dot{\delta p}\\ \dot{\delta q}\\ \dot{\delta r} \end{bmatrix} = \begin{bmatrix} -\beta_x & 0 & 0 \\ 0 & -\beta_y & 0 \\ 0 & 0 & -\beta_z \end{bmatrix} \begin{bmatrix} \delta p\\ \delta q\\ \delta r \end{bmatrix} + \begin{bmatrix} w_p\\ w_q\\ w_r \end{bmatrix} \\ \\ \end{eqnarray}$

$\beta_x$ 、 $\beta_y$ 、 $\beta_z$ については事前に静かな状態で測定実験をして、その結果から求めます。

加速度計と地磁気センサ

3軸の加速度計と３軸の地磁気センサはそれぞれ重力と地磁気を検知できます。静的状態ではそれぞれのセンサがマルチコプターの方向に応じて、どの様な検知結果を得られるのかを考察してみたいと思います。

重力ベクトル

慣性空間において重力は常に鉛直下向きなので、重力ベクトル ${\boldsymbol{g}}_i$ は以下の様になります。

$\begin{eqnarray} {\boldsymbol{g}}_i= \begin{bmatrix} 0&0&g \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

地磁気ベクトル

地球の磁力線は、赤道付近は地面にほぼ平行で、極に近づくほど傾いているのが一般的です。

慣性空間に対しての地磁気ベクトル ${\boldsymbol{m}}_i$ は以下の様になります。

$\begin{eqnarray} {\boldsymbol{m}}_i= \begin{bmatrix} m_n&m_e&m_d \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

NED座標系で考える際には、北の方向をx軸に取るとすると、 ${m_{e}}$ は０とすることが一般的の様です。

ここでは、マルチコプタの初期の置かれた状態で、機体座標と慣性座標が一致する様にしたいので、地磁気ベクトルの各成分が０ではない一般的な場合を取り扱っていきたいと思います。

重力と地磁気の座標変換

慣性空間で定義された重力 ${\boldsymbol{g}}_i$ を機体座標空間へ座標変換し ${\boldsymbol{g}}_b$ とします。

$\begin{eqnarray} {\boldsymbol{g}}_b={\boldsymbol{E}}{\boldsymbol{g}}_i \\ \\ \end{eqnarray}$

重力ベクトルは二つの要素が０なので展開しても、それほど複雑ではなく展開した結果は以下となります。

$\begin{eqnarray} \begin{bmatrix} a_x\\ a_y\\ a_z\\ \end{bmatrix} &=& \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1 q_2+q_0 q_3) & 2(q_1 q_3-q_0 q_2)\\ 2(q_1 q_2 -q_0 q_3) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2 q_3+q_0 q_1)\\ 2(q_1 q_3 + q_0 q_2) & 2(q_2 q_3 - q_0 q_1) & q_0^2 - q_1^2 - q_2^2 + q_3^2\\ \end{bmatrix} \begin{bmatrix} 0\\ 0\\ g \end{bmatrix}\\ &=& \begin{bmatrix} 2(q_1 q_3-q_0 q_2)g\\ 2(q_2 q_3+q_0 q_1)g\\ (q_0^2 - q_1^2 - q_2^2 + q_3^2)g\\ \end{bmatrix} \\ \\ \end{eqnarray}$

同様に、慣性空間で定義された地磁気 ${\boldsymbol{m}}_i$ を機体座標空間へ座標変換し ${\boldsymbol{m}}_b$ とします。

$\begin{eqnarray} {\boldsymbol{m}}_b={\boldsymbol{E}}{\boldsymbol{m}}_i \\ \\ \end{eqnarray}$

地磁気についても展開すると

$\begin{eqnarray} \begin{bmatrix} m_x\\ m_y\\ m_z \end{bmatrix} &=& \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1 q_2+q_0 q_3) & 2(q_1 q_3-q_0 q_2)\\ 2(q_1 q_2 -q_0 q_3) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2 q_3+q_0 q_1)\\ 2(q_1 q_3 + q_0 q_2) & 2(q_2 q_3 - q_0 q_1) & q_0^2 - q_1^2 - q_2^2 + q_3^2\\ \end{bmatrix} \begin{bmatrix} m_n\\ m_e\\ m_d \end{bmatrix}\\ &=& \begin{bmatrix} (q_0^2+q_1^2-q_2^2-q_3^2) m_n +2(q_1 q_2+q_0 q_3) m_e + 2(q_1 q_3-q_0 q_2) m_d\\ 2(q_1 q_2 -q_0 q_3) m_n + (q_0^2 - q_1^2 + q_2^2 - q_3^2) m_e + 2(q_2 q_3+q_0 q_1) m_d\\ 2(q_1 q_3 + q_0 q_2) m_n + 2(q_2 q_3 - q_0 q_1) m_e + (q_0^2 - q_1^2 - q_2^2 + q_3^2) m_d\\ \end{bmatrix} \\ \\ \end{eqnarray}$

加速度計と地磁気センサによる計測

機体に取り付けられている、加速度計と地磁気センサは先に述べた、重力と地磁気に誤差が加えられた結果を出力します。

計測値を ${\boldsymbol{a}}_m$ 、 ${\boldsymbol{m}}_m$ とし、計測誤差を ${\boldsymbol{\delta a}}$ 、 ${\boldsymbol{\delta m}}$ とすると以下の様に表記できます。

$\begin{eqnarray} {\boldsymbol{a}}_m&=&{\boldsymbol{g}}_b + {\boldsymbol{\delta a}} \\ {\boldsymbol{m}}_m&=&{\boldsymbol{m}}_b + {\boldsymbol{\delta m}} \\ \end{eqnarray}$

誤差の成分は白色ノイズと、動きによる加速度や地磁気の変化などになります。それらの成分を以下の様にします。

$\begin{eqnarray} {\boldsymbol{\delta a}}&=& \begin{bmatrix} \delta a_x & \delta a_y & \delta a_z \end{bmatrix}^\mathsf{T} \\ {\boldsymbol{\delta m}} &=& \begin{bmatrix} \delta m_x & \delta m_y & \delta m_z \end{bmatrix}^\mathsf{T} \\ \end{eqnarray}$

重力と地磁気の観測方程式

後ほど、拡張カルマンフィルタを用いて方向の推定を行う際に、システムの観測量として加速度を地磁気を使用します。そこで、加速度と観測量をまとめた観測量ベクトル ${\boldsymbol{z}}$ を次の様にします。

$\begin{eqnarray} {\boldsymbol{z}}= \begin{bmatrix} a_x&a_y&a_z&m_x&m_y&m_z \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

観測方程式はまとめて書くと次の様になります。

$\begin{eqnarray} \begin{bmatrix} a_x\\ a_y\\ a_z\\ m_x\\ m_y\\ m_z \end{bmatrix} &=& \begin{bmatrix} 2(q_1 q_3-q_0 q_2)g\\ 2(q_2 q_3+q_0 q_1)g\\ (q_0^2 - q_1^2 - q_2^2 + q_3^2)g\\ (q_0^2+q_1^2-q_2^2-q_3^2) m_n +2(q_1 q_2+q_0 q_3) m_e + 2(q_1 q_3-q_0 q_2) m_d\\ 2(q_1 q_2 -q_0 q_3) m_n + (q_0^2 - q_1^2 + q_2^2 - q_3^2) m_e + 2(q_2 q_3+q_0 q_1) m_d\\ 2(q_1 q_3 + q_0 q_2) m_n + 2(q_2 q_3 - q_0 q_1) m_e + (q_0^2 - q_1^2 - q_2^2 + q_3^2) m_d\\\end{bmatrix} \\&+& \begin{bmatrix} \delta a_x\\ \delta a_y\\ \delta a_z\\ \delta m_x\\ \delta m_y\\ \delta m_z \end{bmatrix} \\ \\ \end{eqnarray}$

拡張カルマンフィルタアルゴリズム

拡張カルマンフィルタのアルゴリズムのあらましをご紹介します。

拡張カルマンフィルタは非線形な状態方程式と観測方程式を、動作点で線形化しつつカルマンフィルタを使っていくアルゴリズムです。

拡張カルマンフィルタにおいて推定されるkステップ目の推定量は以下です。

状態の推定値　 $\hat{\boldsymbol{x}}_k$
誤差共分散の推定値　 $\hat{\boldsymbol{P}}_k$

計算にあたり必要とするもの

状態方程式 $\dot{\boldsymbol{x}}={\boldsymbol{f(x)}}+{\boldsymbol{G}}{\boldsymbol{w}}$
観測方程式 ${\boldsymbol{z}}={\boldsymbol{h(x)}}+{\boldsymbol{v}}$
システムノイズの共分散行列 ${\boldsymbol{Q}}$
観測ノイズの共分散行列 ${\boldsymbol{R}}$
状態と誤差共分散の初期値 ${\boldsymbol{x}_0}$ 、 ${\boldsymbol{P}_0}$

したごしらえ

状態方程式の離散化

カルマンフィルタも拡張カルマンフィルタも離散系のアルゴリズムとして用います。したがって、微分方程式（連続時間）で定義されている状態方程式を離散化する必要があります。最も簡単な方法としては、微分を短い時間 ${\Delta t}$ で近似したものを用いて、離散化することです。

まず、状態方程式において、微分を数値微分と置き換えると

$\begin{eqnarray} \frac{{\boldsymbol{x}}_{k+1} -{\boldsymbol{x}}_{k}}{\Delta t} = {\boldsymbol{f}({\boldsymbol{x}_k})} +{\boldsymbol{G}}{\boldsymbol{w}} \\ \\ \end{eqnarray}$

${\boldsymbol{x}}_{k+1}$ について整理すると

$\begin{eqnarray} {\boldsymbol{x}}_{k+1} = {\boldsymbol{x}}_{k}+{\boldsymbol{f}({\boldsymbol{x}_k})} {\Delta t} +{\boldsymbol{G}}{\boldsymbol{w}}{\Delta t} \\ \\ \end{eqnarray}$

ここで、次の様な置き換えを行うと（添字 $d$ はDiscrete（離散）の頭文字dです。）

$\begin{eqnarray} {\boldsymbol{f}_d \boldsymbol{(x}_k \boldsymbol{)}}={\boldsymbol{x}}_{k} + {\boldsymbol{f(x}_k \boldsymbol{)}} \Delta t \\ \\ \end{eqnarray}$

$\begin{eqnarray} {\boldsymbol{G}}_k={\boldsymbol{G}} \Delta t \\ \\ \end{eqnarray}$

離散化状態方程式は次の様に、見た目は離散化前と同じ様な式になります。

$\begin{eqnarray} {\boldsymbol{x}}_{k+1}={\boldsymbol{f}_d \boldsymbol{(x}_k \boldsymbol{)} }+{\boldsymbol{G}}_k {\boldsymbol{w}} \\ \\ \end{eqnarray}$

ヤコビアンの導出

ベクトル関数をベクトルで偏微分したものをヤコビアンと呼びます。ヤコビアンは行列になります。１次元で言えば接線の傾きに当たるものがヤコビアンです。

曲線のある場所での接線とその方程式を求める作業が線形化ですので、状態方程式と観測方程式のヤコビアンを求めることが線形化作業そのものになります。

状態方程式の ${\boldsymbol{f}_d}({\boldsymbol{x}_k})$ と観測方程式の ${\boldsymbol{h}}({\boldsymbol{x}}_k)$ を状態量ベクトルで編微分して以下の様に、それぞれを ${\boldsymbol{F}}({\boldsymbol{x}}_k)$ 、 ${\boldsymbol{H}}({\boldsymbol{x}}_k)$ とします。

$\begin{eqnarray} {\boldsymbol{F}}({\boldsymbol{x}}_k) =\frac{\partial \boldsymbol{f}_d}{\partial \boldsymbol{x}_k} ={\boldsymbol{I}}+\frac{\partial \boldsymbol{f}}{\partial \boldsymbol{x}_k} \Delta t \\ \\ \end{eqnarray}$

$\begin{eqnarray} {\boldsymbol{H}}({\boldsymbol{x}}_k) =\frac{\partial \boldsymbol{h}}{\partial \boldsymbol{x}_k} \\ \\ \end{eqnarray}$

特に ${\boldsymbol{F}}({\boldsymbol{x}}_k)$ については、離散化された状態方程式のヤコビアンなので注意してください。

ヤコビアンについて

ベクトルをベクトルで編微分すると行列になり、ヤコビアンと呼ばれています。具体例は次の様になります。

微分される関数ベクトルを次の様なものとします。

$\begin{eqnarray} {\boldsymbol{f}} = \begin{bmatrix} f_1 &f_2 & f_3 \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

微分するベクトルを次な様なものとします。

$\begin{eqnarray} {\boldsymbol{x}} = \begin{bmatrix} x_1 &x_2 & x_3 & x_4 \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

すると、ヤコビアンは次の様な行列になります。

$\begin{eqnarray} \frac{\partial \boldsymbol{f}}{\partial \boldsymbol{x}} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} &\frac{\partial f_1}{\partial x_3} &\frac{\partial f_1}{\partial x_4}\\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} &\frac{\partial f_2}{\partial x_3} &\frac{\partial f_2}{\partial x_4}\\ \frac{\partial f_3}{\partial x_1} & \frac{\partial f_3}{\partial x_2} &\frac{\partial f_3}{\partial x_3} &\frac{\partial f_3}{\partial x_4}\\ \end{bmatrix} \\ \\ \end{eqnarray}$

アルゴリズム

拡張カルマンフィルタは以下に示す、観測更新と予測更新を各ステップごとに繰り返していきます。

流れを大雑把に書くと次の様になります。

カルマンゲインを計算
観測値とカルマンゲインによって推定値を修正して推定値を得る
前のステップで得られた状態量と誤差共分散により、予測値を得る

一つのステップの中で、推定と予測の二つの部分に分られます。

ここでは、推定された値にはハット（＾）を、予測された値にはバー（ー）を記号の上につけることにします。

観測更新

観測方程式ヤコビアンの準備

$\begin{eqnarray} {\boldsymbol{H}}_k =\left. \frac{\partial \boldsymbol{h}}{\partial \boldsymbol{x}_k} \right|_{{\boldsymbol{x}}_k = \bar{\boldsymbol{x}}_k} \\ \\ \end{eqnarray}$

カルマンゲイン更新

$\begin{eqnarray} {\boldsymbol{K}}_k =\bar{\boldsymbol{P}}_k {{\boldsymbol{H}}_k}^\mathsf{T} {\left[ {{\boldsymbol{H}}_k} {\bar{\boldsymbol{P}}_k} {{\boldsymbol{H}}_k}^\mathsf{T} + {\boldsymbol{R}} \right]}^{-1} \\ \\ \end{eqnarray}$

状態推定

$\begin{eqnarray} \hat{\boldsymbol{x}}_k =\bar{\boldsymbol{x}}_{k} + {\boldsymbol{K}}_k \left[ {\boldsymbol{z}}_k - {\boldsymbol{h}}(\bar{\boldsymbol{x}}_k) \right] \\ \\ \end{eqnarray}$

誤差共分散推定

$\begin{eqnarray} \hat{\boldsymbol{P}}_k =\bar{\boldsymbol{P}}_{k} - {\boldsymbol{K}}_k {\boldsymbol{H}}_k \bar{\boldsymbol{P}}_{k} \\ \\ \end{eqnarray}$

予測更新

状態の予測

$\begin{eqnarray} \bar{\boldsymbol{x}}_{k+1} =\boldsymbol{f}_d ( \hat{\boldsymbol{x}}_{k} ) \\ \\ \end{eqnarray}$

状態方程式ヤコビアンの準備

$\begin{eqnarray} {\boldsymbol{F}_k} =\left. \frac{\partial \boldsymbol{f}_d}{\partial \boldsymbol{x}_k} \right|_{{\boldsymbol{x}}_k = \bar{\boldsymbol{x}}_k} ={\boldsymbol{I}}+\left. \frac{\partial \boldsymbol{f}}{\partial \boldsymbol{x}_k} \right|_{{\boldsymbol{x}}_k = \bar{\boldsymbol{x}}_k} \Delta t \\ \\ \end{eqnarray}$

共分散の予測

$\begin{eqnarray} \bar{\boldsymbol{P}}_{k+1} ={\boldsymbol{F}_k} \hat{\boldsymbol{P}}_{k} {{\boldsymbol{F}}_k}^\mathsf{T}+ {\boldsymbol{G}_k}{\boldsymbol{Q}}{\boldsymbol{G}_k}^\mathsf{T} \\ \\ \end{eqnarray}$

マルチコプターの方向推定への拡張カルマンフィルタの適用

推定する状態量

拡張カルマンフィルタを用いて推定する状態量 ${\boldsymbol{x}}$ は、以下の様に、クォータニオンとジャイロのバイアスになります。

$\begin{eqnarray} {\boldsymbol{x}}= \begin{bmatrix} q_0 & q_1 & q_2 & q_3 & \delta p & \delta q & \delta r \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

状態方程式

前述した通り、状態量はクォータニオンとジャイロバイアスなので、クォータニオンの微分を表す回転運動のキネマティクスとバイアスのダイナミクスが状態方程式となります。ここで再掲します。

回転運動のキネマティクス

角速度とジャイロにより観測量される角速度とバイアス誤差の関係

$\begin{eqnarray} \begin{bmatrix} p\\q\\r \end{bmatrix} = \begin{bmatrix} p_m\\ q_m\\ r_m \end{bmatrix} - \begin{bmatrix} \delta p\\ \delta q\\ \delta r \end{bmatrix} + \begin{bmatrix} w_{mp} \\ w_{mq}\\ w_{mr} \end{bmatrix} \\ \\ \end{eqnarray}$

ジャイロバイアスのダイナミクス

これらを展開して整理し、まとめて１本の状態方程式にすると以下となります。

$\begin{eqnarray} \begin{bmatrix} \dot{q_0}\\ \dot{q_1}\\ \dot{q_2}\\ \dot{q_3}\\ \dot{\delta p}\\ \dot{\delta q}\\ \dot{\delta r} \end{bmatrix} &=& \begin{bmatrix} -\frac{1}{2}(p_m-\delta p) q_1 -\frac{1}{2}(q_m-\delta q) q_2-\frac{1}{2}(r_m - \delta r) q_3\\ \frac{1}{2}(p_m-\delta p) q_0 +\frac{1}{2}(r_m - \delta r) q_2 -\frac{1}{2}(q_m-\delta q) q_3\\ \frac{1}{2}(q_m-\delta q) q_0 -\frac{1}{2}(r_m - \delta r) q_1 + \frac{1}{2}(p_m-\delta p) q_3\\ \frac{1}{2}(r_m - \delta r) q_0 +\frac{1}{2}(q_m-\delta q) q_1 -\frac{1}{2}(p_m-\delta p) q_2\\ -\beta_x \delta p\\ -\beta_y \delta q\\ -\beta_z \delta r \end{bmatrix} \\&+& \begin{bmatrix} -1&-1&-1&0&0&0\\ 1&-1&1&0&0&0\\ 1&1&-1&0&0&0\\ -1&1&1&0&0&0\\ 0&0&0&1&0&0\\ 0&0&0&0&1&0\\ 0&0&0&0&0&1 \end{bmatrix} \begin{bmatrix} w_{mp} \\ w_{mq}\\ w_{mr}\\ w_p\\ w_q\\ w_r \end{bmatrix} \\ \\ \end{eqnarray}$

状態方程式は次の形になっています。

$\begin{eqnarray} \dot{\boldsymbol{x}}={\boldsymbol{f(x)}}+{\boldsymbol{G}}{\boldsymbol{w}} \\ \\ \end{eqnarray}$

それぞれの要素を明示します。

$\begin{eqnarray} {\boldsymbol{f(x)}}= \begin{bmatrix} -\frac{1}{2}(p_m-\delta p) q_1 -\frac{1}{2}(q_m-\delta q) q_2-\frac{1}{2}(r_m - \delta r) q_3\\ \frac{1}{2}(p_m-\delta p) q_0 +\frac{1}{2}(r_m - \delta r) q_2 -\frac{1}{2}(q_m-\delta q) q_3\\ \frac{1}{2}(q_m-\delta q) q_0 -\frac{1}{2}(r_m - \delta r) q_1 + \frac{1}{2}(p_m-\delta p) q_3\\ \frac{1}{2}(r_m - \delta r) q_0 +\frac{1}{2}(q_m-\delta q) q_1 -\frac{1}{2}(p_m-\delta p) q_2\\ -\beta_x \delta p\\ -\beta_y \delta q\\ -\beta_z \delta r \end{bmatrix} \\ \\ \end{eqnarray}$

$\begin{eqnarray} {\boldsymbol{G}}= \begin{bmatrix} -1&-1&-1&0&0&0\\ 1&-1&1&0&0&0\\ 1&1&-1&0&0&0\\ -1&1&1&0&0&0\\ 0&0&0&1&0&0\\ 0&0&0&0&1&0\\ 0&0&0&0&0&1 \end{bmatrix} \\ \\ \end{eqnarray}$

観測量

観測量をまとめた観測量ベクトルを次の様にします。

$\begin{eqnarray} {\boldsymbol{z}}= \begin{bmatrix} a_x&a_y&a_z&m_x&m_y&m_z \end{bmatrix}^\mathsf{T} \\ \\ \end{eqnarray}$

観測方程式

観測方程式は加速度と地磁気の観測を表す方程式です。すでに解説しました。ここに再掲します。

加速度の観測方程式

$\begin{eqnarray} \begin{bmatrix} a_x\\ a_y\\ a_z \end{bmatrix} = \begin{bmatrix} 2(q_1 q_3-q_0 q_2)g\\ 2(q_2 q_3+q_0 q_1)g\\ (q_0^2 - q_1^2 - q_2^2 + q_3^2)g\\ \end{bmatrix} + \begin{bmatrix} \delta a_x\\ \delta a_y\\ \delta a_z \end{bmatrix} \\ \\ \end{eqnarray}$

地磁気の観測方程式

観測方程式をまとめて書くと次の様になります。

$\begin{eqnarray} \begin{bmatrix} a_x\\ a_y\\ a_z\\ m_x\\ m_y\\ m_z \end{bmatrix} &=& \begin{bmatrix} 2(q_1 q_3-q_0 q_2)g\\ 2(q_2 q_3+q_0 q_1)g\\ (q_0^2 - q_1^2 - q_2^2 + q_3^2)g\\ (q_0^2+q_1^2-q_2^2-q_3^2) m_n +2(q_1 q_2+q_0 q_3) m_e + 2(q_1 q_3-q_0 q_2) m_d\\ 2(q_1 q_2 -q_0 q_3) m_n + (q_0^2 - q_1^2 + q_2^2 - q_3^2) m_e + 2(q_2 q_3+q_0 q_1) m_d\\ 2(q_1 q_3 + q_0 q_2) m_n + 2(q_2 q_3 - q_0 q_1) m_e + (q_0^2 - q_1^2 - q_2^2 + q_3^2) m_d\\ \end{bmatrix} \\&+& \begin{bmatrix} \delta a_x\\ \delta a_y\\ \delta a_z\\ \delta m_x\\ \delta m_y\\ \delta m_z \end{bmatrix} \\ \\ \end{eqnarray}$

以上の観測方程式も、以下の形をしています。

$\begin{eqnarray} \boldsymbol{z} = \boldsymbol{h} (\boldsymbol{x}) + \boldsymbol{v} \\ \\ \end{eqnarray}$

特に重要な部分は、以下です。

$\begin{eqnarray} \boldsymbol{h} (\boldsymbol{x})= \begin{bmatrix} 2(q_1 q_3-q_0 q_2)g\\ 2(q_2 q_3+q_0 q_1)g\\ (q_0^2 - q_1^2 - q_2^2 + q_3^2)g\\ (q_0^2+q_1^2-q_2^2-q_3^2) m_n +2(q_1 q_2+q_0 q_3) m_e + 2(q_1 q_3-q_0 q_2) m_d\\ 2(q_1 q_2 -q_0 q_3) m_n + (q_0^2 - q_1^2 + q_2^2 - q_3^2) m_e + 2(q_2 q_3+q_0 q_1) m_d\\ 2(q_1 q_3 + q_0 q_2) m_n + 2(q_2 q_3 - q_0 q_1) m_e + (q_0^2 - q_1^2 - q_2^2 + q_3^2) m_d\\ \end{bmatrix} \\ \\ \end{eqnarray}$

状態方程式と観測方程式のヤコビアン

状態方程式の ${\boldsymbol{f}}$ と観測方程式の ${\boldsymbol{h}}$ を状態量ベクトルで編微分したものを示します。

$\begin{eqnarray} \frac{ \partial { \boldsymbol{f} } }{ \partial { \boldsymbol{x} }_k }&=&\\ &\frac{1}{2}& \begin{bmatrix} 0 & -(p_m - \delta p) & -(q_m - \delta q) & -(r_m - \delta r) & q_1 & q_2 & q_3\\ p_m - \delta p & 0 & r_m - \delta r & -(q_m - \delta q) & -q_0 & q_3 & - q_2\\ q_m - \delta q & -(r_m - \delta r) & 0 & p_m - \delta p & - q_3 & - q_0 & q_1\\ r_m - \delta r & q_m - \delta p & -(p_m - \delta p) & 0 & q_2 & -q_1 & - q_0\\ 0&0&0&0&-2\beta_x&0&0\\ 0&0&0&0&0&-2\beta_y&0\\ 0&0&0&0&0&0&-2\beta_z\\ \end{bmatrix} \\ \\ \end{eqnarray}$

$\begin{eqnarray} \frac{ \partial { \boldsymbol{h} } }{ \partial { \boldsymbol{x} }_k }&=&\\ &2& {\small \begin{bmatrix} -q_2g & q_3g & -q_0g & q_1g & 0 & 0 & 0\\ q_1g & q_0g & q_3g & q_2g & 0 & 0 & 0\\ q_0g & -q_1g & -q_2g & q_3g & 0 & 0 & 0\\ q_0 m_n + q_3 m_e - q_2 m_d & q_1 m_n +q_2 m_e + q_3 m_d & -q_2 m_n + q_1 m_e - q_0 m_d & -q_3 m_n + q_0 m_e + q_1 m_d & 0 & 0 & 0\\ -q_3 m_n + q_0 m_e + q_1 m_d & q_2 m_n - q_1 m_e + q_0 m_d & q_1 m_n + q_2 m_e + q_3 m_d & -q_0 m_n - q_3 m_e + q_2 m_d & 0 & 0 & 0\\ q_2 m_n - q_1 m_e + q_0 m_d & q_3 m_n -q_0 m_e - q_1 m_d & q_0 m_n +q_3 m_e - q_2 m_d & q_1 m_n + q_2 m_e + q_3 m_d & 0 & 0 & 0\\ \end{bmatrix}} \\ \\ \end{eqnarray}$

（※上式は横に長いので横にスクロールして見られます。）

そして、以上から

$\begin{eqnarray} {\boldsymbol{F}_k} ={\boldsymbol{I}}+\left. \frac{\partial \boldsymbol{f}}{\partial \boldsymbol{x}_k} \right|_{{\boldsymbol{x}}_k = \bar{\boldsymbol{x}}_k} \Delta t \\ \\ \end{eqnarray}$

$\begin{eqnarray} {\boldsymbol{H}}_k =\left. \frac{\partial \boldsymbol{h}}{\partial \boldsymbol{x}_k} \right|_{{\boldsymbol{x}}_k = \bar{\boldsymbol{x}}_k} \\ \\ \end{eqnarray}$

システムノイズの共分散行列

システムノイズの共分散 ${\boldsymbol{Q}}$ とは先に挙げたジャイロドリフトのダイナミクス式にある、ホワイトノイズの分散の値です。

これらはパラメータ $\beta_{xyz}$ とともに、ジャイロを静置した得られたデータとダイナミクス式を用いてアラン分散が一致する様に推定したり、ローパスフィルターにかけたデータから推定するなどの方法があります。

観測ノイズの共分散行列

観測ノイズの共分散 ${\boldsymbol{R}}$ は加速度と地磁気の測定データから求めて使用します。

そして、コーディングへ・・・

ここまで、準備が整えば、コーディング作業に進みます。間違えない様に着実に作業を進めていきます。

行列の積や逆行列演算を簡単にするためにC++の行列演算ライブラリのEigenの導入を考えています。今はRaspberry Pi Pico（以下Pico）をフライトコントローラにしようと考えています。 Picoは浮動小数点演算のハードを持っていないのでもしかしたらきついかもしれませんが、まずは本記事の正しさの検証も含めてPicoに実装してみます。（ダメなら、他のマイコンにします。）

GIthubにマルチコプタに実装された形でサンプルプログラムをアップしています。PicoでKalmanフィルタの部分の計算が６msほどかかっています。かなり遅いので、コードの見直し、クロックアップの余地はないか、色々検討してみたいと思います。

また、このコードが正しいかどうかの検証はさらにこれからしなければなリませんので、信頼の無いコードです。まずはシミュレーション、その後、実センサで検証していきたいと思います。検証しながら、本記事も改訂していきますので、よろしくお願いします。

github.com

おわりに

以上で拡張カルマンフィルタによるマルチコプターの方向推定とジャイロのバイアス推定の解説を終わりにします。

なかなか時間がかかりました。間違いがあるかもしれません。記事の確認のため実機への実装作業を進めています。実装しつつ間違いが発見されれば、その都度修正します。

本記事に基づいて実機が飛行できた際は、動画の掲載もいたします。（飛び始めました。）

以下の動画は大きな音が出ますのでご注意ください。

ハンチングがとれてきた。電池フル充電だと挙動がおかしくロバストじゃない。ゲイン調整は今日はこれで終わりにして、EKFのところやトリム調整機能などコードの見直しをしよう。 pic.twitter.com/D4EiU57VoR
— こうへい（Kouhei Ito） (@kouhei_kanazawa) 2021年11月27日