[2021.6.17追記]
下記記事では、回転行列 $R$ で直接微分して勾配法を適用していますが、 典型的な多様体上での最適化問題であるのでLie代数を使用するなどより良い方法があると思われます。(そういう工夫をしていないので以下の方法ではシュミットの直交化法を使うということになっています)
Madgwick Filterという姿勢推定に使用されるアルゴリズムがあるんですが、論文An efficient orientation filter for inertial and inertial/magnetic sensor arrays によるとQuaternion (四元数) を用いて書かれています。Madgwick Filterは演算量が少ないというのが特徴で、論文では
The benefits of the filter include: (1) computationally inexpensive; requiring 109 (IMU) or 277 (MARG) scalar arithmetic operations each filter update,
と書かれています。ただ、演算量という観点ですとQuaternionを使わずに回転行列で表現して、回転行列表現のまま更新していった方が速い気がしました。というのはquaternionでの座標変換(論文の(5)式)
$$ {}^Bv={}^A_B\hat{q}\otimes{}^Av\otimes{}^A_B\hat{q}^{\ast}$$
の計算は64回の演算が必要ですが、回転行列の場合には同値な計算が18演算で済みます。
但し、Quaternionは4次元ベクトルであるのに対して、回転行列は9次元ですから、差分近似を行った際の近似誤差の乗り方はQuaternionよりも大きくなるのではないかと思います。しかし、実際に速くなるならば $\Delta t$を小さく取れるので総合的に見たらもしかすると精度が上がるかもしれません。結局はトレードオフの問題です。また、制御に使用する場合は、毎フレームフィードバックが掛かるので多少の誤差は問題ない可能性もあります。制御理論良く分からないのですごく適当な事を言っているかもしれません。
という事でMadgwick FilterをQuaternionの代わりに回転行列で記述したらどうなるかに興味が湧いたので調べてみました。(既に誰か調べているかもしれませんが)
話がそれますが、論文の(6)は間違いだと思います。正しくは右辺を転置する必要があると思います。
Madgwick Filterとは
ドローンなどの制御では、機体の現在の姿勢を知る事が必要です。そこで、通常は慣性計測装置と磁気センサーが用いられます。センサーからは
- 加速度
- 角速度
- 磁気
それぞれx,y,z方向で計9つの計測値が各時刻で得られます。これらの値から現在の姿勢を知ることが目標です。
odmetryによる更新式
センサーに固定された座標系を $S$、静止座標系を $E$ とします。$t$を時刻とします。$S$ から $E$への回転行列を $R_t$ とします。$R_t$ が分かれば機体の姿勢が分かります。また、センサーで得られる角速度を $\omega_t$ とします。$\omega_t$はS上のベクトルである事に注意。
センサーの回転によって $E$ 上の固定点が $S$から見て $x_0\rightarrow x_t$ と動いたとすると、$E$では動いていないので
$$ R_tx_t = R_0x_0 $$
となります。両辺を微分すると
$$\frac{{\rm d} R_t}{{\rm d} t}x_t + R_t \frac{{\rm d}x_t}{{\rm d} t} = 0$$
となります。ここでセンサーが $\omega_t$ で回転しているという事は、$S$からみると$E$は逆方向に回転しているように見えるので
$$ \frac{{\rm d}x_t}{{\rm d}t} = -\omega_t\times x_t$$
が成り立ちます。したがって
$$ \frac{{\rm d} R_t}{{\rm d} t}x_t = R_t(\omega_t\times x_t)$$
が任意の$x_t$について成立するので
$$ \frac{{\rm d} R_t}{{\rm d} t} = R_t[\omega_t\times] $$
ただし、
$$[\omega_t\times] = \begin{pmatrix}
0 & -\omega_{z,t} & \omega_{y,t} \\
\omega_{z,t} & 0 & -\omega_{x,t} \\
-\omega_{y,t} & \omega_{x,t} & 0
\end{pmatrix}$$
です。この式が論文中の式(11)と等価なはずです。続いてこれを差分近似して
$$ R_{\omega,t} = R_{t-1} + R_{t-1}[\omega_t\times]\Delta t\cdots (A)$$
を得ます。これが論文中の式(13)に相当しますが、表現が違う為近似誤差の乗り方が変わり厳密には一致しません。$\omega_{t-1}$でなく$\omega_t$にしているのは論文に合わせてです。角速度を積分して得られる値という意味で添え字に$\omega$を付けています。
observationによる更新式
話はそれますが、論文中の(15)式は間違っていて
$$ f({}^S_E\hat{q},{}^E\hat{d},{}^S\hat{s}) = \frac{1}{2}||{}^S_E\hat{q}^{\ast}\otimes{}^E\hat{d}\otimes{}^S_E\hat{q}-{}^S\hat{s}||^2 \cdots (15)$$
のはずです
$a_t=(a_{x,t},a_{y,t},a_{z,t})^T$ をセンサーから得られる加速度ベクトルを正規化したものとします。また $g=(0,0,1)^T$ を正規化された重力加速度とします。$a_t$ は $S$のベクトル、$g$は$E$のベクトルです。$a_t$ には機体の運動で生じる加速度と重力加速度が加算されていますが、$a_t$ でえられるのは「重力加速度のみである」という仮定を置きます。以下は論文からの引用。
In the context of an orientation filter, it will initially be assumed that an accelerometer will measure only gravity, a magnetometer will measure only the earth’s magnetic field.
すると $R_t^Tg = a_t$ が成立します。同じように磁気センサー $m_t=(m_{x,t},m_{y,t},m_{z,t})^T$ も地磁気 $b=(b_x,0,b_z)^T$ のみを測定すると仮定すれば $R_t^Tb=m_t$ が成立します。そこで
$$ f(R_t,a_t,m_t) = \frac{1}{2}||R_t^Tg - a_t||^2 + \frac{1}{2}||R_t^Tb-m_t||^2 $$
と置いてこれを最小化する $R_t$ を探します。以上が論文中の(14)～(18)に相当します。
これを $R_t$ で微分すると
$$ \begin{align}
\nabla f &= (R_t^Tg-a_t)g^T + (R_t^Tb-m_t)b^T \\
&= R_t^T(gg^T+bb^T)-a_tg^T-m_tb^T
\end{align} $$
になります。ここで$gg^T+bb^T$は特異行列になり逆行列を持たないので $\nabla f=0$ により解を求めることは出来ません。論文では勾配法で更新を行っていますが、
It is acceptable to compute one iteration per time sample provided that the convergence rate governed by µt is equal or greater than the physical rate of change of orientation.
との事ですので、同様に各時刻に1 stepだけ更新を行う事にして、
$$ R_{\nabla,t} = R_{t-1} - \mu_t\frac{\nabla f}{||\nabla f||} $$
とします。勾配法で求めたという意味で添え字に $\nabla$ を付けています。
$\mu_t$ は論文に従って
$$ \mu_t = \alpha||\omega||\Delta t,\quad (\alpha > 1) $$
と置きます。以上よりobservationによる $R_t$ の更新式が得られました。
$$ R_{\nabla,t} = R_{t-1} - \mu_t\frac{\nabla f}{||\nabla f||}\cdots (B)$$
filter fusion
odometryに基づく推定値と、observationに基づく推定値が得られました。
$$ \begin{align}
R_{\omega,t} &= R_{t-1}+R_{t-1}[\omega_t\times]\Delta t\cdots (A) \\
R_{\nabla,t} &= R_{t-1} - \mu_t\frac{\nabla f}{||\nabla f||}\cdots (B)
\end{align}$$
これらを重み $\gamma_t$ で加算したものを実際の推定値とします。
$$ R_{t} = \gamma_t R_{\nabla,t} + (1-\gamma_t)R_{\omega,t} $$
論文では式(37)～(40)までいろいろな近似が行われていますが、同じ近似を用いて
$$\begin{align}
R_t&=\frac{\beta\Delta t}{\mu_t}\left(-\mu_t\frac{\nabla f}{||\nabla f||}\right)+(1-0)\left(R_{t-1}+R_{t-1}[\omega_t\times]\Delta t\right)\\
&=R_{t-1}+\left(R_{t-1}[\omega_t\times]-\beta\frac{\nabla f}{||\nabla f||}\right)\Delta t
\end{align}$$
となります。これが最終的な更新式となります。
ただし、 $R_t$ は回転行列で無ければならないですから、更新後にシュミットの直交化法を使って回転行列にします。
Magnetic distortion compensation
論文の(45)(46)式に相当するものは
$$\begin{align}
h_t &= R_tm_t \\
b_t &= \left(\sqrt{h_{x,t}^2+h_{y,t}^2}, 0, h_{z,t}\right)^T \\
\end{align}$$
となります。
Gyroscope bias drift compensation
論文中の(47)式は
$$\omega_{\epsilon,t}\times = R_{t-1}^T\nabla f/||\nabla f||$$
になると思います。
演算量
論文によるとMARG(磁気を含む)場合は277回の加算・乗算が必要のようです。
The benefits of the filter include: (1) computationally inexpensive; requiring 109 (IMU) or 277 (MARG) scalar arithmetic operations each filter update,
今回導出した更新式がどうか見積もってみると
- $a_t$, $m_t$ の正規化: mul: 6, add: 6, sqrt:2, inv: 2
- $R_{t-1}[\omega_t\times]$: mul: 18, add: 9
- $\nabla f = R_t^T(gg^T+bb^T)-a_tg^T-m_tb^T$: mul: 21, add: 16
- $\beta\nabla f/||\nabla f||$: mul: 13, add: 6, sqrt: 1, inv: 1
- $R_{t-1}+\left(R_{t-1}[\omega_t\times]-\beta\frac{\nabla f}{||\nabla f||}\right)\Delta t$: mul: 9, add: 15
- $R_t$ の正規化: mul: 18, add: 10, sqrt: 2, inv: 2
- Magnetic distortion compensation: mul: 11, add: 11, sqrt: 1
- Gyroscope bias drift compensation: mul:12, add: 12
なので併せて
- mul: 108
- add: 85
- sqrt: 6
- inv: 5
回です。加算・乗算は277->193とまあまあ減りました。sqrtが1回増えている事を考慮しても、多分速くなるだろうと思います。
それから機体そのものの運動は多分剛体の運動方程式で記述すると思うので、論文の更新式だとquaternionを得た後に回転行列に変換する必要があるんじゃないかと思います。そこの変換もなくなるので多分さらに得をします。間違った事を言ってたら教えてください。
数値実験
以上で得られた更新式でどのくらい結果が一致するのか数値実験をしてみました。
$\omega$の各要素は一様分布$[-10,10]$から取り、それ以外のパラメータは論文と同一にしました。
Quaternionによる更新式で計算した差分 $\Delta q$ と、回転行列による更新式で計算した結果を、Quaternion表現に戻して初期状態との差分を取った $\Delta R$についての相対誤差
$$ \frac{\left|(||\Delta R|| - ||\Delta q||)\right|}{||\Delta q||} $$
を計算してみると、大体 $10^{-2}\sim 10^{-3}$ くらいのオーダーになりましたので、思ったよりは一致しているなという感じです。
[追記] quaternionの方向を考慮しない、おかしな比較をしていました。
Quaternionによる更新式で得られた $q'$ と、回転行列により更新式で得られた $q'_R$ の差を、$q$ の変化量で割った
$$ \frac{||q'_R-q'||}{||q'-q||} $$
を見るのが正しいですね。大体 $10^{-2}$ くらいのオーダーになりました。まあこんなもんかという感じですね。
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <string.h>
#define TESTCASES 10000
#define dt 0.001f
#define gyro_error (M_PI * 5.0f / 180.0f)
#define beta (0.8660254037844386f * gyro_error) // sqrt(3/4) * gyro_error
static void
normalize_v(float *v, size_t n)
{
float norm = 0;
for (int i = 0; i < n; i++)
norm += v[i]*v[i];
norm = sqrtf(norm);
for (int i = 0; i < n; i++)
v[i] /= norm;
}
static void
normalize_q(float q[4])
{
float coef = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
if (q[0] < 0)
coef *= -1; /* keep w positive */
q[0] /= coef;
q[1] /= coef;
q[2] /= coef;
q[3] /= coef;
}
static void
q_update(float w[3], float a[3], float m[3], float b[2], float q[4])
{
/* odmetry
* dq_w = q * w / 2
*/
float dq_w[4];
dq_w[0] = (- q[1]*w[0] - q[2]*w[1] - q[3]*w[2]) * 0.5f;
dq_w[1] = ( q[0]*w[0] + q[2]*w[2] - q[3]*w[1]) * 0.5f;
dq_w[2] = ( q[0]*w[1] - q[1]*w[2] + q[3]*w[0]) * 0.5f;
dq_w[3] = ( q[0]*w[2] + q[1]*w[1] - q[2]*w[0]) * 0.5f;
/* observation of acceleration
* dq_a = Ja^T * fa
*/
float fa[3];
float dq_a[4];
fa[0] = 2*(q[1]*q[3] - q[0]*q[2]) - a[0];
fa[1] = 2*(q[0]*q[1] + q[2]*q[3]) - a[1];
fa[2] = 1 - 2*(q[1]*q[1] + q[2]*q[2]) - a[2];
dq_a[0] = -2*q[2]*fa[0] + 2*q[1]*fa[1];
dq_a[1] = 2*q[3]*fa[0] + 2*q[0]*fa[1] - 4*q[1]*fa[2];
dq_a[2] = -2*q[0]*fa[0] + 2*q[3]*fa[1] - 4*q[2]*fa[2];
dq_a[3] = 2*q[1]*fa[0] + 2*q[2]*fa[1];
/* observation of magnetic flux
* dq_m = Jm^T * fm
*/
float fm[3];
float dq_m[4];
fm[0] = 2*b[0]*(0.5 - q[2]*q[2] - q[3]*q[3]) + 2*b[1]*(q[1]*q[3] - q[0]*q[2]) - m[0];
fm[1] = 2*b[0]*(q[1]*q[2] - q[0]*q[3]) + 2*b[1]*(q[0]*q[1] + q[2]*q[3]) - m[1];
fm[2] = 2*b[0]*(q[0]*q[2] + q[1]*q[3]) + 2*b[1]*(0.5 - q[1]*q[1] - q[2]*q[2]) - m[2];
dq_m[0] = -2*b[1]*q[2]*fm[0] + (-2*b[0]*q[3] + 2*b[1]*q[1])*fm[1] + 2*b[0]*q[2]*fm[2];
dq_m[1] = 2*b[1]*q[3]*fm[0] + (2*b[0]*q[2] + 2*b[1]*q[0])*fm[1] + (2*b[0]*q[3] - 4*b[1]*q[0])*fm[2];
dq_m[2] = (-4*b[0]*q[2] - 2*b[1]*q[0])*fm[0] + (2*b[0]*q[1] + 2*b[1]*q[3])*fm[1] + (2*b[0]*q[0] - 4*b[1]*q[2])*fm[2];
dq_m[3] = (-4*b[0]*q[3] + 2*b[1]*q[1])*fm[0] + (-2*b[0]*q[0] + 2*b[1]*q[2])*fm[1] + 2*b[0]*q[1]*fm[2];
/* dq_o = (dq_a + dq_m)/||dq_a + dq_m|| */
float dq_o[4];
for (int i = 0; i < 4; i++)
dq_o[i] = dq_a[i] + dq_m[i];
normalize_q(dq_o);
for (int i = 0; i < 4; i++)
q[i] += (dq_w[i] - beta*dq_o[i])*dt;
normalize_q(q);
}
static void
normalize_R(float R[3][3])
{
float n = sqrtf(R[0][2]*R[0][2] + R[1][2]*R[1][2] + R[2][2]*R[2][2]);
R[0][2] /= n;
R[1][2] /= n;
R[2][2] /= n;
R[0][0] = R[1][1]*R[2][2] - R[1][2]*R[2][1];
R[1][0] = R[2][1]*R[0][2] - R[2][2]*R[0][1];
R[2][0] = R[0][1]*R[1][2] - R[0][2]*R[1][1];
n = sqrtf(R[0][0]*R[0][0] + R[1][0]*R[1][0] + R[2][0]*R[2][0]);
R[0][0] /= n;
R[1][0] /= n;
R[2][0] /= n;
R[0][1] = R[1][2]*R[2][0] - R[1][0]*R[2][2];
R[1][1] = R[2][2]*R[0][0] - R[2][0]*R[0][2];
R[2][1] = R[0][2]*R[1][0] - R[0][0]*R[1][2];
}
static void
R_update(float w[3], float a[3], float m[3], float b[2], float R[3][3])
{
/* odmetry
* dR_w = R[w x]
*/
float dR_w[3][3];
dR_w[0][0] = R[0][1]*w[2] - R[0][2]*w[1];
dR_w[0][1] = R[0][2]*w[0] - R[0][0]*w[2];
dR_w[0][2] = R[0][0]*w[1] - R[0][1]*w[0];
dR_w[1][0] = R[1][1]*w[2] - R[1][2]*w[1];
dR_w[1][1] = R[1][2]*w[0] - R[1][0]*w[2];
dR_w[1][2] = R[1][0]*w[1] - R[1][1]*w[0];
dR_w[2][0] = R[2][1]*w[2] - R[2][2]*w[1];
dR_w[2][1] = R[2][2]*w[0] - R[2][0]*w[2];
dR_w[2][2] = R[2][0]*w[1] - R[2][1]*w[0];
/* observation
* dR_o = R^T(gg^T + bb^T) - ag^T - mb^T
*/
float ggbb[2][2];
float dR_o[3][3];
ggbb[0][0] = b[0]*b[0];
ggbb[0][1] = ggbb[1][0] = b[0]*b[1];
ggbb[1][1] = b[1]*b[1] + 1;
dR_o[0][0] = R[0][0]*ggbb[0][0] + R[2][0]*ggbb[0][1] - m[0]*b[0];
dR_o[0][1] = 0;
dR_o[0][2] = R[0][0]*ggbb[0][1] + R[2][0]*ggbb[1][1] - a[0] - m[0]*b[1];
dR_o[1][0] = R[0][1]*ggbb[0][0] + R[2][1]*ggbb[0][1] - m[1]*b[0];
dR_o[1][1] = 0;
dR_o[1][2] = R[0][1]*ggbb[0][1] + R[2][1]*ggbb[1][1] - a[1] - m[1]*b[1];
dR_o[2][0] = R[0][2]*ggbb[0][0] + R[2][2]*ggbb[0][1] - m[2]*b[0];
dR_o[2][1] = 0;
dR_o[2][2] = R[0][2]*ggbb[0][1] + R[2][2]*ggbb[1][1] - a[2] - m[2]*b[1];
normalize_v((float*)dR_o, 9);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
R[i][j] += (dR_w[i][j] - beta*dR_o[i][j])*dt;
}
}
normalize_R(R);
}
static void
R_to_q(float R[3][3], float quat[4])
{
float tr = R[0][0] + R[1][1] + R[2][2];
if (1 + tr > 0) {
quat[0] = sqrtf(1 + tr)/2;
quat[1] = (R[2][1] - R[1][2])/(4 * quat[0]);
quat[2] = (R[0][2] - R[2][0])/(4 * quat[0]);
quat[3] = (R[1][0] - R[0][1])/(4 * quat[0]);
} else if ((R[0][0] > R[1][1]) && (R[0][0] > R[2][2])) {
quat[1] = sqrtf(1 + R[0][0] - R[1][1] - R[2][2])/2;
quat[0] = (R[2][1] - R[1][2])/(4 * quat[1]);
quat[2] = (R[0][1] + R[1][0])/(4 * quat[1]);
quat[3] = (R[0][2] + R[2][0])/(4 * quat[1]);
} else if (R[1][1] > R[2][2]) {
quat[2] = sqrtf(1 + R[1][1] - R[0][0] - R[2][2])/2;
quat[0] = (R[0][2] - R[2][0])/(4 * quat[2]);
quat[1] = (R[0][1] + R[1][0])/(4 * quat[2]);
quat[3] = (R[1][2] + R[2][1])/(4 * quat[2]);
} else {
quat[3] = sqrtf(1 + R[2][2] - R[0][0] - R[1][1])/2;
quat[0] = (R[1][0] - R[0][1])/(4 * quat[3]);
quat[1] = (R[0][2] + R[2][0])/(4 * quat[3]);
quat[2] = (R[1][2] + R[2][1])/(4 * quat[3]);
}
}
static float
unif(void)
{
return 2*(float)rand()/(float)RAND_MAX-1;
}
int
main(void)
{
float sum_relative_error = 0.0;
srand(time(0));
for (int i = 0; i < TESTCASES; i++) {
float q[4];
float R[3][3];
/* initial state */
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
R[i][j] = unif();
}
}
normalize_R(R);
R_to_q(R, q);
/* save q for error calculation */
float q_old[4];
memcpy(q_old, q, sizeof(q));
/* angular velocity */
float w[3];
for (int i = 0; i < 3; i++) w[i] = unif()*10;
/* acceleration */
float a[3];
for (int i = 0; i < 3; i++) a[i] = unif();
normalize_v(a, 3);
/* magnetic flux */
float m[3];
for (int i = 0; i < 3; i++) m[i] = unif();
normalize_v(m, 3);
/* reference magnetic flux (bx, bz)*/
float b[2];
for (int i = 0; i < 2; i++) b[i] = unif();
normalize_v(b, 2);
/* state update */
q_update(w, a, m, b, q);
R_update(w, a, m, b, R);
/* compute error */
float q2[4];
R_to_q(R, q2);
float delta_q = sqrtf(
(q[0] - q_old[0])*(q[0] - q_old[0]) +
(q[1] - q_old[1])*(q[1] - q_old[1]) +
(q[2] - q_old[2])*(q[2] - q_old[2]) +
(q[3] - q_old[3])*(q[3] - q_old[3])
);
float delta_R = sqrtf(
(q2[0] - q_old[0])*(q2[0] - q_old[0]) +
(q2[1] - q_old[1])*(q2[1] - q_old[1]) +
(q2[2] - q_old[2])*(q2[2] - q_old[2]) +
(q2[3] - q_old[3])*(q2[3] - q_old[3])
);
sum_relative_error += fabsf(delta_R - delta_q)/delta_q;
}
printf("mean squared error = %e\n", sum_relative_error/TESTCASES);
}
次のステップ
趣味でRaspberry Pi ZeroのHAT型のドローンを作っているので、実際にこの方法で姿勢推定が実行できるのか実装して試してみようと思います。いつになるか分かりませんが、やったら報告します。
