$$
\newcommand{\f}{\frac}
\newcommand{\vd}[3]{\begin{pmatrix}{#1}\\{#2}\\{#3}\end{pmatrix}}
\newcommand{\w}{\omega}
$$
# ロボゼミレポート

工学部　マテリアル工学科　C03-190697 高松　周平

今回のNHKロボコン２０１９において僕が担当した、IMU、3DLRF、mr2用ビジュアライザなどについて簡単にまとめます。
## IMU
* quaternionとUKFを使いたい
* ネットはオイラー角とEKFしか書いてない
### パラメーター
* gyro_offset:0点補正用の値
* gyro_scale:raw->rad/sにするパラメータ
* $output=(rawval-offset)* scale$
* acc_offset:gyroと同様
* acc_scale:gyroと同様
* $output=(rawval-offset)* scale$
* g=gravity_acc
* global_sys->imu_sys
* ここまででとりあえずimu_sysでの加速度、角速度は求まる
### ルンゲ・クッタ法
$$
f(x,t)=\f{dx}{dt}(x,t)\\
x_{n+1}=x_{n}+\f{1}{6}(k_1+2k_2+2k_3+k_4)\\
dt=t_{n+1}-t_{n}\\
k_1=dtf(x_n,t_n)\\
k_2=dtf(x_n+\f{k_1}{2},t_n+\f{dt}{2})\\
k_3=dtf(x_n+\f{k_2}{2},t_n+\f{dt}{2})\\
k_4=dtf(x_n+k_3,t_n+dt)\\
$$
### 状態方程式
$$
Q_{f→i},\ X_{f→i}\\
X_i = {Q_{f→i}}^{-1}(X_f - X_{f→i})\\
Q_i=Q_{f→i}Q_f\\
G_f=\vd{0}{0}{g}\\
A_i,\ \w_iが毎周期くる\\
A_i = {Q_{f→i}}^{-1}G_fとしてQ_{f→i}を推定する\\
|A_i|\risingdotseq|G_f|=gのときのみ行う\\
推定されたQ_{f→i}がもとの値と近ければ補正を行う？\\
その判断はQ_{before}\times {Q_{estimate}}^{-1}の\thetaが小さければよい？？
$$
### 参考ページ
* [IMUについてまとめ](https://myenigma.hatenablog.com/entry/2015/11/09/183738)
* [IMUでEKF](https://hamachannel.hatenablog.com/entry/2018/05/13/155504)
* [hoge](https://garchiving.com/calculate-angle-of-3axis-rotation-in-6axis-sensor/#toc2)

## 3DLRF
### UKF
前のセメスターで実装した、UKFを用いて自己位置推定を行った。なんとか動くようになったが、計算量が多く間に合わなかった。ここからは別の人が実装したのだが、３DLRFによる自己位置の補正段のが多いので、EIFを新たに実装しこれを用いることで計算時間を間に合わせた。
### EIF
詳しく実装を書きはしないが、情報フィルターは状態の分散共分散行列の逆行列を状態としてもつ。こうすることで補正段の計算が逆行列を取る作業がなくなり計算量がへるというものだ。更新段が多ければカルマンフィルター、補正段が多ければ情報フィルターを用いると良いのだろう。
## mr2ビジュアライザ
OpenGLとgtkmmを用いてmr2のビジュアライザを作った。下の画像のようなものだ。