Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
nyxrobotics opened this issue Dec 18, 2023 · 3 comments · May be fixed by #36
Open

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

nyxrobotics opened this issue Dec 18, 2023 · 3 comments · May be fixed by #36
Labels
bug Something isn't working

Comments

@nyxrobotics
Copy link

nyxrobotics commented Dec 18, 2023

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

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

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

@nyxrobotics nyxrobotics added the bug Something isn't working label Dec 18, 2023
@nyxrobotics nyxrobotics linked a pull request Jan 9, 2024 that will close this issue
@jsupratman13
Copy link

まだ調べ途中ですが,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

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
Copy link
Author

nyxrobotics commented Jan 11, 2024

リンク確認しましたが $\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
Copy link
Author

nyxrobotics commented Jan 11, 2024

クォータニオンの微分についても記事がいくつかありました
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();

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug Something isn't working
Projects
None yet
Development

Successfully merging a pull request may close this issue.

2 participants