9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応

V2.5 NEW主な変更点

【V2.4】令和5年3月にリリース済み
較正モードの追加により、ジャイロスコープ、加速度センサ、地磁気センサの初期バイアスは出荷時の測定のみならず、ユーザー様のもとでも測定することはできる。 ※ ユーザー様のお手元にある旧バージョン製品のファームウェアのバージョンアップは、ユーザー様のもとで実施可能なので、詳細については、別途順次ご案内する。

【V2.5】令和5年12月にリリース済み
TDK Smart Motion DMPコードリビジョンに伴うファームウェアのアップデート、ROSパッケージ更新なし。
※ ユーザー様のお手元にある旧バージョン製品のファームウェアのバージョンアップは、ユーザー様のもとで実施可能なので、令和5年12月末までご案内済み。

はじめに

9軸IMU(型番hayate_imu)は、コロナ禍の中で開発した新商品、令和3年3月まで開発~製造、令和3年4月上旬の出荷と予定して、皆さんの学術研究にお役に立てるようと願って、どうぞご検討ご利用のほど宜しくお願い申し上げます。

製品紹介

9軸センサhayate imu、低消費電力プロセッサーCortexM0+、TDK MPU-9250後継機種である、1.71V低電圧で動作可能なICM-20948使用、6軸/9軸融合クォータニオン(四元数)はFPGA on chip(DMP3)から低遅延出力、別途ソフトでフュージョン必要なし、最大出力レート225Hz、同時に加速度(アクセル)3軸データ225Hz、角速度(ジャイロ)3軸データ225Hz、地磁気(コンパス)3軸データ70Hzまで出力可能、補正済み、ROS対応。ロボット、ドローンなど低遅延が必要とされる科学研究、電子機械の検証試作ヘの活用が期待される。

主な仕様

・ 型番 hayate_imu rev.C 6軸フュージョン or ver.B 9軸フュージョン切替可能
・ 内蔵チップ Cortex-M0+、TDK Invensense ICM-20948(9軸)実装 ※1
・ 外部接続 USB Type-Cコネクタ、USB +5V給電 ※2 ※3
・ 最大出力レート ※4
  - 6軸フュージョン or 9軸フュージョン回転ベクトル四元数 225Hz
  - 加速度(アクセル)3軸センサ  225Hz
  - 角速度(ジャイロ)3軸センサ  225Hz
  - 地磁気(コンパス)3軸センサ  70Hz

・ 測定レンジ
  - 加速度(アクセル)センサ  ±16g
  - 角速度(ジャイロ)センサ  ±2000dps
  - 地磁気(コンパス)センサ  ±4900µT

・ 消費電力 50mW以下(環境温度21℃の実測値)
・ 寸法 30mm × 31.4mm × 4.8mm(突起物含む)
・ 重量 4g以下
・ 取付穴 M3x4、隣り合う穴の中心間距離24.4mm

※1 内蔵Cortex-M0+とICM-20948間インターフェースはSPI(4Mbps)使用、加速度センサ(消耗)、角速度センサ(温度、ドリフト)、地磁気センサ(磁気変動)にダイナミック補正。
※2 USB対向装置OS環境 Ubuntu 16.04以降推奨。
※3 USB対向装置ROS環境 Kinetic以降推奨。
※4 最大出力レートはhayate imuの実力値、IMU対向装置(USB接続先)での実効値はその装置のリソース(CPUクロック周波数、メモリ容量・スピード)に関わる。

デモ情報

hayate_imu ROSパッケージ | Githubリポジトリ

9軸IMUセンサ ICM-20948内蔵 6軸/9軸シュージョン 出力レート225Hz 低遅延 USB出力 ROS対応 | YouTube

9dof_hayate_imu_youtube
9dof_hayate_imu_youtube

販売情報

【製品名称】hayate_imu rev.C 6軸フュージョン or ver.B 9軸フュージョン
【開発会社】ROBOT翔(株式会社翔雲)
【発売時期】令和3年4月上旬頃
【取扱店舗】9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応 | ROBOT翔

後継機種

9軸センサーICM-42688+MMC5983 6軸&9軸回転ベクトル&3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続

参考資料

Migrating from MPU-9250 to ICM-20948-InvenSense
http://wiki.ros.org/ja/9dof_hayate_imu

関連記事

エンコーダ付きDCモータPID制御の実験-hayte_imu応用例
9軸IMUセンサ ICM-20948をロボットに組み込もう
オイラー角~ジンバルロック~クォータニオン
ROS・Unity・ロボット・ドローン姿勢制御に関わるクォータニオン

