IMU

　2ヶ月くらい前に作ってたやつ、中断してたのを再開した。
　前回は、マイコン側はセンサからデータを読み出し、RAWデータをPCに送り、PCで四元数に反映していた。
　今回は、スタンドアロンで動作するように改造している。

　C#のVector3とQuaternionをC++に移植して、マイコン内で四元数を積分している。
　前回は加速度・地磁気によって絶対姿勢を求め、角速度で短期的な、加速度・地磁気で長期的な姿勢の変化を推定していた。
　今回は、とりあえず動作確認が目的なので、角速度での姿勢更新のみを実装している。また、ジャイロのオフセットは補正していない。

　机の上に置いて、姿勢変化が起きない状態でのグラフ。

　x,y,zがそれぞれの軸のみを積分した値、azi,ele,rollが四元数から計算した値。

　x,y,zはセンサのオフセットを補正していないので、およそ1次関数に近似できる形になっている。
　一方で、この状態はひねりを加えた姿勢変化と同様なので、azi,ele,rollはうねうねとした形になる。

　とりあえず、誤差はあれど、センサを動かせばAzi/Ele/Rollが適切に変化するのは確認できている。

***

　缶サットのような用途で使う場合、打ち上げ時に大きな衝撃と加速度が加わる。また、飛行時は気流の影響などによって常に姿勢が変化していく。そのため、打ち上げから着地までは、加速度・地磁気による絶対姿勢の推定は行えない可能性が高い。
　となると、いかに角速度だけで正確な姿勢を推定するか、というのが問題になる。

　ロケットで低高度(100m程度)へ打ち上げる場合、打上から30秒程度の時間で着地するから、その間の積分誤差が許容値以下であれば問題ない。
　逆に、ドローンや気球から投下する場合は、ロケットのような強い衝撃が無い反面、飛行時間は長いと考えられる。

***

　誤差の推定は、拡張カルマンフィルタとか使えばいいんだろうけど、アレはなかなか謎いからなぁ。
　例えば、x軸に+オフセット、-オフセット、ゼロオフセット、のように3種類の値を作り、yzでも同様にし、ゼロオフセットを姿勢の推定に使い、他のソースからの姿勢を反映する場合は、9種類の中から一番誤差が少ないオフセット量を採用し、以降でも同じように処理する、というふうにすれば、処理の内容は理解しやすいけど、計算量が10倍程度になってしまう。

　誤差がどれくらいかといった部分も含めて、今後の課題。


　倍精度が使えれば演算誤差の問題は減るけど、演算速度が犠牲になる。まぁ、ソフト実装の倍精度でも衛星軌道計算とかヨユーでこなせるので、四元数の計算くらいなら問題ないと思う。

　F7系が乗ったNucleoでは倍精度FPUが乗ったモノもあるらしい。3000円くらいで売ってるらしいので、性能比で考えればSTBee F4miniより相当コスパがいい。けど、Nucleoって、Discoveryほどじゃないにしても結構ゴテゴテいろいろついてるイメージなんだよなぁ。ST-Linkとか、ST-Linkとか、あとST-Linkとか。。。割ればいいんだけど。


　某ロケットはFPGAで倍精度浮動小数点を計算していて、云々、というのを読んだ気がするんだけど、ちゃんと読んでないので記憶が曖昧。気が向いたら読み直そうっと。