これまでの不具合

hrpsysに入っていたKFは，内部状態をオイラー角にするために加速度センサの値からオイラー角を求めているが，これのせいで特異点が出てしまう．

ので，加速度をRPYに変換しなければ良くて，そうするには加速度センサの値を変換すること無く使えば良い．

そもそも加速度センサをRPYに変換することで線形になって（？）いて嬉しかったが，拡張カルマンフィルタを使えば非線形でもOKなのでそれを使う．

拡張カルマンフィルタ

$$ \begin{eqnarray} 加速度センサの出力　&：&　\boldsymbol{a_k}\\ ジャイロセンサの出力　&：&　\boldsymbol{\omega_k}\\ ほしい物　&：&　ルートリンクに対する姿勢\boldsymbol{q_k}\\ 状態量\boldsymbol{x_{k}}　&：&　\boldsymbol{q_k}（これにプラスでドリフト項も必要？）\\ 状態方程式　&：&　\boldsymbol{x_{k}} = f(\boldsymbol{x_{k-1}}, \boldsymbol{u_{k}}, \boldsymbol{w_{k}})\\ 入力　&：&　\boldsymbol{u_{k}} = \boldsymbol{\omega_k}　（ジャイロ）\\ 時間変移に関する雑音 process noise　&：&　\boldsymbol{w_{k}}\\ 観測方程式　&：&　\boldsymbol{z_{k}} = h(\boldsymbol{x_{k}}, \boldsymbol{v_{k}})\\ 出力　&:&　\boldsymbol{z_{k}} = \boldsymbol{a_{k}} （加速度）\\ 観測雑音 observation noise &：&　\boldsymbol{v_{k}}\\ \end{eqnarray} $$

式

基本的にはwikiに従っていて，

状態方程式

$$ x_{k+1} = f(x_{k}, u_{k}) + w_{k}\\ w_k \sim N(0, Q_k) $$

観測方程式

$$ z_k = h(x_k) + v_k\\ v_k \sim N(0, R_k) $$

f と h の詳細

状態量$x$はworld座標から見たIMUセンサの位置姿勢，入力$u$はジャイロセンサ，出力$z$は加速度センサとする．

fは内部状態である現在の姿勢と入力であるジャイロから次の時刻の姿勢を求める関数であり，なんでこうなるかは不明だが，A_sensor_fusion_algorithm_for_an_integrated_angular_position_estimation_with_inertial_measurement_unitsの式（1）によると，

$$ \begin{eqnarray} &&\dot{x_k} = \frac{1}{2}\begin{pmatrix} 0 & -{\omega}_x & -{\omega}_y & -{\omega}_z \\ {\omega}_x & 0 & {\omega}_z & -{\omega}_y \\ {\omega}_y & -{\omega}_z & 0 & {\omega}_x \\ {\omega}_z & {\omega}_y & -{\omega}_x & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & \frac{x_{k+1} - x_k}{dt} = \frac{1}{2} \begin{pmatrix} 0 & -{\omega}_x & -{\omega}_y & -{\omega}_z \\ {\omega}_x & 0 & {\omega}_z & -{\omega}_y \\ {\omega}_y & -{\omega}_z & 0 & {\omega}_x \\ {\omega}_z & {\omega}_y & -{\omega}_x & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & x_{k+1} = x_k + \frac{dt}{2} \begin{pmatrix} 0 & -{\omega}_x & -{\omega}_y & -{\omega}_z \\ {\omega}_x & 0 & {\omega}_z & -{\omega}_y \\ {\omega}_y & -{\omega}_z & 0 & {\omega}_x \\ {\omega}_z & {\omega}_y & -{\omega}_x & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & x_{k+1} = \begin{pmatrix} 1 & -{\omega}_x \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} \\ {\omega}_x \cdot \frac{dt}{2} & 1 & {\omega}_z \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} \\ {\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} & 1 & {\omega}_x \cdot \frac{dt}{2} \\ {\omega}_z \cdot \frac{dt}{2} & {\omega}_y \cdot \frac{dt}{2} & -{\omega}_x \cdot \frac{dt}{2} & 1 \end{pmatrix}_k x_k \\ & \Leftrightarrow & x_{k+1} = f(x_{k}, u_{k}) \end{eqnarray} $$

となるので， $$ F_k = \frac{\partial f}{\partial x} = \begin{pmatrix} 1 & -{\omega}_x \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} \\ {\omega}_x \cdot \frac{dt}{2} & 1 & {\omega}_z \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} \\ {\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} & 1 & {\omega}_x \cdot \frac{dt}{2} \\ {\omega}_z \cdot \frac{dt}{2} & {\omega}_y \cdot \frac{dt}{2} & -{\omega}_x \cdot \frac{dt}{2} & 1 \end{pmatrix}_k $$ となる．

ポイントとして，クォータニオン計算便利ノートの2枚目の右上二行目にあるように， $$ \begin{eqnarray} &&x_{k+1} = \begin{pmatrix} 1 & -{\omega}_x \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} \\ {\omega}_x \cdot \frac{dt}{2} & 1 & {\omega}_z \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} \\ {\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} & 1 & {\omega}_x \cdot \frac{dt}{2} \\ {\omega}_z \cdot \frac{dt}{2} & {\omega}_y \cdot \frac{dt}{2} & -{\omega}_x \cdot \frac{dt}{2} & 1 \end{pmatrix}_k x_k\\ &\Leftrightarrow& x_{k+1} = x_k * quaternion\left(1, {\omega}_x \frac{dt}{2}, {\omega}_y \frac{dt}{2}, {\omega}_z \frac{dt}{2}\right) \end{eqnarray} $$ となること．

一方，hは内部状態である姿勢から出力である加速度センサの値を求める関数なので，直立時の加速度センサの値が重力であることを使って， $$ x_k = \left( \begin{array}{c} w \\ x \\ y \\ z \end{array} \right) $$

としたとき，ここの「クォータニオン→回転行列」を参考にして，

$$ \begin{eqnarray} &&z_k = \begin{pmatrix} w^2 + x^2 - y^2 - z^2 & 2xy + 2wz & 2xz - 2wy \\ 2xy - 2wz & w^2 - x^2 + y^2 - z^2 & 2yz + 2wx \\ 2xz + 2wy & 2yz - 2wx & w^2 - x^2 - y^2 + z^2 \end{pmatrix}_k^T \left( \begin{array}{c} 0 \\ 0 \\ 9.8 \end{array} \right)\\ &\Leftrightarrow& z_k = \left( \begin{array}{c} (2xz + 2wy) * 9.8 \\ (2yz - 2wx) * 9.8 \\ (w^2 - x^2 - y^2 + z^2) * 9.8 \end{array} \right)\\ &\Leftrightarrow& z_k = h(x_k) \end{eqnarray} $$ となる．

ここに転置が入るかどうかがポイントで，Quaternion-Based_Extended_Kalman_Filter_for_Determining_Orientation_by_Inertial_and_Magnetic_Sensingの式(7)の真ん中やA_sensor_fusion_algorithm_for_an_integrated_angular_position_estimation_with_inertial_measurement_unitsの式(3)では回転行列に転置が入っていないが，四元数を使ってみよう！の式(2)では回転行列を転置して使っていて，自分調べでは後者が正しい．

よって， $$ H_k = \frac{\partial h}{\partial x} = 2 * 9.8 * \begin{pmatrix} y & z & w & x \\ -x & -w & z & y \\ w & -x & -y & z \end{pmatrix} $$

ちなみに，An_Introduction_to_the_Kalman_Filterによると，

$$ \begin{eqnarray} x_k &=& \tilde{x_k} + F(x_{k-1}-\hat{x_{k-1}}) + Ww_{k-1}\\ z_k &=& \tilde{z_k} + H(x_k - \tilde{x_k}) + Vv_k\\ F &=& \frac{\partial f}{\partial x} \\ W &=& \frac{\partial f}{\partial w} \\ H &=& \frac{\partial h}{\partial x} \\ V &=& \frac{\partial h}{\partial v} \end{eqnarray} $$

であるが，

$$ x_{k+1} = \begin{pmatrix} 1 & -{\omega}_x \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} \\ {\omega}_x \cdot \frac{dt}{2} & 1 & {\omega}_z \cdot \frac{dt}{2} & -{\omega}_y \cdot \frac{dt}{2} \\ {\omega}_y \cdot \frac{dt}{2} & -{\omega}_z \cdot \frac{dt}{2} & 1 & {\omega}_x \cdot \frac{dt}{2} \\ {\omega}_z \cdot \frac{dt}{2} & {\omega}_y \cdot \frac{dt}{2} & -{\omega}_x \cdot \frac{dt}{2} & 1 \end{pmatrix}_k x_k + \left( \begin{array}{c} {w_k}_0 \\ {w_k}_1 \\ {w_k}_2 \\ {w_k}_3 \end{array} \right) $$

$$ z_k = \left( \begin{array}{c} (2xz + 2wy) * 9.8 \\ (2yz - 2wx) * 9.8 \\ (w^2 - x^2 - y^2 + z^2) * 9.8 \end{array} \right) + \left( \begin{array}{c} {v_k}_0 \\ {v_k}_1 \\ {v_k}_2 \end{array} \right) $$

と考えると，$W$も$V$も単位行列ということになる．

論文

Quaternion-Based_Extended_Kalman_Filter_for_Determining_Orientation_by_Inertial_and_Magnetic_Sensingの式(3)はクォータニオンが普通のクォータニオンと違うので変なだけで，Implementation_and_Experimental_Results_of_a_Quaternion-Based_Kalman_Filter_for_Human_Body_Motion_Trackingの式(3)~(6)やA_sensor_fusion_algorithm_for_an_integrated_angular_position_estimation_with_inertial_measurement_unitsの式（1）が正しいと思って良いと思っている．

ここの2枚目の右上のように，クォータニオンの積は行列で表せる．

バイアスも考える

ジャイロは積分されるので，ジャイロセンサのバイアスは無視出来ない．

事前にバイアスを求める
状態量にバイアス項を含めて推定する

という方針が考えられ，ここでは後者を書く．

姿勢を表すクォータニオンのみを状態変数にしていた場合の式において，バイアス項を考慮すると， $$ \begin{eqnarray} &&\dot{x_k} = \frac{1}{2}\begin{pmatrix} 0 & -({\omega}_x - b_x) & -({\omega}_y - b_y) & -({\omega}_z - b_z) \\ ({\omega}_x - b_x) & 0 & ({\omega}_z - b_z) & -({\omega}_y - b_y) \\ ({\omega}_y - b_y) & -({\omega}_z - b_z) & 0 & ({\omega}_x - b_x) \\ ({\omega}_z - b_z) & ({\omega}_y - b_y) & -({\omega}_x - b_x) & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & \frac{x_{k+1} - x_k}{dt} = \frac{1}{2} \begin{pmatrix} 0 & -({\omega}_x - b_x) & -({\omega}_y - b_y) & -({\omega}_z - b_z) \\ ({\omega}_x - b_x) & 0 & ({\omega}_z - b_z) & -({\omega}_y - b_y) \\ ({\omega}_y - b_y) & -({\omega}_z - b_z) & 0 & ({\omega}_x - b_x) \\ ({\omega}_z - b_z) & ({\omega}_y - b_y) & -({\omega}_x - b_x) & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & x_{k+1} = x_k + \frac{dt}{2} \begin{pmatrix} 0 & -({\omega}_x - b_x) & -({\omega}_y - b_y) & -({\omega}_z - b_z) \\ ({\omega}_x - b_x) & 0 & ({\omega}_z - b_z) & -({\omega}_y - b_y) \\ ({\omega}_y - b_y) & -({\omega}_z - b_z) & 0 & ({\omega}_x - b_x) \\ ({\omega}_z - b_z) & ({\omega}_y - b_y) & -({\omega}_x - b_x) & 0 \end{pmatrix}_k x_k \\ & \Leftrightarrow & x_{k+1} = \begin{pmatrix} 1 & -({\omega}_x - b_x) \cdot \frac{dt}{2} & -({\omega}_y - b_y) \cdot \frac{dt}{2} & -({\omega}_z - b_z) \cdot \frac{dt}{2} \\ ({\omega}_x - b_x) \cdot \frac{dt}{2} & 1 & ({\omega}_z - b_z) \cdot \frac{dt}{2} & -({\omega}_y - b_y) \cdot \frac{dt}{2} \\ ({\omega}_y - b_y) \cdot \frac{dt}{2} & -({\omega}_z - b_z) \cdot \frac{dt}{2} & 1 & ({\omega}_x - b_x) \cdot \frac{dt}{2} \\ ({\omega}_z - b_z) \cdot \frac{dt}{2} & ({\omega}_y - b_y) \cdot \frac{dt}{2} & -({\omega}_x - b_x) \cdot \frac{dt}{2} & 1 \end{pmatrix}_k x_k \end{eqnarray} $$ となる．

というわけで，状態変数にバイアス項を加えると， $$ \begin{eqnarray} \left( \begin{array}{c} w_{k+1} \\ x_{k+1} \\ y_{k+1} \\ z_{k+1} \\ {b_x}_{k+1} \\ {b_y}_{k+1} \\ {b_z}_{k+1} \end{array} \right) = \begin{pmatrix} w_k & + & -({\omega}_x - {b_x}_k) \cdot \frac{dt}{2} \cdot x_k & + & -({\omega}_y - {b_y}_k) \cdot \frac{dt}{2} \cdot y_k & + & -({\omega}_z - {b_z}_k) \cdot \frac{dt}{2} \cdot z_k\\ ({\omega}_x - {b_x}_k) \cdot \frac{dt}{2} \cdot w_k & + & x_k & + & ({\omega}_z - {b_z}_k) \cdot \frac{dt}{2} \cdot y_k & + & -({\omega}_y - {b_y}_k) \cdot \frac{dt}{2} \cdot z_k\\ ({\omega}_y - {b_y}_k) \cdot \frac{dt}{2} \cdot w_k & + & -({\omega}_z - {b_z}_k) \cdot \frac{dt}{2} \cdot x_k & + & y_k & + & ({\omega}_x - {b_x}_k) \cdot \frac{dt}{2} \cdot z_k\\ ({\omega}_z - {b_z}_k) \cdot \frac{dt}{2} \cdot w_k & + & ({\omega}_y - {b_y}_k) \cdot \frac{dt}{2} \cdot x_k & + & -({\omega}_x - {b_x}_k) \cdot \frac{dt}{2} \cdot y_k & + & z_k\\ {b_x}_k \\ {b_y}_k \\ {b_z}_k \end{pmatrix} \end{eqnarray} $$

姿勢を表すクォータニオンの微分と角速度の関係式

ロボットモーションのP62に書いてあって， $$ K_E = \begin{pmatrix} -x & w & z & -y \\ -y & -z & w & x \\ -z & y & -z & w \end{pmatrix} $$ としたときに， $$ \begin{eqnarray} \left( \begin{array}{c} \dot{w}\\ \dot{x}\\ \dot{y}\\ \dot{z} \end{array} \right) = \frac{1}{2} {K_E}^T \left( \begin{array}{c} w_x\\ w_y\\ w_z \end{array} \right) \end{eqnarray} $$ となるらしく，これを信じると上の方で書いた姿勢を表すクォータニオンの微分と角速度の関係式は成り立つことが分かる．

robotが加速する場合

Quaternion-Based Extended Kalman Filter for Determining Orientation by Inertial and Magnetic Sensingの式（13）に書いてあるように，ロボットが加速した場合は加速度センサの値を信頼しないようにするのが良さそう．優先度低め．

RPYの拡張カルマンフィルタ

hrpsysに実装されているのは $$ \left( \begin{array}{c} \dot{\alpha} \\ \dot{\beta} \\ \dot{\gamma} \end{array} \right) = \left( \begin{array}{ccc} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{array}\right )\left( \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end{array} \right) $$