2+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

6軸9軸IMUパフォーマンス

はじめに

6軸、9軸IMUのパフォーマンスについて、さまざまな視点からテスト可能となり、以下のとおり、センサフュージョン、出力周波数、ドリフト、4元数出力、遅延において、弊社ならではのテスト項目、手順をご紹介する。

確認環境の例

・Ubuntu 18.04
・ROS Melodic
・MCU: SAMD21G18A
・IMU: TDK Invensense ICM-20948

センサフュージョン、4元数出力

FPGAなどハードウェアによるセンサフュージョン(4元数出力)、出力値の分散が小さいほうが望ましいので、rvizで出力結果を確認する。

$roslaunch icm20948_imu_driver icm20948_imu.launch
$rostopic imu/data

出力周波数

ROSコマンドで出力周波数を簡単に確かめる。200Hz以上つまり5ms間隔データの出力が望ましい。

$rostopic hz imu/data

ドリフト

数時間〜数十時間に亘って、rqt&rvizで静止状態センサの出力はどれくらい変動したかを確かめる。平均値の変動、分散とも小さいほうが望ましい。

$rosrun rqt_plot rqt_plot

遅延時間

500〜1000fps 高速レコーディングカメラで撮影した動画を解析して、遅延時間を確かめる。fpsが高い、IMUの遅延時間が短いほうが望ましい。

動画コンテンツ

以下イメージをクリックすると、youtubeへジャンプする。

imu_performance_test
imu_performance_test

関連記事

9軸IMU 6軸/9軸フュージョン ICM-20948をロボットに組み込もう
9軸IMU MPU-9250をロボットに組み込もう
6軸IMU MPU-6050をロボットに組み込もう

3+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

9軸IMUセンサ ICM-20948をロボットに組み込もう

はじめに

TDK Invensense製9軸IMUのICM-20948は、MPU-9250の後継機種で、MPU-9250のVDDは2.4V~3.6V、VDDIOは1.71V~VDDに対して、ICM-20948のVDDは1.71V~3.6V、VDDIOは1.71V~1.95Vに低めに設定して、省電力となった。また、デジタルモーションプロセッサDMP(ICM-20948内蔵FPGA)によるデータフュージョン(FPGAによるFusion)の特長が継承して、さらにRAM容量が拡張して、6軸フュージョンのみならず9軸フュージョンまで増強して、較正機能もあると、以下参考文献を読むと詳細まで分かる。

環境

・Ubuntu 18.04
・ROS Melodic
・MCU: Cortex M0+
・IMU: ICM-20948

DMP3の出力確認

以下のように、出力レート50Hz、加速度Ax Ay Az、角速度Gx Gy Gz、磁場Mx My Mz、4元数Qw Qx Qy Qzの順に出力させる。

imu-icm20948-output
imu-icm20948-output

4時間+にわたる連続動作して出力を確かめる。確認環境は完全に静止な状態でもないので、ドリフトは納得いく範囲内にとどまっている。rvizで確かめてもドリフトが肉眼では見えないほど。ドリフトにおいては、MPU-9250から大いに改善されたと見られる。

imu-icm20948-output-4hours
imu-icm20948-output-4hours

出力確認動画は以下イメージをクリックすると、youtubeへジャンプする。

icm20948_imu_ros
icm20948_imu_ros

最後に

MPUシリーズと比べて、ユーザの事前校正いらず、長時間(実験は4時間程度まで)においても、ドリフトとくにヨウ角(Yaw、方向角)のドリフトは目立たないほどになった。また、1.71Vの低電圧でも動作可能なのでスマートデバイスや、ロボットの長時間電池駆動が可能になる。なお、出力レートはMax 225Hzと確認できた。ICM-20948 DMP3(IMU内蔵FPGA)から出力した、Accel/Gyro/Mag計9軸データ出力にFusion Quaternionの4元数データがそのまま利用可能で、遅延もソフト・カルマンフィルタなどより少なく他機種IMUより優れる(低遅延、6軸/9軸フュージョンデータ出力レート225Hz)ため、ロボットの精度向上に利用可能。1.8V VDDIO対応、DMP3の出力に手間かかった末、地磁気センサ出力は75Hzまでと少し残念だが、総じて優秀としか思わないICM-20948をロボット装置に組み込もうと決めた。

商品化モジュール

