Edited at

クォータニオンを用いたカルマンフィルタによるGPS/IMUを複合したドローンの自己位置推定


はじめに

金谷先生の『3次元回転』を勉強したので、回転表現に親しむためにクォータニオンベースでEKF(Extended Kalman Filter)を用いてGPS(Global Position System)/IMU(Inertial Measurement Unit)センサフュージョンして、ドローンの自己位置推定をしました。


Extended Kalman Filter(EKF)とは

Extended Kalman Filter(EKF)は線形Kalman Filter(KF)を非線形モデルに適用できるよう拡張されたものです。

KFに関して詳しくは以下の記事を参照ください。

カルマンフィルタってなに?

パラメータ推定(2)カルマンフィルター

シンプルなモデルとイラストでカルマンフィルタを直観的に理解してみる

EKFに関して詳しくは以下の記事を参照ください。

拡張カルマンフィルタによる自己位置推定動作の可視化


クォータニオンとは

クォータニオンは3成分の回転軸(ベクトル)と1成分の回転角(スカラー)で3次元空間の回転を表すもので、ゲームや宇宙・防衛分野など幅広い分野でよく使われています。

クォータニオン(やオイラー角)に関して詳しくは以下の記事を参照ください。

クォータニオン (Quaternion) を総整理! ~ 三次元物体の回転と姿勢を鮮やかに扱う ~


問題設定

IMU(ジャイロと加速度センサ)で予測(動作)更新を行い、GPSで観測更新を行います。

以下詳細です。


記号

$k$:離散時間k

$dt$:時間刻み

$\boldsymbol{x}$:状態量

$\boldsymbol{p}$:位置

$\boldsymbol{v}$:速度

$\boldsymbol{a}$:加速度

$\boldsymbol{q}$:クォータニオン

$\omega$:角速度

$u_d$:動作値

$f$:運動モデル

$l$:動作値を状態空間に写す関数

$r_w$:運動ノイズ

$y$:観測量

$h$:観測モデル

$q_w$:観測ノイズ

$\boldsymbol{K}$:カルマンゲイン

$\boldsymbol{I_{n\times n}}$:$n\times n$の単位行列

$\sigma$:分散

$\boldsymbol{\nu}$:歪非対称行列

$\boldsymbol{R}$:回転行列

$\boldsymbol{g}$:重力


モデル


  • 状態量(位置姿勢)

    $ \boldsymbol{x_k} =[p_{xk} \quad p_{yk} \quad p_{zk} \quad v_{xk} \quad v_{yk} \quad v_{zk} \quad q_{xk} \quad q_{yk} \quad q_{zk} \quad q_{wk}]^T$

    $p$:位置、$v$:速度、$q$:クォータニオン

  • 誤差状態量

    $ \boldsymbol{\delta x_k} =[\delta p_{xk} \quad \delta p_{yk} \quad \delta p_{zk} \quad \delta v_{xk} \quad \delta v_{yk} \quad \delta v_{zk} \quad \delta \theta_{xk} \quad \delta \theta_{yk} \quad \delta \theta_{zk} ]^T$

    $\delta \theta $:回転誤差

  • 動作値

    $\boldsymbol{u_{dk}}= [a_{xk} \quad a_{yk} \quad a_{zk} \quad \omega_{xk} \quad \omega_{yk} \quad \omega_{zk}]^T$

    $a$:imuで計測した加速度、$\omega$:imuで計測した角速度

  • 観測値

    $\boldsymbol{y_k}=[p_{xk}^{gps} \quad p_{yk}^{gps} \quad p_{zk}^{gps}]^T$

    $p$:GPSで計測した位置


  • 運動モデル

    $\boldsymbol{x_{k}} = f(x_{k-1}) + l(u_{dk}) + q_w$

    $f$:運動モデル、$l$:動作値を状態空間に写す関数,$q_w$:運動ノイズ

  • 観測モデル

    $\boldsymbol{y_{k}} = h(x_{k}) + r_w$

    $h$:観測モデル、$r_w$:観測ノイズ


回転


  • 歪対称行列$\boldsymbol{\nu}$

\boldsymbol{\nu(\nu)} = 

\begin{pmatrix}
0 & - \nu_z & \nu_y \\
\nu_z & 0 & \nu_x \\
- \nu_y & \nu_x & 0 \\
\end{pmatrix}


  • 回転行列$\boldsymbol{R}$


\boldsymbol{R} = 

\begin{pmatrix}
q_w^2 + q_x^2 - q_y^2 - q_z^2 & 2(q_x q_y - q_w q_z) & 2(q_x q_z + q_w q_y) \\
2(q_x q_y - q_w q_z) & q_w^2 - q_x^2 + q_y^2 - q_z^2 & 2(q_y q_z + q_w q_x) \\
2(q_x q_z + q_w q_y) & 2(q_y q_z - q_w q_x) & q_w^2 - q_x^2 - q_y^2 + q_z^2\\
\end{pmatrix}


  • オイラー角(roll角,pitch角,yaw角)からクォータニオン$\boldsymbol{q}$への変換

