はじめに

この記事では、拡張カルマンフィルタを用いて6軸IMUの姿勢推定を行います。はじめに拡張カルマンフィルタの式を確認します。続いて、IMUの姿勢推定をする際の状態空間モデルの作成方法、ノイズの共分散行列の設定方法、ヤコビ行列の計算方法、初期値の設定方法について説明します。最後にPythonで拡張カルマンフィルタを実装し、スマートフォン（IMU)の姿勢をProcessingを用いて可視化します。

拡張カルマンフィルタ

はじめに、拡張カルマンフィルタの式を確認します。拡張カルマンフィルタは線形カルマンフィルタを非線形モデルに適用できるよう拡張されたものです。基本的に線形カルマンフィルタと大差ありません。

状態空間モデル

\begin{align}
&{\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k})+{\boldsymbol {w}}_{k} \tag{1} \\

&{\boldsymbol  {z}}_{{k}}=h({\boldsymbol  {x}}_{{k}})+{\boldsymbol  {v}}_{{k}} \tag{2}
\end{align}

式(1)の状態方程式は状態の遷移を表し、式(2)の観測方程式は状態空間を観測空間に写像しています。

\begin{align}
&\boldsymbol {w}_k\sim {\mathcal {N}}(\boldsymbol {0} ,\boldsymbol {Q}_k ) \tag{3}\\
&\boldsymbol {v}_k\sim {\mathcal {N}} (\boldsymbol {0} ,\boldsymbol {R}_k ) \tag{4}
\end{align}

$\boldsymbol w_k$と$\boldsymbol v_k$はそれぞれ平均0、共分散行列$\boldsymbol Q_{k}$と$\boldsymbol R_{k}$の多変数正規（ガウス）分布に従う雑音です。

\begin{align}
&{{\boldsymbol {F}}_{k}}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k}} \tag{5} \\

&{{\boldsymbol {H}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}} \tag{6}
\end{align}

ヤコビアンを用いることによって、非線形関数である$f$と$h$を線形関数に近似します。線形カルマンフィルタとの大きな違いはここです。

予測と更新

予測

\begin{align}
&{\hat {\boldsymbol {x}}}_{k|k-1} = f({\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k}) \tag{7} \\

&{\boldsymbol {P}}_{k|k-1} ={{\boldsymbol {F}}_{k}}{\boldsymbol {P}}_{k-1|k-1}{{\boldsymbol {F}}_{k}^{\top }}+{\boldsymbol {Q}}_{k} \tag{8}
\end{align}

更新

\begin{align}
&{\tilde  {{\boldsymbol  {y}}}}_{{k}}={\boldsymbol  {z}}_{{k}}-h({\hat  {{\boldsymbol  {x}}}}_{{k|k-1}}) \tag{9}\\

&{\boldsymbol  {S}}_{{k}}={{{\boldsymbol  {H}}_{{k}}}}{\boldsymbol  {P}}_{{k|k-1}}{{{\boldsymbol  {H}}_{{k}}^{\top }}}+{\boldsymbol  {R}}_{{k}} \tag{10} \\

&{\boldsymbol  {K}}_{{k}}={\boldsymbol  {P}}_{{k|k-1}}{{{\boldsymbol  {H}}_{{k}}^{\top }}}{\boldsymbol  {S}}_{{k}}^{{-1}}  \tag{11} \\

&{\hat  {{\boldsymbol  {x}}}}_{{k|k}}={\hat  {{\boldsymbol  {x}}}}_{{k|k-1}}+{\boldsymbol  {K}}_{{k}}{\tilde  {{\boldsymbol  {y}}}}_{{k}} \tag{12} \\

&{\boldsymbol  {P}}_{{k|k}}=({\boldsymbol  {I}}-{\boldsymbol  {K}}_{{k}}{{{\boldsymbol  {H}}_{{k}}}}){\boldsymbol  {P}}_{{k|k-1}} \tag{13}

\end{align}

式(1)~(6)ができていれば、あとは式(7)~(13)を逐次計算するだけです。