ICM-20948とCoretex M0+を組み込んだ回路を設計して、ROSに対応したロボット専用センサモジュールとして商品化して、2021年1月~、リリースと予定している。この商品は皆さんの学術研究にお役に立てるようと願う。主な仕様は以下のとおり。
・構成 CortexM0+ & TDK Invensense ICM-20948(9軸)実装
・接続 USB Type-Cコネクタ実装
・出力 6軸/9軸融合4元数はFPGA on chipから低遅延で出力、別途ソフトでフュージョン必要なし、出力レート225Hz、同時に加速度(アクセル)3軸データ225Hz、角速度(ジャイロ)3軸データ225Hz、地磁気(コンパス)3軸データ75Hzまで出力可能
・ROSパッケージ、Githubへ公開、ROS Kinetic以降対応、ROS TopicへSubscribeすることでデータが受け取り可能
・rviz実演、実演ビデオあり

【2021年3月いま現在】1回目制作分(評価版)、大学など研究機関へ無料配布中(アンケート調査あり)、WEBでの募集を含めて順次終了。

【2021年4~5月予定】2回目制作分(商用版)、販売の予定。

詳細情報

9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応

取扱店舗

9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 ROS対応 | ROBOT翔

参考文献

Migrating from MPU-9250 to ICM-20948-InvenSense

関連記事

9軸IMUセンサ MPU-9250をロボットに組み込もう
6軸IMUセンサ MPU-6050をロボットに組み込もう

2+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

9軸IMUセンサ MPU-9250をロボットに組み込もう

はじめに

9軸IMUのMPU-9250はTDK InvenSense社製I2Cインターフェースの3軸ジャイロセンサ+3軸加速度センサ+3軸コンパスセンサIC、内蔵DMP(Digital Motion Processor)機能を使うことで、補正済みデータとしての4元数Quaternionまたはオイラー角、ロールRoll・ピッチPitch・ヨウYaw角の出力が選べる。また、MPUシリーズはすでに新規設計非推奨になっているため、後継機種はICMシリーズで、MPU-9250の後継機種はICM-20948となって、MPU-9250に比べて、事前校正不要で、ドリフトの低減、省電力などにおいてパフォーマンスが改善された。本文は、6軸MPU-6050に続いて、9軸MPU-9250 DMPから4元数Quaternionを読み込んで可視化するまでの手順を以下のとおりに示して、ROSドライバをGithubへ公開する。安価のため、MPU-9250サンプルの入手ルートはAliexpressにした。

mpu6050-mpu9250
mpu6050(6軸)-mpu9250(9軸)

I2Cインターフェースは、vcc、gnd、scl、sdaの4pinインターフェース

環境

・ubuntu 18.04 Tinker board(or Raspiberry Pi, PC)
・ROS melodic
・DFRobot Romeo mini v1.1(or arduino uno互換)
・MPU-9250/6500

準備①

・ros-melodic-rosserial-arduino、ros-melodic-rosserial、rviz_imu_pluginを入れる

$sudo apt-get update
$sudo apt-get install ros-melodic-rosserial-arduino
$sudo apt-get install ros-melodic-rosserial
$cd ~catkin_ws/src/
$git clone -b melodic https://github.com/ccny-ros-pkg/imu_tools
$cd ..
$catkin_make_isolated

・mpu9250_imu_rosを入れる

$cd ~/catkin_ws/src/
$git clone https://github.com/soarbear/mpu9250_imu_ros.git
$cd ~/catkin_ws/
$catkin_make_isolated

準備②

・firmware/MPU9250_DMP/MPU9250_DMP.inoをArduino IDEでArduinoに書き込む。

imu/dataの可視化

・実に使われるポートtty????を確認する。
・rvizが自動起動して、画面にあるセンサの動きを観察する。

$sudo ls -l /dev/ttyACM*
$sudo chmod 777 /dev/ttyACM0
$roslaunch mpu9250_imu_driver mpu9250_imu.launch

・以下スクリーンショットをクリックすると、youtubeへ遷移する。

mpu6050_imu_ros
mpu9250_imu_ros

センサ融合について

MPU-9250内蔵DMPおよび、センサ融合またはデータ同化Fusionに定番アルゴリズムであるKalman Filterの他、Complementary Filter、Madgwick Filterがある。振動やシステム誤差によって測定値に大きな影響あり、フィルタリングが必須とは言える。

ソースコード

mpu9250_imu_rosソースコード(Github)

後継機種

ICM-20948はMPU-9250の後継機種、その製品化情報は 9軸IMU/AHRS 6軸&9軸回転ベクトル&3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続9軸IMU 6軸/9軸フュージョン ICM-20948 Cortex-M0+内蔵 ROS対応

