適応カルマンフィルタの実装

[ShingoNishimoto]

Summary （概要）

適応カルマンフィルタを実装する．

Detail （詳細）

processノイズのパラメータとかを固定にするんではなくて，推定状況に合わせて適応させる適応カルマンフィルタを実装することで，精度の改善を図る．

Impact （影響範囲）

大：tool類が全部死ぬ... みたいな

Priority （重要度）

小：まあ暇なら，みたいな

Note （補足）

何かれば

[ShingoNishimoto]

changed due date to July 15, 2022

[ShingoNishimoto]

経験加速度がうまく推定できてなさそうで，これの影響が波及している感じがする．

[ShingoNishimoto]

とりあえず，residual based Q matchingと，innovation based R matchingの実装を行い結果がどうなるかを確認した．

clockの精度は上がっている．これはおそらく経験加速度のパラメタがちょうどよい値にチューニングされたからではないかなと考えている．

ただ，位置・速度における誤差共分散が時間更新時に大きくなりすぎているのでその原因が何なのかを調べた方がよさそう．

[ShingoNishimoto]

全体としての精度が下がってしまう原因，特に時間更新の部分はQの成分でa, cdt以外の成分がこの計算によって出てくるからと考えられる．ただ，それを防ごうと，a, cdtの成分だけを更新したりしてもうまくいかなかった．

Dynamic noise scalingはやってないので試す価値があるのかもしれない．ただ，STMが観測更新時に必要になるのでそれをどうするべきかを考える必要がある．このSTMを作るときの状態量は観測更新後のものにするのかなど．

[ShingoNishimoto]

STMは普通にKalmanフィルタで観測更新によって更新された直後のものを使用することにして，検証した．

結果，クロックの精度は悪かったが，直接プロセスノイズをinnovation-baseとかでアップデートするのに比べて格段に精度が上がった．これもどれくらいのforgetting factorを使うのかによって結果が変わってきそうではある．もう少し調べる余地はありそう．

[ShingoNishimoto]

クロックの精度が悪い一番の原因は経験加速度のところっぽい．これは適応フィルタの効果が薄れてしまっているからなのか？

[ShingoNishimoto]

経験加速度の部分をもっとしっかりと真値と比較して議論するためには #19 を解決しないことには無理．

なので，そこで考えることにしてマージして一旦closeする．

[ShingoNishimoto]

removed due date