[KalmanFilter]実機のQuaternionExtendedKalmanFilterの姿勢推定結果が正しくない

[Naoki-Hiraoka]

#295

KalmanFilterのQuaternionExtendedKalmanFilterを使用して、実機に「鉛直に立っている姿勢」->「前傾姿勢(pitchが90度以上)」を空中で遷移させたところ、下図のように、正しくない出力が出てきました。

代わりにKalmanFilterのRPYKalmanFilterを使用すると、下図のように、yaw以外は正しい出力が出てきます。

自分なりに原因を調査し、QuaternionExtendedKalmanFilterに以下の3つの修正を加えたところ、下図のようにyaw以外は正しいと思われる出力が出てきました。
修正内容はNaoki-Hiraoka@c7446c7 にあります。

• 時間経過によるノイズが、gyroセンサ由来のものは考慮されていますが、システム由来のものが考慮されていなかったため、考慮するようにしました。
  before
  

  hrpsys-base/rtc/KalmanFilter/EKFilter.h

  Line 83 in 1810137

  _P_a_priori = F * P * F.transpose() + VQVt;

  
  after
  https://github.com/Naoki-Hiraoka/hrpsys-base/blob/81bd1131fac58b9c833f4b739513c08162b8ce9a/rtc/KalmanFilter/EKFilter.h#L94
• 鉛直方向周りの回転成分についてはcorrectionされる情報量が少ないため、gyroのbiasが発散することがありました。biasが時間経過とともに減衰するようにすることで発散を抑えました。
  after
  https://github.com/Naoki-Hiraoka/hrpsys-base/blob/81bd1131fac58b9c833f4b739513c08162b8ce9a/rtc/KalmanFilter/EKFilter.h#L67
• QuaternionExtendedKalmanFilterのパラメータをサービスで変更できるようにしました(姿勢推定結果には直接の関係はありません)

修正結果(yaw以外は正しいと思われる)

上記修正が正しいのか自信がありません。いかがでしょうか。

[Naoki-Hiraoka]

追記
QuaternionExtendedKalmanFilterを使ってStabilizerで制御しながら実機を立たせたり、歩行させたりするためには、上記に加えて以下の変更が必要でした。

• 加速度センサの値の分散fuzzyRに関するパラメータk1,k2を、デフォルトの400,0.03から、2000,500へと大きくする。
  Naoki-Hiraoka@9af862e

[k-okada]

@eisoku9618 @tongtybj が，フィルタ関係を前にみて（結局やめたか，自分で書いたのでOKにしちゃっているか）ので，聞いてみるとよいと思います．

[tongtybj]

@Naoki-Hiraoka

変更点を一通りみました。

いくつか確認したい点があります。

時間経過によるノイズが、gyroセンサ由来のものは考慮されていますが、システム由来のものが考慮されていなかったため、考慮するようにしました。

ここのシステム由来のものは具体的には何を指しているのかな？
あと、Qだけだと何を指しているのかわかりにくいでの、せめてQsysにした方がいいかと(あと口うるさいかもしれないが、メンバー変数はQ_sys_,とか、アンダーバーを付けるべきかと・・・)

鉛直方向周りの回転成分についてはcorrectionされる情報量が少ないため、gyroのbiasが発散することがありました。biasが時間経過とともに減衰するようにすることで発散を抑えました。

多分ここがミソな気がしますね。なんかしらの事前知識を与えないと、この混合fiterはちゃんと動かないというのが、前から思っていることです。これで動けばいいとは思いますが、少しとヒューリスティクスな感じがしますね。

で、姿勢filter系に関するアドバイスですが、
まず、種類として、

• 一軸ずつ独立に行うもの: hrpsysのRPYKalmanFilter
• roll+pitchの混合モデルで重力方向を推定して、yawは別に考える(例えば地磁気とか): ComplementaryFilter
• 全軸混合して行う: hrpsysのQuaternionExtendedKalmanFilter, MadwickFilter

があるとして、

基本、僕は全軸混合モデルを信用していません(何か事前仮定がないと情報量が足りないので)。なので、ドローン班は二番目のComplementaryFilterを使って、とりあえず、roll,pitchの確実な精度を得ています。

他によく見のは、全軸混合モデルにビジョン情報(VIO)とか、四肢と地面との接触状況(Ethの四足ロボット)を追加する方法です。

それと、今のQuaternionExtendedKalmanFilterの比較評価として, MadgwickFilterを使うのがいいと思います。

もしこっちの精度が良かったら(多分こっちの方がいいと思われる・・・)、Cのcodeも公開されているので、hrpsysにMadgwickFilterとして移植するのもありだと思う。

[Naoki-Hiraoka]

