-
Notifications
You must be signed in to change notification settings - Fork 88
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
[KalmanFilter]実機のQuaternionExtendedKalmanFilterの姿勢推定結果が正しくない #1259
Comments
追記
|
@eisoku9618 @tongtybj が,フィルタ関係を前にみて(結局やめたか,自分で書いたのでOKにしちゃっているか)ので,聞いてみるとよいと思います. |
変更点を一通りみました。 いくつか確認したい点があります。
ここの
多分ここがミソな気がしますね。なんかしらの事前知識を与えないと、この混合fiterはちゃんと動かないというのが、前から思っていることです。これで動けばいいとは思いますが、少しとヒューリスティクスな感じがしますね。 で、姿勢filter系に関するアドバイスですが、
があるとして、 基本、僕は全軸混合モデルを信用していません(何か事前仮定がないと情報量が足りないので)。なので、ドローン班は二番目のComplementaryFilterを使って、とりあえず、roll,pitchの確実な精度を得ています。 他によく見のは、全軸混合モデルにビジョン情報(VIO)とか、四肢と地面との接触状況(Ethの四足ロボット)を追加する方法です。 それと、今のQuaternionExtendedKalmanFilterの比較評価として, MadgwickFilterを使うのがいいと思います。 もしこっちの精度が良かったら(多分こっちの方がいいと思われる・・・)、Cのcodeも公開されているので、hrpsysにMadgwickFilterとして移植するのもありだと思う。 |
上図の②③となります.
今回,デフォルトで使用しているRPYフィルターの精度が特異点付近で良くない(参考 #295 )ため,特異点付近での精度の向上が報告されている(参考 #296 )QuaternionExtendedKalmanFilterを使用しようとしていました. |
僕が試していたときは、重力ベクトル回りのバイアス項が発散してしまうことがあって、そこを直しきれなかったような気がします。(ので、そこが今回直ったということで嬉しい)
の2点で挙動が良くなったとのことですが、1については知らなかったのですが、どこかに参考情報があれば教えてもらいたいです。 2については僕も試したことがあってあまり変わらなかった気がしますが、1と2の両方が効いている感じなのでしょうか? 先っぽにIMUがついている6軸ロボットが適当に動いて、FKから求めた姿勢角と推定した姿勢角がどのくらい一致するかのtestコードがhrpsysにあると思うので、それで挙動の変化を見てみると良いと思います。 各種パラメータについては、もともとだいぶ適当かもしれないです。 |
参考にした論文があるわけではありませんが、RPYKalmanFilterとQuaternionExtendedKalmanFilterの実装を比較していた際に、
のに対し、QuaternionExtendedKalmanFilterでは「①: ジャイロセンサの角速度出力に乗るノイズ」のみがQとして加えられている hrpsys-base/rtc/KalmanFilter/EKFilter.h Line 83 in 7263626
ため、試しに②③も加えてみたところ実機での推定結果が良くなった、という経緯です。
ありがとうございます。事情によりすぐにはテストできないかもしれませんが、今度確認いたします。 |
なるほどですね. ちなみに,当時の数式の導出の履歴が以下のkf.pdfに書いてあって,今回話題になっている部分はp10-11にかかっている部分に書いてありました.式(229)が重要そうです. 当時の僕は意図的にQの右下を0にしていたようです. これと @Naoki-Hiraoka さんの話と合わせて考えると,(文章で伝わる気があまりしませんが)RPYKalmanFilter・QuaternionEKFのどちらにおいてもシステムの入力を「ジャイロのみ」として捉えていたのが良くなくて,システムの入力を「ジャイロ」+「バイアス項の微分」として捉えてシステムの運動方程式を立ててあげると全てが繋がりそうです. |
#295
KalmanFilterのQuaternionExtendedKalmanFilterを使用して、実機に「鉛直に立っている姿勢」->「前傾姿勢(pitchが90度以上)」を空中で遷移させたところ、下図のように、正しくない出力が出てきました。
代わりにKalmanFilterのRPYKalmanFilterを使用すると、下図のように、yaw以外は正しい出力が出てきます。
自分なりに原因を調査し、QuaternionExtendedKalmanFilterに以下の3つの修正を加えたところ、下図のようにyaw以外は正しいと思われる出力が出てきました。
修正内容はNaoki-Hiraoka@c7446c7 にあります。
before
hrpsys-base/rtc/KalmanFilter/EKFilter.h
Line 83 in 1810137
after
https://github.com/Naoki-Hiraoka/hrpsys-base/blob/81bd1131fac58b9c833f4b739513c08162b8ce9a/rtc/KalmanFilter/EKFilter.h#L94
after
https://github.com/Naoki-Hiraoka/hrpsys-base/blob/81bd1131fac58b9c833f4b739513c08162b8ce9a/rtc/KalmanFilter/EKFilter.h#L67
修正結果(yaw以外は正しいと思われる)
上記修正が正しいのか自信がありません。いかがでしょうか。
The text was updated successfully, but these errors were encountered: