• 機体姿勢を表す「Quatenion」を「$q_n^b$」で表す時、その時間微分と機体角速度の関係は、以下のようになる。

$\Large \displaystyle \frac{d}{dt}q_n^b=\frac{1}{2}q_n^b*[\vec{\omega}^b]_q$

• ここで、「$\vec{\omega}^b$」は、機体角速度を表す3次元ベクトルである。
• 「$[\cdot]_b$」は、任意の3次元ベクトルを「Quatenion」に変換するオペレータであり、次式で表される。

$\Large \displaystyle [\vec{\omega}^b]_q= \begin{bmatrix} 0 \\ \vec{\omega}^b \\ \end{bmatrix} $

$\Large \displaystyle \frac{d}{dt}q_n^b=\frac{1}{2} \begin{bmatrix} q_0 \\ \vec{q}_n^b \\ \end{bmatrix} * \begin{bmatrix} 0 \\ \vec{\omega}^b \\ \end{bmatrix}$

$\Large \displaystyle = \frac{1}{2} \begin{bmatrix} 0p_0 - \vec{q}_n^b \cdot \vec{\omega}^b\\ 0\vec{q}_n^b + q_0\vec{\omega}^b+ \vec{q}_n^b \times \vec{\omega}^b\\ \end{bmatrix} $

$\Large \displaystyle = \frac{1}{2} \begin{bmatrix} - \vec{q}_n^b \cdot \vec{\omega}^b\\ q_0\vec{\omega}^b - \vec{\omega}^b \times \vec{q}_n^b\\ \end{bmatrix} $

• 今、「$\vec{\omega}^b$」は、ジャイロセンサーにより計測できるが、ジャイロセンサーの出力にはバイアス誤差が含まれる。
• ここでは、そのバイアス誤差を考慮した定式化を行う必要がある。
• 経験的にジャイロセンサーのバイアス誤差「$\dot{\omega}_{bias}$」は、次式のダイナミクスを持つ事が知られている。

$\Large \displaystyle \dot{\vec{\omega}}_{bias}= \begin{bmatrix} -\beta_x & 0 & 0\\ 0 & -\beta_y & 0 \\ 0 & 0 & -\beta_z \\ \end{bmatrix} \vec{\omega}_{bias} +\vec{w}$

• 但し、「$\vec{w}$」は、ホワイトノイズを表している。
• 又、「$\beta$」は、ジャイロセンサーの種類や性能によって決まる定数であり、ジャイロセンサーの静置試験を行って取得したデータから決定すことが出来る。

$\Large \displaystyle $

• GyroscopeEvent は回転速度(rad/s)で提供されるため，回転速度 ＊ S ＊ 180/pi で度数法に変換しています．
• _maxAngle で _maxForegroundMove 分移動するということを表しています．
• 今回の場合では，x 軸に 120 度の回転を加えると，その軸の方向に対して 50 動くことになります

• 大量の粒子をランダムに生成する。
• 粒子には位置や向きといった推定したい状態変数を好きなだけ持たせて構わない。
• それぞれの粒子は重みを持ち、系の実際の状態と粒子の状態が合致する可能性が重みによって (確率的に) 表される。
• 最初の重みは全て同じとする。

• 粒子の次の状態を予測する。
• あなたが予測した実際の系の振る舞いに基づいて、全ての粒子を移動させる。

• 観測値に基づいて粒子の重みを更新する。
• 観測値に近い粒子には観測値から遠い粒子よりも大きい重みが割り当てられる。

• 確率が非常に低い粒子を破棄して、確率の高い粒子のコピーで置き換える。

• もし必要なら、粒子の重み付き平均と重み付き共分散行列を計算して状態の推定値を得る。

• 以下の、初期値時刻「$t=t_0$」と「$N$」個の潜在変数を定めます。

$\Large \displaystyle \{ x_{t_0}^{(n)}\}_{n\in [1,...,N]}$