$q_w = cos(\frac{roll}{2}) cos(\frac{pitch}{2}) cos(\frac{yaw}{2}) + sin(\frac{roll}{2}) sin(\frac{pitch}{2}) sin(\frac{yaw}{2})$

$q_x = sin(\frac{roll}{2}) cos(\frac{pitch}{2}) cos(\frac{yaw}{2}) - cos(\frac{roll}{2}) sin(\frac{pitch}{2}) sin(\frac{yaw}{2})$

$q_y = cos(\frac{roll}{2}) sin(\frac{pitch}{2}) cos(\frac{yaw}{2}) + sin(\frac{roll}{2}) cos(\frac{pitch}{2}) sin(\frac{yaw}{2})$

$q_z = cos(\frac{roll}{2}) cos(\frac{pitch}{2}) sin(\frac{yaw}{2}) - sin(\frac{roll}{2}) sin(\frac{pitch}{2}) cos(\frac{yaw}{2})$


アルゴリズム


予測更新


  • 状態量

    $ \boldsymbol{p_k} = \boldsymbol{p_{k-1}} + \boldsymbol{v_k} dt +\frac{1}{2} \cdot (\boldsymbol{R(q_{k-1})}\boldsymbol{a_{k-1}^{imu} - g})(dt)^2 $
    $ \boldsymbol{v_k} = \boldsymbol{v_{k-1}} + (\boldsymbol{R(q_{k-1})a_{k-1}^{imu} -g})d t$

    $ \boldsymbol{q_k} = \mathbf{R(\omega_{k-1
    }^{imu}dt )} \cdot \mathbf{q_{k-1}}$

ここで、

$\boldsymbol{g}=[0 \quad 0 \quad -g]^T$


  • 共分散

    $\boldsymbol{P_k = F_k P_{k-1}F_k^T + L Q_k L^T}$

    ここで、

\boldsymbol{F_k} = 

\begin{pmatrix}
I_{3 \times 3} & dt \cdot I_{3 \times 3} & 0 \\
0 & I_{3 \times 3} & - R(q_{k-1}) \cdot \nu(\omega_{k-1}^{imu}) \cdot dt\\
0 & 0 & I_{3 \times 3} \\
\end{pmatrix}

\boldsymbol{Q_k} = 

\begin{pmatrix}
\sigma_{a} \cdot I_{3 \times 3} & 0 \\
0 & \sigma_{\omega} \cdot I_{3 \times 3} \\
\end{pmatrix}
(dt)^2

\boldsymbol{L} = 

\begin{pmatrix}
0 & 0\\
I_{3 \times 3} & 0 \\
0 & I_{3 \times 3}

\end{pmatrix}


観測更新


  • カルマンゲイン

    $\boldsymbol{K} = \boldsymbol{P_k H^T (H P_k H^T + R)}^{-1}$


ここで、

\boldsymbol{R} = \sigma_{gps} \cdot \boldsymbol{I_{3 \times 3}}  

\boldsymbol{H} = 

\begin{pmatrix}
\boldsymbol{I_{3 \times 3}} & \boldsymbol{0} & \boldsymbol{0}
\end{pmatrix}


  • 誤差状態量


    $\boldsymbol{\delta x} = \boldsymbol{K} \cdot (\boldsymbol{y_k}-\boldsymbol{p_{k}})$


  • 状態量


    $ \boldsymbol{p_k} = \boldsymbol{p_{k-1}} + \boldsymbol{\delta p_{k}} $


    $ \boldsymbol{v_k} = \boldsymbol{v_{k-1}} + \boldsymbol{\delta v_{k}}$


    $ \boldsymbol{q_k} = \boldsymbol{R}(\boldsymbol{\delta \theta_{k}}) \cdot \boldsymbol{q_{k-1}}$


  • 共分散


    $\boldsymbol{P_k = (I_{9 \times 9} - K H) \cdot P_k}$



コード


  • 回転

def skew_symmetric(v):

return np.array(
[[0, -v[2], v[1]],
[v[2], 0, -v[0]],
[-v[1], v[0], 0]]
)

class Quaternion():
def __init__(self, w=1., x=0., y=0., z=0., axis_angle=None, euler=None):
if axis_angle is None and euler is None:
self.w = w; self.x = x; self.y = y; self.z = z