パラメータ	RPYKalmanFilter	デフォルトのQuaternionExtendedKalmanFilter	今回変更したQuaternionExtendedKalmanFilter
①: ジャイロセンサの角速度出力に乗るノイズ	②と合わせて0.001	0.001	0.001
②: ロボットの姿勢に時間経過で乗るノイズ	①と合わせて0.001	なし	0.001
③: ジャイロセンサのバイアスに時間経過で乗るノイズ	0.003	なし	0.003
④: 加速度センサの出力に乗るノイズ	1000(rad*radの次元)	0.03(m/s^2*m/s^2の次元)	500(m/s^2*m/s^2の次元)
⑤: 加速度センサの出力の信頼度が低い時に④に加えるノイズ	なし	400	2000
⑥: ジャイロセンサのバイアスの減衰の時定数	なし	なし	3

ここのシステム由来のものは具体的には何を指しているのかな？

上図の②③となります．

それと、今のQuaternionExtendedKalmanFilterの比較評価としてMadgwickFilterを使うのがいいと思います。

今回，デフォルトで使用しているRPYフィルターの精度が特異点付近で良くない(参考 #295 )ため，特異点付近での精度の向上が報告されている(参考 #296 )QuaternionExtendedKalmanFilterを使用しようとしていました．
ご指摘頂いた通り，全軸混合モデルは情報量が足りないため，
QuaternionExtendedKalmanFilterの精度で姿勢制御に十分であるうちは当面QuaternionExtendedKalmanFilterを使用しようと思いますが，姿勢制御に問題があると判明した場合にはご紹介いただいた姿勢Filterを検討しようと思います．

[eisoku9618]

僕が試していたときは、重力ベクトル回りのバイアス項が発散してしまうことがあって、そこを直しきれなかったような気がします。（ので、そこが今回直ったということで嬉しい）

1. 時間経過によるノイズが、gyroセンサ由来のものは考慮されていますが、システム由来のものが考慮されていなかったため、考慮するようにしました。
2. biasが時間経過とともに減衰するようにすることで発散を抑えました。

の2点で挙動が良くなったとのことですが、1については知らなかったのですが、どこかに参考情報があれば教えてもらいたいです。

2については僕も試したことがあってあまり変わらなかった気がしますが、1と2の両方が効いている感じなのでしょうか？

先っぽにIMUがついている6軸ロボットが適当に動いて、FKから求めた姿勢角と推定した姿勢角がどのくらい一致するかのtestコードがhrpsysにあると思うので、それで挙動の変化を見てみると良いと思います。

各種パラメータについては、もともとだいぶ適当かもしれないです。

[Naoki-Hiraoka]

の2点で挙動が良くなったとのことですが、1については知らなかったのですが、どこかに参考情報があれば教えてもらいたいです。

参考にした論文があるわけではありませんが、RPYKalmanFilterとQuaternionExtendedKalmanFilterの実装を比較していた際に、
RPYKalmanFilterでは「①: ジャイロセンサの角速度出力に乗るノイズ　+　 ②: ロボットの姿勢に時間経過で乗るノイズ 」と「③: ジャイロセンサのバイアスに時間経過で乗るノイズ」が対角成分に含まれる行列Qとして加えられている

hrpsys-base/rtc/KalmanFilter/RPYKalmanFilter.h

Line 46 in 7263626

P = F * P * F.transpose() + Q;

のに対し、QuaternionExtendedKalmanFilterでは「①: ジャイロセンサの角速度出力に乗るノイズ」のみがQとして加えられている

hrpsys-base/rtc/KalmanFilter/EKFilter.h

Line 83 in 7263626

_P_a_priori = F * P * F.transpose() + VQVt;

ため、試しに②③も加えてみたところ実機での推定結果が良くなった、という経緯です。

2については僕も試したことがあってあまり変わらなかった気がしますが、1と2の両方が効いている感じなのでしょうか？
先っぽにIMUがついている6軸ロボットが適当に動いて、FKから求めた姿勢角と推定した姿勢角がどのくらい一致するかのtestコードがhrpsysにあると思うので、それで挙動の変化を見てみると良いと思います。

ありがとうございます。事情によりすぐにはテストできないかもしれませんが、今度確認いたします。

[eisoku9618]

なるほどですね．

ちなみに，当時の数式の導出の履歴が以下のkf.pdfに書いてあって，今回話題になっている部分はp10-11にかかっている部分に書いてありました．式(229)が重要そうです．
https://github.com/eisoku9618/kuroiwa_demos/releases/tag/v1.0.4

当時の僕は意図的にQの右下を0にしていたようです．

これと @Naoki-Hiraoka さんの話と合わせて考えると，（文章で伝わる気があまりしませんが）RPYKalmanFilter・QuaternionEKFのどちらにおいてもシステムの入力を「ジャイロのみ」として捉えていたのが良くなくて，システムの入力を「ジャイロ」＋「バイアス項の微分」として捉えてシステムの運動方程式を立ててあげると全てが繋がりそうです．
こうすると #1259 (comment) で言うところの1と2と，上のPDFで言うところの式(229)が連動してきて，実験データから最適なパラメータを設定できそうです．