という前提でやっていたが，よく考えると

$$ \begin{array}{cc} & \left( \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end{array}\right) = \left( \begin{array}{ccc} \cos \beta \cos \gamma & - \sin \gamma & 0 \\ \cos \beta \sin \gamma & \cos \gamma & 0 \\ - \sin \beta & 0 & 1 \end{array}\right ) \left( \begin{array}{c} \dot{\alpha} \\ \dot{\beta} \\ \dot{\gamma} \end{array}\right) \\ \Leftrightarrow & \left( \begin{array}{c} \dot{\alpha} \\ \dot{\beta} \\ \dot{\gamma} \end{array}\right) = \left( \begin{array}{ccc} \frac{\cos \gamma}{\cos \beta} & \frac{\sin \gamma}{\cos \beta} & 0\\ -\sin \gamma & \cos \gamma & 0 \\ \tan \beta \cos \gamma & \tan \beta \sin \gamma & 1 \end{array}\right ) \left( \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end{array}\right) \end{array} $$

なので，hrpsysに実装されているのは$\alpha = 0,\beta = 0, \gamma = 0$付近であることが分かった．逆行列を求める部分はここを利用した．

ので，これ故hrpsysは線形カルマンフィルタであったが，非線形にしないといけない．

具体的には $$ \left( \begin{array}{c} \alpha_{k+1} \\ \beta_{k+1} \\ \gamma_{k+1} \\ {b_x}_{k+1} \\ {b_y}_{k+1} \\ {b_z}_{k+1} \end{array}\right) = \left( \begin{array}{c} \alpha_k + \frac{\cos \gamma_k}{\cos \beta_k} \left( {\omega_x}_k - {b_x}_k \right) dt + \frac{\sin \gamma_k}{\cos \beta_k} \left( {\omega_y}_k - {b_y}_k \right) dt \\ \beta_k - \sin \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt + \cos \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt \\ \gamma_k + \tan \beta_k \cos \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt + \tan \beta_k \sin \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt + \left( {\omega_z}_k - {b_z}_k \right) dt\\ {b_x}_k \\ {b_y}_k \\ {b_z}_k \end{array}\right) $$ となるべき．

これをもとに$F$を算出すると $$ F = \left( \begin{array}{c|c|c|c|c|c} 1 & \frac{\tan \beta_k}{\cos \beta_k} \cos \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt + \frac{\tan \beta_k}{\cos \beta_k} \sin \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt & \frac{- \sin \gamma_k}{\cos \beta_k} \left( {\omega_x}_k - {b_x}_k \right) dt + \frac{\cos \gamma_k}{\cos \beta_k} \left( {\omega_y}_k - {b_y}_k \right) dt & - \frac{\cos \gamma_k}{\cos \beta_k} dt & - \frac{\sin \gamma_k}{\cos \beta_k} dt & 0 \\ \hline 0 & 1 & - \cos \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt - \sin \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt & \sin \gamma_k dt & - \cos \gamma_k dt & 0\\ \hline 0 & \frac{1}{\cos^2 \beta_k} \cos \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt + \frac{1}{\cos^2 \beta_k} \sin \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt & 1 - \tan \beta_k \sin \gamma_k \left( {\omega_x}_k - {b_x}_k \right) dt + \tan \beta_k \cos \gamma_k \left( {\omega_y}_k - {b_y}_k \right) dt & - \tan \beta_k \cos \gamma_k dt & - \tan \beta_k \sin \gamma_k dt & - dt \\ \hline 0 & 0 & 0 & 1 & 0 & 0 \\ \hline 0 & 0 & 0 & 0 & 1 & 0 \\ \hline 0 & 0 & 0 & 0 & 0 & 1 \end{array} \right) $$ となる．

一方，$H$は $$ H = \left( \begin{array}{cccccc} 1 & 0 & 0 & 0 & 0 & 0\\ 0 & 1 & 0 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ \end{array} \right) $$