はじめに

金谷先生の『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に関して詳しくは以下の記事を参照ください。
カルマンフィルタってなに？
パラメータ推定（２）カルマンフィルター
 シンプルなモデルとイラストでカルマンフィルタを直観的に理解してみる

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^2$：分散
$\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^2_{a}  \cdot I_{3 \times 3} & 0 \\
0              & \sigma^2_{\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^2_{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}$

コード

コード置き場
https://github.com/rsasaki0109/quaternion-based_kalman_filter

回転

回転表現にはscipyの以下のメソッドを使います。
https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html

予測更新

 def predict_update(CovTemp, pTemp, vTemp, qTemp, delta_t, imu_a,  imu_w):
    rTemp = R.from_quat([*qTemp])
    Rotation_Mat = r.as_matrix()
    pEst = pTemp + delta_t * vTemp + 0.5 * (delta_t ** 2) * (Rotation_Mat @ imu_a - g)
    vEst = vTemp + delta_t * (Rotation_Mat @ imu_a - g)
    rEst = R.from_euler('xyz',[*w_dt], degrees=True) * rTemp

    F = np.eye(9)
    F[0:3, 3:6] = delta_t * np.eye(3)
    a = imu_a.reshape((3, 1)
    a_skew = np.array([[0, -a[2], a[1]],
                       [a[2], 0, -a[0]],
                       [-a[1], a[0], 0]])
    F[3:6, 6:9] = Rotation_Mat @ -a_skew * delta_t

    Q = np.eye(6)
    Q[0:3, 0:3] = var_a * Q[0:3, 0:3]
    Q[3:6, 3:6] = var_w * Q[3:6, 3:6]
    Q = Q * (delta_t ** 2) 
    Cov = F @ CovTemp @ F.T + L @ Q @ L.T
    return pEst, vEst, rEst.as_quat(), Cov

観測更新

def measurement_update(CovTemp, y_k, pTemp, vTemp, qTemp):
    rTemp = R.from_quat([*qTemp])
    RCov = var_gnss * np.eye(3)
    K = CovTemp @ H.T @ np.linalg.inv(H @ CovTemp @ H.T + RCov)

    delta_x = K @ (y_k - pTemp)

    pTemp = pTemp + delta_x[:3]
    vTemp = vTemp + delta_x[3:6]
    rTemp = R.from_rotvec(delta_x[6:]) * rTemp

    CovTemp = (np.eye(9) - K @ H) @ CovTemp

    return pTemp, vTemp, rTemp.as_quat(), CovTemp

メインループ

import numpy as np
from scipy.spatial.transform import Rotation as R

var_a = (0.1)^2
var_w = (0.1)^2
var_gnss = (0.3)^2
gravity = 9.80665

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

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], Cov[k] = predict_update(Cov[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], Cov[k] = measurement_update(Cov[k],
                                                        gnss[i_gnss], pEst[k], vEst[k], qEst[k])
                i_gnss += 1

参考

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
鈴木,野波,”拡張カルマンフィルタを用いた GPS/非 GPS 空間における自律飛行ドローンのナビゲーション”,2017
Weikun Zhen, Sam Zeng, and Sebastian Scherer. "Robust Localization and Localizability Estimation with a Rotating Laser Scanner" , 2017.