カルマンフィルタでIMUのシステムの計算が間違っている

[nyxrobotics]

概要
下記2で割っている作業が誤りのはずです
この状態でサンプルがうまく動いているということはパラメータ調整し直す必要があります

hdl_localization/include/hdl_localization/pose_system.hpp

Line 79 in 62a8828

Quaterniont dq(1, gyro[0] * dt / 2, gyro[1] * dt / 2, gyro[2] * dt / 2);

再現手順
修正しないとどう困るか
原因
修正案

[jsupratman13]

まだ調べ途中ですが，angular velocityからquaternionに変換するについて，dtが十分小さないなら，2で割って近似するやり方が一般的らしいです．

$$\dot{q} = \frac{1}{2} q \bigotimes w'$$

ここで $w' = (0, w_x, w_y, w_z)$ は角速度をquaternion形式とする． $\bigotimes$ は quaternion 掛け算とする．
次にEuler integrationを行い，angular velocityを加えた新たな quaternionを計算する

$$q_{new} = q + \dot{q} \cdot dt$$

参考：https://gamedev.stackexchange.com/questions/108920/applying-angular-velocity-to-quaternion

hdl_localization/include/hdl_localization/pose_system.hpp

Lines 79 to 81 in 62a8828

	Quaterniont dq(1, gyro[0] * dt / 2, gyro[1] * dt / 2, gyro[2] * dt / 2);
	dq.normalize();
	Quaterniont qt_ = (qt * dq).normalized();

上記の場合は differential quaternion $dq$ を作成して， $q$ とquaternion掛け算することで，angular velocityを加えた新たな quaternionを計算しています．上記と比べて簡易していると思います．おそらくdtかつ角速度が小さならeuler integrationを同じことやっていると思いますが，勉強不足でなので調べ続けます．

[nyxrobotics]

リンク確認しましたが $\bigotimes$ の意味がわかりませんでした。内積・外積・回転の合成どちらでしょうか
失礼しました。下記ブログのクオータニオンの掛け算の定義式と理解しました

私は以下の記事でのQuaternionの定義に基づいて考えています
https://qiita.com/drken/items/0639cf34cce14e8d58a5

上記の定義式

$$ w = cos(\theta/2) $$

$$ x = \lambda x * sin(\theta/2) $$

$$ y = \lambda y * sin(\theta/2) $$

$$ z = \lambda z * sin(\theta/2) $$

これを

$$ \theta \ll 1 $$

で近似すると

$$ w \simeq 1 - (\theta/2)^2 $$

$$ x \simeq \lambda x * \frac{\theta}{2} $$

$$ y \simeq \lambda y * \frac{\theta}{2} $$

$$ z \simeq \lambda z * \frac{\theta}{2} $$

dtで微分すると

$$ \frac{dw}{dt} \simeq - \frac{1}{2} * \theta * \frac{d\theta}{dt} $$

$$ \frac{dx}{dt} \simeq \frac{d\lambda x}{dt} * \frac{\theta}{2} + \lambda x * \frac{d\theta/dt}{2} $$

$$ \frac{dy}{dt} \simeq \frac{d\lambda y}{dt} * \frac{\theta}{2} + \lambda y * \frac{d\theta/dt}{2} $$

$$ \frac{dz}{dt} \simeq \frac{d\lambda z}{dt} * \frac{\theta}{2} + \lambda z * \frac{d\theta/dt}{2} $$

θ = 0とすると Quaterniont dq(0, gyro[0] * dt / 2, gyro[1] * dt / 2, gyro[2] * dt / 2); なら成り立ちそうです
しかしクオータニオンそのものを微分してもノルムが1にならないのでなにか間違えています

[nyxrobotics]

クォータニオンの微分についても記事がいくつかありました
https://ushitora.net/archives/3061
この定義だとdqとqtの合成は掛け算ではなく足し算です
よって下記なら成立しそうです（動作しました）

  Quaterniont dq = Quaterniont(0, gyro[0] * dt_ / 2.0, gyro[1] * dt_ / 2.0, gyro[2] * dt_ / 2.0) * qt;
  Quaterniont qt_next = Quaterniont(qt.w() + dq.w(), qt.x() + dq.x(), qt.y() + dq.y(), qt.z() + dq.z()).normalized();

計算順序を考慮しないで良いのでオイラー角で計算するより良い気がします