• 粒子フィルタではこの潜在変数の集合の元を粒子と呼び、粒子を更新していくことで潜在変数を推定していきます。
• 粒子の初期値は適当に1つずつ決めてもいいし，乱数で決めていってもいいし，通常は全て同じ値で初期化することさえ問題ありません．
• 今回は全て同じ値で初期化することにします
• (今回はハイパーパラメータ「$\theta_{sys}$」も「$\theta_{obs}$」もこの段階で何らかの値に定めます．色んな値に変えて遊びましょう．)

• 各粒子の1単位時間先の潜在変数をサンプリングします．

$\Large \displaystyle \forall n\ x_i^{(n)}\sim p(x_t^{(n)}|x_{t-1}^{(n)})$

• このステップは各粒子ごとに独立なため，並列に実行することが可能です．
• 各粒子で同じ値のものがあったとしても，サンプリングが確率的に揺らぐようなモデルである限りは1期先の予測はバラバラな値に予測されます．
• こういう事情から粒子が同じ値をもつことを許しても何の問題もないわけです．

• 所与の「$y_t$」と先に得られた潜在変数の予測値「$x_t^{(n)}$」を観測モデルに代入することで予測確率を計算します．

$\Large \displaystyle \mu_t^{(n)}=p(y_t|x_t^{(n)})$

• 各粒子の予測確率を計算したらこれらを正規化します．

$\Large \displaystyle \tilde{\mu}_t^{(n)}=\frac{\mu_t^{(n)}}{\sum_{n=1}^N\mu_t^{(n)}}$

• 所与の「$y_t$」と先に得られた潜在変数の予測値「$x_t^{(n)}$」を観測モデルに代入することで予測確率を計算します．

$\Large \displaystyle \forall n\ c_t^{(n)}\sim Categorical(c_t^{(n)}|)$

$\Large \displaystyle x_t^{(n)}=c_t^{(n)^T} \begin{pmatrix} x_t^{(1)} \\ \vdots \\ x_t^{(N)} \end{pmatrix} $

• 「$c_t^{(n)} \in \{ 0,1\}^N$」は、ある成分の値が1でそれ以外が0の値を持つ「$N$」次元のベクトルです．
• 「$c_t^{(n)} $」は、ステップ2で計算した重み「$\tilde{\mu}_t$」をパラメータとするカテゴリカル分布から生成します．
• このステップによって実現確率の低い粒子は実現確率のより高い粒子に取って代わられる可能性が出てきます．
• 極限的には最適な粒子が大多数を占めるようになっていく感じです．
• 「$t\rightarrow t+1$」として、ステップ1に戻ります．

$\large \displaystyle \large 1:Algorithm Particle_filter(X_{t-1},u_t, z_t):\\ \large 2:\ \ \ \bar{X_t}=X_t = 0\\ \large 3:\ \ \ for\ M=1\ to\ M\ do\\ \large 4:\ \ \ \ \ \ sample\ x_t^{[m]}\sim p(x_t|u_t, x_{t-1}^{[m]})\\ \large 5:\ \ \ \ \ \ w_t^{[m]}=p(z_t|x_{t}^{[m]})\\ \large 6:\ \ \ \ \ \ \bar{X_t}=\bar{X_t}+\lt x_t^{[m]},w_t^{[m]}\gt\\ \large 7:\ \ \ endfor\\ \large 8:\ \ \ for\ M=1\ to\ M\ do\\ \large 9:\ \ \ \ \ \ draw\ i\ with \ probalility\propto w_t^{[i]}\\ \large 10:\ \ \ \ add\ x_t^{[i]}\ to\ X_t\\ \large 11:\ endfor\\ \large 12:\ return\ X_t $

• Byte0: 73
• Byte1: 40
• Byte2: 64
• bit1: 1=人の動きを検知、0=人の動きを不検知
• Byte3: 00
• Byte4: 42
• 動体を検出してからの時間（秒）
• Byte5: 01
• bit6: 明るい
• bit7: 暗い