四元数のダウンサンプリング(あるいは平均値)

　実験中のAHRS、ジャイロ(ARS: angular rate sensor)を積分したものと、加速度・地磁気から求めた、独立した2つが得られる。加速度・地磁気は積分しないので長期的な安定性は高い反面、ノイズが多く、短期的な安定性は非常に悪い。フィルタリングしたい。

　システム的には512Hzで計算しているので(ただし地磁気センサはもっと遅い)、これを単純なFIR LPFに通そうとすると、かなり大変なことになる(最低でも250タップ程度は必要になる)。

　ということで、以前SDRで遊んだときに覚えた、CIC+FIRの組み合わせを試してみた。

　CICで16分の1にダウンサンプリングしてからFIRで0.25に絞って最終的にDC付近ではフラットで、ある程度急峻な特性になるはず。

***

　フィルタの応答

　青がCICの応答、オレンジがFIRの応答、赤が総応答。

　2Hzあたりまでフラットで4Hzで-6dBくらい、帯域外は-50dBから-70dBくらい、最大で-110dBくらい、という感じの特性になる。-50dBだと300分の1未満くらい、-100dBだと1万分の1くらい。

　CICは浮動小数点演算ができないので、FP32ベースの四元数を一旦I32ベースにキャストしてからCICに通して、もう一度FP32に戻してからFIRに入れている(CICの前後で適当なスケーリングを行っている)。

　arm_mathだと固定小数点の三角関数とかも用意されてるし、適当なタイミングで四元数の正規化もやってるので、最初から最後まで固定小数点で処理してもいいのかもしれないけど。キッチリCortexに最適化すればそこそこパフォーマンス出るのかもしれないけど、今の所浮動小数点演算でも問題なさそうな感じ。そもそも四元数って基本的に積和だけで計算できるんで、FPU積んでたら浮動小数点でもネックになるような処理はないし。ARSから姿勢変化量を計算するときにsinとcosを1回ずつ呼ぶだけだから、三角関数の演算は大した負荷じゃない(加速度・地磁気→姿勢は外積取るだけなので三角関数自体が不要)。オイラー角に変換するときはatan2を2回とasinを呼ぶので、こっちのほうが計算量は多い。もっとも、せいぜい秒16回とか32回とか、その程度なので姿勢の伝搬に比べたら大した量ではない。

***

　で、適当に揺らしたのをオンボードで計算してグラフ化してみた図

　先行する2つ、なめらかな方がARS、ノイズ混じりな方が加速度+地磁気による姿勢。0.5秒分遅れているのが加速度+地磁気にフィルタを通したもの。姿勢を64Hzで吐き出してグラフ化しているので、ダウンサンプリングした姿勢(32Hz)はステップ状の出力になる。

　全体的にノイズは減ってるような気はする。ただ、ARSからの姿勢と結構食い違ってる気がする。これがフィルタリングによる副作用なのか、センサの特性の問題(ARSのゲインとか)なのか、よくわからない。

　そもそもちゃんとフィルタリングできているのかもわからないし。

　やっぱりステッピングモータとかに載せて適当なサインカーブの姿勢変動与えてみるしかないかな? しっかり位置制御できるならSincカーブとか与えてもいいのかもしれないけど。

***

　ジャイロセンサって、正負でゲインエラーが変わる、みたいな特性ってあるんだろうか? なんかそんな感じの気がする。もしそうだとしたらちょっと面倒なことになりそう。やっぱりちゃんと特性とってみないとだめかな?

***

　しばらく前にやった大掃除以降、Nucleoを割ったST-Link v2が行方不明で、しょーがないからV3買うか、とか思った矢先に出てきた。また買うタイミングを逃した。。。一緒に9軸センサとかも買い足そうかと思ってたんだけど。