参考文献

1-Jeff Rowberg氏: I2C driver
2-ROS Repository: ROS imu_tools

関連記事

9軸IMU/AHRS 6軸&9軸回転ベクトル&3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続
9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応
9軸IMU ICM-20948をロボットに組み込もう
6軸IMU MPU-6050をロボットに組み込もう

3+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

カルマンフィルタの導出

はじめに

wiki: 「カルマンフィルタ\((Kalman Filter, KF\)と略す\()\) は、誤差のある観測値を用いて、ある動的システムの状態を推定あるいは制御するための、無限インパルス応答フィルタの一種である」。観測値に観測雑音、状態予測値にシステム雑音があって場合により時間とともに誤差がドンドン蓄積してそのまま使えないので、観測と予測のガウス性を利用した線形センサデータフュージョンがカルマンフィルタの原点である。

状態空間モデル

時系列解析の中で、予測値と観測値の間何らかの因果関係を見つけて、何らかの方法でそれらのデータを絡んでモデル化して状態を推定していく。ここで汎用的な状態方程式観測方程式は以下の式にする。
$$\begin{eqnarray*}&& x_t = F_{t-1}(x_{t-1} ) + q_{t-1}\\&& y_t = H_t(x_t) + r_t\end{eqnarray*}$$
ただし、システム雑音、観測雑音\((q_t, r_t)\)は期待値が\(0\)、分散が\((Q_t, R_t)\)の独立正規分布(ガウス分布)にする。
$$\begin{eqnarray*}&& p(q_t) ~ N(0,Qt)\\&& p(r_t) ~ N(0,Rt)\\&& E[q_t r_t]=0 \end{eqnarray*}$$

基本原理

カルマンフィルタの原理は以下のイメージに示すように、最小解析誤差推定(最小二乗)から予測値、観測値をそれぞれの割合\(I-K_t H_t , K_t\)で線形合成した値を状態推定値にする。

kalman_filter_principle_n
kalman_filter_principle_n

式の導出

ベイズの定理やガウス分布からいくつかのの導出方法はあるが、本記事はシンプルな最小二乗法からカルマンフィルタの黄金\(5\)式\(\space *\)を誘導する。

線形システム

\(F(.),H(.)\)とも線形関数の場合、以下線形カルマンフィルタの誘導となる。例えば\( F(x)=Ax+B\)、\(A,B\)とも実数行列、\(x\)は変数行列とする。

まずは、予測値(数学期待)を求めるのは出発点なので、すべては以下の式から始まる。
$$ \gamma_t = F_{t-1}(\gamma_{t-1}) \space \space c $$
推定値\(x_t^{est}\)は、重み\(K_t\)の調整で予測値\(x_t\)と観測値\(y_t\)の間にあろうとして、以下線形表現がある。
$$ x_t^{est} = x_t + K_t(y_t-H_t x_t) \\
{\small = F_{t-1}(x_{t-1}^{true}+p_{t-1}) + q_{t-1}+ K_t\{(H_t x_t^{true}+r_t) }\\
{\small – H_t(F_{t-1}(x_t^{true}+p_{t-1})+q_{t-1})\} }\\
{\scriptsize = x_t^{true}+F_{t-1}p_{t-1}+q_{t-1}+K_t(r_t-H_t F_{t-1}p_{t-1}-H_t q_{t-1}) }$$

解析誤差(共分散\(P_t\))は以下の式より求める。
$$ P_t = E[(x_t^{est}-x_t^{true})^2] \space \space \\
{\scriptsize = E[(F_{t-1}p_{t-1} + q_{t-1} + K_tr_t-K_t H_t F_{t-1}p_{t-1}-K_t H_t q_{t-1})^2] }\\
{\scriptsize = F_{t-1}P_{t-1}F_{t-1}^T-2F_{t-1}P_t F_{t-1}^TH_t^T K_t^T+Q_{t-1}-2Q_{t-1}H_t^T K_t^T }\\
{\scriptsize + K_t R_t K_t^T+K_t H_t F_{t-1}P_{t-1}F_{t-1}^TH_t^T K_t^T+K_t H_t Q_{t-1}H_t^T K_t^T } $$