6軸IMUの姿勢推定

実際に、拡張カルマンフィルタを6軸IMUの姿勢推定に適用していきます。私たちが頑張らなくてはいけないのは以下の３つだけです。

式(1)~(4)の定義
式(5)~(6)の計算
初期値の決定（${\hat {\boldsymbol {x} }}_{0|0}$と$\boldsymbol {P} _{0|0}$）

状態方程式 - 式(1)

はじめに、式(1)の状態方程式を考えていきます。今回、我々が求めたい状態とはIMUの姿勢です。姿勢の表現方法はいくつかありますが（オイラー角、回転行列、クォータニオンなど）、今回はオイラー角を使用します。オイラー角は基準座標系のxyz軸（ロール、ピッチ、ヨー）のそれぞれで回転した角度で姿勢を表現します。ロール角$\phi$、ピッチ角$\theta$、ヨー角$\psi$を用いて$\boldsymbol{x}_k$の推定値$\boldsymbol{\hat{x}}_k$は以下のように定義できます。

\boldsymbol{\hat{x}}_k
 = \begin{pmatrix}
\phi_k \\
\theta_k \\
\psi_k
\end{pmatrix} \tag{14}

サンプリング間隔を$\delta t$とするとき、$\delta t$間にロール、ピッチ、ヨーがそれぞれ$\delta \phi$、$\delta \theta$、$\delta \psi$だけ変化するならば、$\boldsymbol{\hat{x}}_k$は以下のように更新されます。

\boldsymbol{\hat{x}}_k
 = \begin{pmatrix}
\phi_{k} \\
\theta_{k} \\
\psi_{k}
\end{pmatrix}
 = \begin{pmatrix}
\phi_{k-1} + \delta \phi\\
\theta_{k-1} + \delta \theta\\
\psi_{k-1} + \delta \psi
\end{pmatrix}  \tag{15}

導出は省略しますが、$\delta \phi$、$\delta \theta$、$\delta \psi$は$\phi$、$\theta$、$\psi$とIMUで観測した角速度$\boldsymbol {w}=(w_x, w_y, w_z)$で表現できます。（導出の詳細は「３軸角速度センサによる姿勢推定」、「6軸IMU～拡張カルマンフィルタ」が大変参考になります）

\begin{pmatrix}
\delta \phi\\
\delta \theta\\
\delta \psi
\end{pmatrix}

= \begin{pmatrix}
1 & \frac{\sin\phi\sin\theta}{\cos\theta} & \frac{\cos\phi\sin\theta}{\cos\theta} \\
0 & \cos\phi & -\sin\phi \\
0 & \frac{\sin\phi}{\cos\theta} & \frac{\cos\phi}{\cos\theta}
\end{pmatrix}

\begin{pmatrix}
w_x \delta t\\
w_y \delta t\\
w_z \delta t
\end{pmatrix} \tag{16}

ここで制御入力$\boldsymbol {u}_k$を以下のように定義します。

\boldsymbol {u}_k
= \begin{pmatrix}
u_{k,x} \\
u_{k,y} \\
u_{k,z} 
\end{pmatrix}
= \begin{pmatrix}
w_{k,x} \delta t\\
w_{k,y} \delta t\\
w_{k,z} \delta t
\end{pmatrix} \tag{17}

式(15)~(17)より、状態方程式の$f$が求まりました。

\boldsymbol{\hat{x}}_k
 = \begin{pmatrix}
\phi_{k-1} + u_{k,x} + u_{k,y}\frac{\sin\phi\sin\theta}{\cos\theta} + u_{k,z}\frac{\cos\phi\sin\theta}{\cos\theta} \\
\theta_{k-1} + u_{k,y}\cos\phi - u_{k,z}\sin\phi\\
\psi_{k-1} + u_{k,y}\frac{\sin\phi}{\cos\theta} + u_{k,z}\frac{\cos\phi}{\cos\theta}
\end{pmatrix}
= f(\boldsymbol{\hat{x}}_{k-1},{\boldsymbol {u}}_{k})  \tag{18}

見るからに非線形な関数です。

観測方程式 - 式(2)

続いて、式(2)の観測方程式について考えていきます。すでに$\boldsymbol x_{k}$はオイラー角であることは決まっています。つまり、ここで考えるべきは、「観測値$\boldsymbol z_{k}$を何とするか」です。関数$h$は$\boldsymbol x_{k}$と$\boldsymbol z_{k}$によって決まります。

結論からいうと、加速度$\boldsymbol {a}=(a_x, a_y, a_z)$を用いて姿勢を推定し、それが$\boldsymbol z_{k}$となります。導出は省略しますが、$\boldsymbol z_{k}$は以下の式で表されます（基準座標系において重力加速度が-z軸方向を向いている場合）。（導出の詳細は「３軸加速度センサを用いた姿勢推定」、「6軸IMU～拡張カルマンフィルタ」が大変参考になります）

\boldsymbol z_{k}
= \begin{pmatrix}
\phi_k \\
\theta_k
\end{pmatrix}
= \begin{pmatrix}
\arctan(\frac{a_{k,y}}{a_{k,z}}) \\
\arctan(\frac{-a_{k,x}}{\sqrt{a_{k,y}^2 + a_{k,z}^2}})
\end{pmatrix} \tag{19}

$\boldsymbol z_{k}$にヨー角$\psi_k$がない理由を簡単に説明しておきます。加速度からIMUの姿勢を求める際、基準座標系では重力加速度が常に同じ方向を向いているという性質を用います。具体的には、センサ座標系で観測した加速度と基準座標系における重力加速度を一致させるための回転量を求めてやればいいのです。しかし、基準座標系のヨー軸と重力加速度の方向は常に平行です。そのため、加速度からヨー角を求めることはできないのです。

続いて、$h$を求めます。これは式(14)の$\boldsymbol x_{k}$と(19)の$\boldsymbol z_{k}$をみれば、以下の式になることは簡単にわかります。

h 
= \begin{pmatrix}
1 & 0 & 0 \\
0 & 1 & 0\\
\end{pmatrix} \tag{20}

$h$は非線形ではなく、線形写像となりました。

ノイズ - 式(3)(4)

ノイズである$\boldsymbol w_{k}$と$\boldsymbol v_{k}$を求めます。$\boldsymbol w_{k}$と$\boldsymbol v_{k}$はそれぞれ平均0、共分散行列$\boldsymbol Q_{k}$と$\boldsymbol R_{k}$の多変数正規（ガウス）分布に従う雑音です。共分散行列とはベクトルの要素間の共分散の行列です。ざっくりいうと共分散が大きいほど、状態遷移時、観測時に含まれる誤差が大きいということです。今回の場合は共分散の中でも自己共分散（共分散行列の対角成分）が重要となります。なぜなら、IMUのロール軸、ピッチ軸、ヨー軸は互いに直交しており独立しているからです。つまり、$\boldsymbol Q_{k}$と$\boldsymbol R_{k}$は以下の形状になることが予想されます。

\begin{pmatrix}
k_{\phi} & 0 & 0 \\
0 & k_{\theta} & 0 \\
0 & 0 & k_{\psi}
\end{pmatrix} \tag{21}

$k_{\phi}$、$k_{\theta}$、$k_{\psi}$はそれぞれ$\phi$、$\theta$、$\psi$の自己共分散です。この値は実験や事前知識により見積もります。例えば、$\boldsymbol Q_{k}$であれば角速度センサのノイズ特性やサンプリングレート、$\boldsymbol R_{k}$であれば加速度センサのノイズ特性に依存するでしょう。

実装の際には以下の値を使います。

\begin{align}
&\boldsymbol Q_{k}
= \begin{pmatrix}
0.0174 \delta t^2 & 0 & 0 \\
0 & 0.0174 \delta t^2 & 0 \\
0 & 0 & 0.0174 \delta t^2
\end{pmatrix} \tag{22} \\

&\boldsymbol R_{k}
= \begin{pmatrix}
\delta t^2 & 0 \\
0 & \delta t^2 \\
\end{pmatrix} \tag{23}
\end{align}

「6軸IMU～拡張カルマンフィルタ」の値を参考にさせていただきました。

ヤコビアンの計算 - 式(5)(6)

$f$を以下としたとき、

f(\boldsymbol{\hat{x}}_{k-1},{\boldsymbol {u}}_{k})
= \begin{pmatrix}
f_{\phi} \\
f_{\theta} \\
f_{\psi}
\end{pmatrix}  \tag{24}

式(18)と(24)より、$f$のヤコビアンは以下のように計算できます。

\begin{align}
{{\boldsymbol {F}}_{k}} 
&=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k}} \\
&= \begin{pmatrix}
\frac{\partial f_{\phi}}{\partial \phi} & \frac{\partial f_{\phi}}{\partial \theta} & \frac{\partial f_{\phi}}{\partial \psi} \\
\frac{\partial f_{\theta}}{\partial \phi} & \frac{\partial f_{\theta}}{\partial \theta} & \frac{\partial f_{\theta}}{\partial \psi} \\
\frac{\partial f_{\psi}}{\partial \phi} & \frac{\partial f_{\psi}}{\partial \theta} & \frac{\partial f_{\psi}}{\partial \psi} 
\end{pmatrix} \\
&= \begin{pmatrix}
1+u_{k,y}\frac{\cos{\phi}\sin\theta}{\cos\theta}-u_{k,z}\frac{\sin\phi\sin\theta}{\cos\theta} & u_{k,y}\frac{\sin\phi}{\cos^2\theta}+u_{k,z}\frac{\cos\phi}{\cos^2\theta} & 0 \\
-u_{k,y}\sin\phi-u_{k,z}\cos\phi & 1 & 0 \\
u_{k,y}\frac{\cos\phi}{\cos\theta}-u_{k,z}\frac{\sin\phi}{\cos\theta} & u_{k,y}\frac{\sin\phi\sin\theta}{\cos^2\theta}+u_{k,z}\frac{\cos\phi\sin\theta}{\cos^2\theta} & 1
\end{pmatrix} \tag{25}
\end{align}

初期値

最後に初期値について考えます。初期状態$\boldsymbol{\hat{x}}_{0}$が$\mathcal N(\boldsymbol 0 ,\boldsymbol \Sigma_0)$に従うとき、初期値は以下のように決まります。

\begin{align}
&{\hat {\boldsymbol {x} }}_{0|0}
=E{\bigl [}\boldsymbol {x}_{0}{\bigr ]}
=\boldsymbol {x}_{0}  \tag{26}\\

&\boldsymbol {P} _{0|0}
=E{\bigl [}\left(\boldsymbol {x}_{0}-{\hat {\boldsymbol {x} }}_{0}\right)\left(\boldsymbol {x} _{0}-{\hat {\boldsymbol {x} }}_{0}\right)^{T}{\bigr ]}
=\boldsymbol \Sigma_0  \tag{27}
\end{align}

実装の際には以下の値を使います。

\begin{align}
&\boldsymbol {x}_{0} 
= \begin{pmatrix}
0 \\
0 \\
0
\end{pmatrix} \tag{28}\\
&\boldsymbol \Sigma_0
= \begin{pmatrix}
0.0174 \delta t^2 & 0 & 0 \\
0 & 0.0174 \delta t^2 & 0 \\
0 & 0 & 0.0174 \delta t^2
\end{pmatrix} \tag{29}
\end{align}

初期状態ではセンサ座標系と基準座標系は一致していることにしました。$\boldsymbol \Sigma_0$の値は「6軸IMU～拡張カルマンフィルタ」の値を参考にさせていただきました。

実装

コードはGithubにあげてあります。

拡張カルマンフィルタを用いた6軸IMUの姿勢推定

基本的に式(1)~(13)を、Pythonに書き下しただけです。

imu_ekf.py

import numpy as np

def f(x, u):
    u_x, u_y, u_z = u[0][0], u[1][0], u[2][0]
    c1, s1 = np.cos(x[0][0]), np.sin(x[0][0])
    c2, s2 = np.cos(x[1][0]), np.sin(x[1][0])
    c3, s3 = np.cos(x[2][0]), np.sin(x[2][0])
    x = np.array([
        [x[0][0]+u_x+u_y*s1*s2/c2+u_z*c1*s2/c2],
        [x[1][0]+u_y*c1-u_z*s1],
        [x[2][0]+u_y*s1/c2+u_z*c1/c2]
    ])
    return x

def h(x):
    y = np.eye(2, 3) @ x
    return y

def predict_x(x, u):
    x = f(x, u)
    return x

def predict_P(P, F, Q):
    P = F @ P @ F.T + Q
    return P

def calc_F(x, u):
    u_x, u_y, u_z = u[0][0], u[1][0], u[2][0]
    c1, s1 = np.cos(x[0][0]), np.sin(x[0][0])
    c2, s2 = np.cos(x[1][0]), np.sin(x[1][0])
    c3, s3 = np.cos(x[2][0]), np.sin(x[2][0])
    F = np.array([
        [1+u_y*c1*s2/c2-u_z*s1*s2/c2, u_y*s1/c2**2+u_z*c1/c2**2, 0],
        [-u_y*s1-u_z*c1, 1, 0],
        [u_y*c1/c2-u_z*s1/c2, u_y*s1*s2/c2**2+u_z*c1*s2/c2**2, 1]
    ])
    return F

def calc_H():
    H = np.eye(2, 3)
    return H

def update_y_res(z, x):
    y_res = z - h(x)
    return y_res

def update_S(P, H, R):
    S = H @ P @ H.T + R
    return S

def update_K(P, H, S):
    K = P @ H.T @ np.linalg.inv(S)
    return K

def update_x(x, y_res, K):
    x = x + K @ y_res
    return x

def update_P(P, H, K):
    I = np.identity(3)
    P = (I - K @ H) @ P
    return P

def ekf(x, u, z, P, R, Q):
    # Predict
    F = calc_F(x, u)
    x = predict_x(x, u)
    H = calc_H()
    P = predict_P(P, F, Q)

    # Update
    y_res = update_y_res(z, x)
    S = update_S(P, H, R)
    K = update_K(P, H, S)
    x = update_x(x, y_res, K)
    P = update_P(P, H, K)
    return x, P

デモ用のコード

スマートフォンで計測した6軸IMUデータをUDPで受け取り、逐次姿勢を更新し、その結果をProcesssingにUDPで送信するコードです。AndroidとiOSで加速度センサデータの正負が逆なので、その点に注意しましょう。

demo.py

import numpy as np
from socket import socket, AF_INET, SOCK_DGRAM
from imu_ekf import *

def calc_u(gyro, dt):
    gyro = np.array([
        [gyro[0]],
        [gyro[1]],
        [gyro[2]]
    ])
    u = gyro * dt
    return u

def calc_z(acc):
    z = np.array([
        [np.arctan(acc[1]/acc[2])], 
        [-np.arctan(acc[0]/np.sqrt(acc[1]**2+acc[2]**2))]
        ])
    return z

def convert_euler_to_Rxyz(x):
    c1 = np.cos(x[0][0])
    s1 = np.sin(x[0][0])
    c2 = np.cos(x[1][0])
    s2 = np.sin(x[1][0])
    c3 = np.cos(x[2][0])
    s3 = np.sin(x[2][0])
    Rx = np.array([
        [1, 0, 0],
        [0, c1, -s1],
        [0, s1, c1],
    ])
    Ry = np.array([
        [c2, 0, s2],
        [0, 1, 0],
        [-s2, 0, c2],
    ])
    Rz = np.array([
        [c3, -s3, 0],
        [s3, c3, 0],
        [0, 0, 1],
    ])
    Rxyz = Rz @ Ry @ Rx
    return Rxyz

def main():
    # UDP settings
    HOST = ''   
    R_PORT = 4001
    S_PORT = 4002
    ADDRESS = "127.0.0.1"
    rs = socket(AF_INET, SOCK_DGRAM)
    ss = socket(AF_INET, SOCK_DGRAM)
    rs.bind((HOST, R_PORT))

    # ekf init
    x = np.array([[0], [0], [0]])
    P = np.diag([1.74E-2*dt**2, 1.74E-2*dt**2, 1.74E-2*dt**2])

    acc = None
    ts_pre = None

    while True:
        r_msg, address = rs.recvfrom(8192)
        sensor_type = str(r_msg.decode('utf-8').split('\t')[1])
        ts = float(r_msg.decode('utf-8').split('\t')[0])
        data_x = float(r_msg.decode('utf-8').split('\t')[-1].split(',')[1])
        data_y = float(r_msg.decode('utf-8').split('\t')[-1].split(',')[2])
        data_z = float(r_msg.decode('utf-8').split('\t')[-1].split(',')[3])

        if sensor_type == 'ACC':
            # acc = np.array([data_x, data_y, data_z]) # Android
            acc = -np.array([data_x, data_y, data_z]) # iOS

        if sensor_type == 'GYRO' and acc is not None:
            gyro = np.array([data_x, data_y, data_z])

            if ts_pre is not None:
                dt = ts - ts_pre
                u = calc_u(gyro, dt)
                z = calc_z(acc)
                R = np.diag([1.0*dt**2, 1.0*dt**2])
                Q = np.diag([1.74E-2*dt**2, 1.74E-2*dt**2, 1.74E-2*dt**2])
                # ekf
                x, P = ekf(x, u, z, P, R, Q)
                # send to viz
                Rxyz = convert_euler_to_Rxyz(x)
                r11, r12, r13 = Rxyz[0][0], Rxyz[0][1], Rxyz[0][2]
                r21, r22, r23 = Rxyz[1][0], Rxyz[1][1], Rxyz[1][2]
                r31, r32, r33 = Rxyz[2][0], Rxyz[2][1], Rxyz[2][2]
                s_msg = str(r11)+','+str(r12)+','+str(r13)+','+str(r21)+','+str(r22)+','+str(r23)+','+str(r31)+','+str(r32)+','+str(r33)
                ss.sendto(s_msg.encode(), (ADDRESS, S_PORT))
            ts_pre = ts

    rs.close()
    ss.close()

if __name__ == '__main__':
    main()

実行

環境は以下の通りです。
Python3.7.0
numpy==1.15.1

今回はスマートフォンの6軸IMUデータをホストへ送信するために、HASC Loggerというアプリを使用します。HASC Loggerをスマートフォンにインストールします。HASC Loggerから[ホストのIP]:4001へ100Hzの加速度と角速度をUDPで送信します。設定の方法は「角速度から回転行列を求める - 実装編」を参考にしてください。また、ProcesssingにUDPのライブラリをインストールする必要があります。こちらも「角速度から回転行列を求める - 実装編」を参考にしてください。

demo.pyを実行します。

python demo.py

Proccesingを実行します。スマートフォンの姿勢がリアルタイムで表示されたら成功です。

拡張カルマンフィルタで6軸IMUの姿勢推定を行ってみました#カルマンフィルタ #姿勢推定 #スマートフォン pic.twitter.com/EEs0T3nr1i
— yakiimo121 (@yakiimo121) June 26, 2021

余談

今回は姿勢の表現にオイラー角を使用しました。そのため2つの軸が一致すると、ちゃんとジンバルロックが起きます。

オイラー角を使ったので、ジンバルロックを体験できました#カルマンフィルタ #姿勢推定 #スマートフォン pic.twitter.com/BU8emx9xbe
— yakiimo121 (@yakiimo121) June 26, 2021

回転行列やクォータニオンなどの姿勢表現を使えばジンバルロックを回避できます。