elif axis_angle is not None:
axis_angle = np.array(axis_angle)
norm = np.linalg.norm(axis_angle)
self.w = np.cos(norm / 2)
axis_q = axis_angle / norm * np.sin(norm / 2)
self.x = axis_q[0].item(); self.y = axis_q[1].item(); self.z = axis_q[2].item()

else:
roll = euler[0]; pitch = euler[1]; yaw = euler[2]

cy = np.cos(yaw * 0.5); sy = np.sin(yaw * 0.5)
cr = np.cos(roll * 0.5); sr = np.sin(roll * 0.5)
cp = np.cos(pitch * 0.5); sp = np.sin(pitch * 0.5)

self.w = cr * cp * cy + sr * sp * sy
self.x = sr * cp * cy - cr * sp * sy
self.y = cr * sp * cy + sr * cp * sy
self.z = cr * cp * sy - sr * sp * cy

def to_mat(self):
v = np.array([self.x, self.y, self.z]).reshape(3,1)
return (self.w ** 2 - v.T @ v) * np.eye(3) + \
2 * v @ v.T + 2 * self.w * skew_symmetric(v)


  • 予測更新

 def predict_update(pCovTemp, pTemp, vTemp, qTemp, delta_t, imu_a,  imu_w):

Rotation_Mat = Quaternion(*qTemp).to_mat()
pEst = pTemp + delta_t * vTemp + 0.5 * (delta_t ** 2) * (Rotation_Mat @ imu_a - g)
vEst = vTemp + delta_t * (Rotation_Mat @ imu_a - g)
qEst = Quaternion(euler = delta_t * imu_w).quat_mult(qTemp)

F = np.eye(9)
imu = imu_a.reshape((3, 1))
F[0:3, 3:6] = delta_t * np.eye(3)
F[3:6, 6:9] = Rotation_Mat @ (-skew_symmetric(imu)) * delta_t

Q = np.eye(6)
Q[0:3, 0:3] = var_imu_a * Q[0:3, 0:3]
Q[3:6, 3:6] = var_imu_w * Q[3:6, 3:6]
Q = Q * (delta_t ** 2)
pCov = F @ pCovTemp @ F.T + lJac @ Q @ lJac.T
return pEst, vEst, qEst, pCov


  • 観測更新

def measurement_update(pCovTemp, y_k, pTemp, vTemp, qTemp):

RCov = var_gnss * np.eye(3)
K = pCovTemp @ hJac.T @ np.linalg.inv(hJac @ pCovTemp @ hJac.T + RCov)

delta_x = K @ (y_k - pTemp)

pTemp = pTemp + delta_x[:3]
vTemp = vTemp + delta_x[3:6]
qTemp = Quaternion(axis_angle = delta_x[6:]).quat_mult(qTemp)

pCovTemp = (np.eye(9) - K @ hJac) @ pCovTemp

return pTemp, vTemp, qTemp, pCovTemp


  • メインループ


g = np.array([0, 0, -gravity])
lJac = np.zeros([9, 6])
lJac[3:, :] = np.eye(6)
hJac = np.zeros([3, 9])

# Initial values
pEst[0] = gt_p[0]
vEst[0] = gt_v_ini
qEst[0] = gt_q_ini
pCov[0] = np.eye(9)

i_gnss = 0

for k in range(1, imu_a.shape[0]):

delta_t = imu_a_t[k] - imu_a_t[k - 1]
pEst[k], vEst[k], qEst[k], pCov[k] = predict_update(pCov[k-1], pEst[k-1], vEst[k-1], qEst[k-1],
delta_t,imu_a[k - 1],imu_w[k - 1])

if i_gnss < gnss_t.shape[0] and abs(gnss_t[i_gnss] - imu_a_t[k]) < 0.01:
pEst[k], vEst[k], qEst[k], pCov[k] = measurement_update(pCov[k],
gnss[i_gnss], pEst[k], vEst[k], qEst[k])
i_gnss += 1


結果

自己位置推定結果が以下です。

imu_gnss.gif

良さそうですね!

比較用に、IMUのみでの位置姿勢推定の結果が以下になります。

imu.gif

全然ダメですね!IMUだけでは位置推定は厳しいことがわかります。


参考

K Feng,"A New Quaternion-Based Kalman Filter",2017

Joan Solà,"Quaternion kinematics for the error-state Kalman filter",2017

Daniel Choukroun et al,"A Novel Quaternion Kalman Filter",2006

An Improved EKF - The Error State Extended Kalman Filter

目黒淳一,"都市部環境下に適用するGPSを用いた移動システムに関する研究",2008

金谷健一,"3次元回転: パラメータ計算とリー代数による最適化",2019

Qiita クォータニオン (Quaternion) を総整理! ~ 三次元物体の回転と姿勢を鮮やかに扱う ~

クォータニオン計算便利ノート

Diebel J,"Representing attitude",2006