ここから二乗の最小化より解析誤差(共分散\(P_t\))を最小値に至らせる\(K_t\)を求める。
$$ \frac {\partial P_t } {\partial K_t } = 0 \\
{\small \frac {\partial P_t } {\partial K_t } = -2F_{t-1} P_{t-1} F_{t-1}^T H_t^T -2Q_{t-1}H_t^T } \\
{\small +2K_t R_t + 2K_t H_t F_{t-1} P_{t-1} F_{t-1}^T H_t^T +2K_tH_tQ_{t-1}H_t^T } \\
{\small K_t = (F_{t-1}P_{t-1}F_{t-1}^T + Q_{t-1}) H_t^T \{R_t } \\
{\small + H_t(F_{t-1}P_{t-1}F_{t-1}^T+Q_{t-1})H_t^T\}^{-1} } $$

これで、状態予測時\( K_t = 0 \)より、式\(\cdot\)を\({\small P_t = F_{t-1}P_{t-1}F_{t-1}^T + Q_{t-1} \space ** }\)に、\( K_t\)は、以下のように簡略化する。
$$ K_t = P_t H_t^T (R_t + H_t P_t H_t^T)^{-1} \space \space *** $$
これから観測値を入れて、\(K_t\) を使って推定値を求める。
$$ x_t^{est} = x_t + K_t(y_t – H_t x_t) \space \space **** $$
推定値を求めたら、続いて\(P_t\)を更新する。前述した\(#\)式の展開の項らより、最後の式は誘導できる。
$$ P^{\prime}_t = (I – K_t H_t)P_t \space \space ***** $$
\(P^{\prime}_t\)は、\((t+1)\)時刻の\(P_t\)として利用される。

非線形システム

線形カルマンフィルタの\(F(.),H(.)\)は非線形関数の場合、\(f(.),h(.)\)と記す。例えば\(f(x)=Ax^2+B\)、\(A,B\)とも実数行列、\(x\)は変数行列として、ティラー展開の最初の\(2\)項のみ使って、\( \hat{f}(x)=f(a)+\frac {\partial {f(x)}} {\partial {x}}|_{x=a}x \) のように\(Aa^2+B+2A(Aa^2+B)x\)に線形化することができる。\(3\)項目以降は捨てるので大いに誤差を招く可能性ある。状態予測の際に線形カルマンフィルタの\(F\)を\(f\)、観測値の取り入れる際に線形カルマンフィルタの\(H\)を\(h\)に、共分散行列の計算の際に\(F,H\)を\( \frac {\partial {f(x)}} {\partial {x}}|_{x=x_{t-1}}, \frac {\partial {h(x)}} {\partial {x}}|_{x=x_t}\)に置き換えて済む。

これまでカルマンフィルタの誘導で、状態値推定の手順を以下のとおり整理しておく。

推定の手順

時系列の状態推定に応用して、以下状態予測\(Predict\)→測定更新 \(Measurement \space Update\)→状態更新\(Correct, Update\)→時間更新\(Time \space Update\)→次の状態予測\(Predict\)→\(…\)の繰り返すことによって、時系列とともに状態の数学期待最小解析誤差(共分散)を求める手順となる。

Step0=パラメータ初期化(t=0)

状態推定(数学期待)、解析誤差共分散の初期化
$$\begin{eqnarray*}&& \hat{x}_0 = E[x_0]\\&& \hat{P_0} = E[(x_0-\hat{x_0})(x_0-\hat{x_0})^T]\end{eqnarray*}$$

Step1=状態値、共分散行列予測(t>0)

$$\begin{eqnarray*}&& \bar{x_t} = F_{t-1}(\hat{x}_{t-1})\\&& \bar{P_t} = F_{t-1}\hat{P}_{t-1}F_{t-1}^T + Q_{t-1}\end{eqnarray*}$$
ただし、\(\bar{x_t}\)は予測値、\(\bar{P_t}\)は事前共分散行列、\(Q_{t-1}=E[q_{t-1}q^T_{t-1}]\)

Step2=カルマンゲイン、状態値、共分散行列更新

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + R_t)^{-1}\\
&& \hat{x_t} = \bar{x_t} + K_t(y_t – H_t\bar{x_t})\\&&\hat{P_t} = (I-K_tH_t)\bar{P_t} \end{eqnarray*}$$
ただし、\(K_t\)はカルマンゲイン、\(\hat{x_t}\)は状態推定値、\(\hat{P_t}\)は事後共分散行列、\(R_{t}=E[r_{t}r^T_{t}]\)

KFフローチャート

Kalman_Filter_Flowchart_2
Kalman_Filter_Flowchart_2

拡張カルマンフィルタ

拡張カルマンフィルタ\((Extended Kalman Filter, EKF\)と略す\()\)は、非線形フィルタリングである。前述した状態方程式、観測方程式より、以下の状態空間モデルの\(f(⋅)\)または\(h(⋅)\)が非線形関数であり、拡張カルマンフィルタが適用される。テイラー展開より、2次微分以降の項目を省略して、非線形である\(f(⋅), h(⋅)\)の1次微分を線形化とし、前述したカルマンフィルタのアルゴリズムが適用可能となる。しかし、\(f(⋅), h(⋅)\)の微分では\(f(⋅), h(⋅)\)の一部しか表現できず、この線形化処理(1次微分)が誤差を大きく招く可能性がある。
$$\begin{eqnarray*}&& F_{t-1} =\frac{\partial f_{t-1}(x)}{\partial x}|_{x=\hat{x}_{t-1}}\\&& H_{t} =\frac{\partial h_{t}(x)}{\partial x}|_{x=\hat{x}_{t}}\end{eqnarray*}$$

状態空間モデル

$$\begin{eqnarray*}&& x_t = f_{t-1}(x_{t-1}) + q_{t-1}\\&& y_t = h_t(x_t) + r_t\end{eqnarray*}$$

Step0=パラメータ初期化(t=0)

$$\begin{eqnarray*}&& \hat{x}_0 = E[x_0]\\&& \hat{P_0} = E[(x_0-\hat{x_0})(x_0-\hat{x_0})^T]\end{eqnarray*}$$

Step1=状態値、共分散行列予測(t>0)

$$\begin{eqnarray*}&& \bar{x_t} = f_{t-1}(\hat{x}_{t-1})\\&& \bar{P_t} = F_{t-1}\hat{P}_{t-1}F_{t-1}^T + Q_{t-1}\end{eqnarray*}$$
ただし、\(\bar{x_t}\)は予測値、\(\bar{P_t}\)は事前共分散行列、\(Q_{t-1}=E[q_{t-1}q^T_{t-1}]\)

Step2=カルマンゲイン、状態値、共分散行列更新

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + R_t)^{-1}\\
&& \hat{x_t} = \bar{x_t} + K_t(y_t – h_t\bar{x_t})\\&&\hat{P_t} = (I-K_tH_t)\bar{P_t} \end{eqnarray*}$$
ただし、\(K_t\)はカルマンゲイン、\(\hat{x_t}\)は状態推定値、\(\hat{P_t}\)は事後共分散行列、\(R_t=E[r_t r^T_t]\)

EKFフローチャート

Extended_Kalman_Filter_Flowchart_2
Extended_Kalman_Filter_Flowchart_2

拡張カルマンフィルタを6軸IMUへの適用

6軸IMUへの適用例として、本サイトの記事 6軸IMU~拡張カルマンフィルタ に載ってある。

カルマンフィルタの再考

カルマンフィルタでは、状態推定値を予測結果\(x_t\)(実装例ではジャイロセンサーデータ)と観測データ\(y_t\)(実装例では加速度センサデータ)の線形結合で合成し,その誤差を最小にする推定法だと分かる。これはシステム誤差、観測誤差の数学期待が0の正規分布との前提条件から由来した推定法である。しかし、カルマンフィルタをかけることで、状態推定値は予測結果と観測データの間にあるのは、真値からかなり乖離してしまう場合にあるのか。これはシステム誤差と観測誤差が無相関かつ直交という前提から、勿論真値が推定値と観測値の間にある結論を結ぶ考えである。また、ジャイロセンサーデータと、加速度センサデータとも観測値\(y_t\)にする方法がある。それぞれのパーフォーマンスの検証は、比較的精確な実験環境(比較用の高精度ジャイロセンサ、加速度センサ、エンコーダ、モータ)がないと、実は容易ではない。というよりも、シミュレーションをかけてカルマンフィルタのアルゴリズムを検証するのが、確実に可能である。

参考文献

1-wikipedia: カルマンフィルタオイラー角
2-Greg Welch氏、Gary Bishop氏: An Introduction to the Kalman Filter
3-田島洋氏: マルチボディダイナミクスの基礎―3次元運動方程式の立て方

関連記事

オイラー角~ジンバルロック~クォータニオン
SLAM~拡張カルマンフィルタ
SLAM~Unscentedカルマンフィルタ
9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応
9軸IMU ICM-20948をロボットに組み込もう
YDLIDAR G4=16m 薄型 ROS対応SLAM LIDAR
研究開発用 台車型ロボット キット

4+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア