こんにちは。たけぴよです。今回は、クォータニオンとジャイロセンサのバイアスを拡張カルマンフィルタを用いて推定します。
数式自体はいろいろなところに載っていますが、成分まで書かれているものは多くなかったので、なるべく成分表示を用いて記録していきます。後半はC++での実装を行います。
実装した際に、状態料にジャイロセンサのバイアスを、観測量に地磁気の値をそれぞれ加えた実際に動かしたら上手いこと動きませんでした。なのでそれらを使っていない上手くいった実装を後半に示しました。
クォータニオンの数学
クォータニオンを$\bf{q}$、共役クォータニオンを$\bf{q}^\star$と置くと、$\bf{q}$による座標系の回転に伴うベクトル$\bf{v}$の変化は、変化後のベクトルを$\bf{w}$と置くと以下のようになります。
$$\bf{w} = \bf{q}^\star\bf{v}\bf{q} = \bf{Q}\bf{v} $$
$${\bf q} = \begin{bmatrix}q_0&q_1&q_2&q_3\end{bmatrix}^{\bf T}, {\bf q^\star} = \begin{bmatrix}q_0&-q_1&-q_2&-q_3\end{bmatrix}^{\bf T}$$
{\bf Q} = \begin{bmatrix}
q_0^2+q_1^2-q_2^2-q_3^2 & 2q_1q_2 + 2q_0q_3 & 2q_1q_3 - 2q_0q_2 \\
2q_1q_2 - 2q_0q_3 & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2q_2q_3 + 2q_0q_1 \\
2q_1q_3 + 2q_0q_2 & 2q_2q_3 - 2q_0q_1 & q_0^2-q_1^2-q_2^2+q_3^2
\end{bmatrix}
クォータニオンの積は以下のようになります。
{\bf p} = \begin{bmatrix}p_0&p_1&p_2&p_3\end{bmatrix}^{\bf T} , {\bf q} = \begin{bmatrix}q_0&q_1&q_2&q_3\end{bmatrix}^{\bf T}\\
{\bf pq} = \begin{bmatrix} p_0&-p_1&-p_2&-p_3 \\
p_1&p_0&-p_3&p_2 \\
p_2&p_3&p_0&-p_1 \\
p_3&-p_2&p_1&p_0
\end{bmatrix}
\begin{bmatrix}
q_0\\q_1\\q_2\\q_3
\end{bmatrix}
= \begin{bmatrix} q_0&-q_1&-q_2&-q_3 \\
q_1&q_0&q_3&-q_2 \\
q_2&-q_3&q_0&q_1 \\
q_3&q_2&-q_1&q_0
\end{bmatrix}
\begin{bmatrix}
p_0\\p_1\\p_2\\p_3
\end{bmatrix}
クォータニオンの時間微分は以下のようになります。三次元ベクトルのクォータニオン表記を添え字qで表します。
({\boldsymbol \omega(t)})_q = \begin{bmatrix} 0 & \omega_x & \omega_y & \omega_z \end{bmatrix}^{\bf T} \\
\frac{d{\bf q}}{dt} = \frac{1}{2} {\bf q}(t) ({\boldsymbol \omega}(t))_q
状態方程式
地面に固定された座標系の軸を、磁北をXr軸,重力方向をZr軸,XrZr平面の垂直方向をYr軸にとった座標系rとします。
機体の重心を原点として、機体前方をXb軸、機体右方向をYb軸、機体下方をZb軸として取った座標系をbとします。
rから見たbのクォータニオンを${\bf q} = \begin{bmatrix}q_0&q_1&q_2&q_3\end{bmatrix}^{\bf T}$とします。
またrからみた重力ベクトルと地磁気ベクトルをそれぞれ ${\bf g_r} = \begin{bmatrix}0&0&g\end{bmatrix}^{\bf T},;{\bf m_r} = \begin{bmatrix}m_n&0&m_d\end{bmatrix}^{\bf T}$とします。これらは後で直接測定する必要があります。
ジャイロセンサによって得られた角速度を${\boldsymbol \omega_{measure}} = \begin{bmatrix}\omega_x&\omega_y&\omega_z\end{bmatrix}^{\bf T}$,バイアス誤差を${\bf b} = \begin{bmatrix}b_x&b_y&b_z\end{bmatrix}^{\bf T}$とすると、角速度は以下のような関係を満たします。
$${\boldsymbol \omega_{measure}} = {\boldsymbol \omega}(t) + {\bf b} $$
したがってクォータニオンの時間微分は以下のように表されます。
$$\dot{{\bf q}} = \frac{1}{2} {\bf q} (({\boldsymbol \omega_{measure}})_q - ({\bf b}) _q )$$
また、ジャイロセンサのバイアスは以下のようなダイナミクスを持つらしいです。
\dot{{\bf b}} = {\boldsymbol \beta}{\bf b} + {\bf w} \\
\boldsymbol \beta = \begin{bmatrix}-\beta_x & 0 & 0 \\
0 & -\beta_y & 0 \\
0 & 0 & -\beta_z\end{bmatrix}
$\boldsymbol \beta$と${\bf w}$の値は分散などを調整して後で決定する必要があります。
以上より状態量を${\bf x} = \begin{bmatrix} {\bf q^T} & {\bf b^T} \end{bmatrix}^{\bf T}$としたときの状態方程式は
{\bf \dot{x}} = \bf f(x) + Gw \\
{\bf f(x)} = \begin{bmatrix} \frac{1}{2} {\bf q} (({\boldsymbol \omega_{measure}})_q - ({\bf b}) _q )
\\
\boldsymbol \beta \bf b
\end{bmatrix} \\
\bf G = \begin{bmatrix} 0_{4×3} \\
I_{3×3}
\end{bmatrix} \\
この状態方程式を時間に関して離散化します。
\bf x_{t+1} = \bf x_t + f(x_t)\Delta t + G \Delta t w \\
\bf x_{t+1} = f_t(x_t) + G_tW
fを成分表示すると以下のようになります。
{\bf f_t(x_t)} = \begin{bmatrix} q_0-\frac{1}{2}(\omega_x-b_x)q_1\Delta t -\frac{1}{2}(\omega_y-b_y)q_2\Delta t -\frac{1}{2}(\omega_z-b_z)q_3\Delta t \\
\frac{1}{2}(\omega_x-b_x)q_0\Delta t+ q_1 + \frac{1}{2}(\omega_z-b_z)q_2\Delta t - \frac{1}{2}(\omega_y-b_y)q_3\Delta t \\
\frac{1}{2}(\omega_y-b_y)q_0\Delta t - \frac{1}{2}(\omega_z-b_z)q_1\Delta t + q_2 + \frac{1}{2}(\omega_x-b_x)q_3\Delta t \\
\frac{1}{2}(\omega_z-b_z)q_0\Delta t + \frac{1}{2}(\omega_y-b_y)q_1\Delta t - \frac{1}{2}(\omega_x-b_x)q_2\Delta t + q_3 \\
b_x -\beta_x \Delta t b_x \\
b_y -\beta_y \Delta t b_y \\
b_z -\beta_z \Delta t b_z
\end{bmatrix}
観測方程式
加速度の誤差と地磁気誤差をそれぞれ$ \Delta {\bf a}, ; \Delta {\bf m}$と置くと、加速度センサと地磁気センサの出力は次のようになります。
\begin{bmatrix}{\bf a_{measure}} \\ {\bf m_{measure}} \end{bmatrix} = \begin{bmatrix} {\bf Qg_r} \\
{\bf Qm_r} \end{bmatrix} + \begin{bmatrix} \Delta {\bf a} \\
\Delta \bf m \end{bmatrix}
これを以下のように定義します。
\bf y_t = h_t(x_t) + v_t
hを成分表示すると以下のようになります。
\begin{bmatrix} 2(q_1q_3 - q_0q_2)g \\
2(q_2q_3 + q_0q_1)g \\
(q_0^2-q_1^2-q_2^2+q_3^2)g \\
(q_o^2+q_1^2-q_2^2-q_3^2)m_n + 2(q_1q_3 - q_0q_2)m_d \\
2(q_1q_2 - q_0q_3)m_n + 2(q_2q_3 + q_0q_1)m_d \\
2(q_1q_3 + q_0q_2)m_n + (q_0^2-q_1^2-q_2^2+q_3^2)m_d
\end{bmatrix}
ヤコビ行列
{\bf F_t} = (\frac{\partial{\bf f_t}}{\partial{\bf x_t}})_{\bf x_t=\hat{x}_{t/t}} = \begin{bmatrix}
1 & -\frac{1}{2}(\omega_x - b_x)\Delta t & -\frac{1}{2}(\omega_y - b_y)\Delta t & -\frac{1}{2}(\omega_z - b_z)\Delta t & \frac{1}{2}q_1\Delta t & \frac{1}{2}q_2\Delta t & \frac{1}{2}q_3\Delta t \\
\frac{1}{2}(\omega_x - b_x)\Delta t & 1 & \frac{1}{2}(\omega_z - b_z)\Delta t & - \frac{1}{2}(\omega_y - b_y)\Delta t & -\frac{1}{2}q_0\Delta t & \frac{1}{2}q_3\Delta t & -\frac{1}{2}q_2\Delta t \\
\frac{1}{2}(\omega_y - b_y)\Delta t & -\frac{1}{2}(\omega_z - b_z)\Delta t & 1 & \frac{1}{2}(\omega_x - b_x)\Delta t & -\frac{1}{2}q_3\Delta t & -\frac{1}{2}q_0\Delta t & \frac{1}{2}q_1\Delta t \\
\frac{1}{2}(\omega_z - b_z)\Delta t & \frac{1}{2}(\omega_y - b_y)\Delta t & -\frac{1}{2}(\omega_x - b_x)\Delta t & 1 & \frac{1}{2}q_2\Delta t & -\frac{1}{2}q_1\Delta t & -\frac{1}{2}q_0\Delta t \\
0 & 0 & 0 & 0 & 1-\beta_x\Delta t & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 1-\beta_y\Delta t & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 1-\beta_z\Delta t
\end{bmatrix}
{\bf H_t} = (\frac{\partial{\bf h_t}}{\partial{\bf x_t}})_{\bf x_t=\hat{x}_{t/t-1}} = \begin{bmatrix} -2q_2g & 2q_3g & -2q_0g & 2q_1g & 0 & 0 & 0 \\
2q_1g & 2q_0g & 2q_3g & 2q_2g & 0 & 0 & 0 \\
2q_0g & -2q_1g & -2q_2g & 2q_3g & 0 & 0 & 0 \\
2q_0m_n-2q_2m_d & 2q_1m_n+2q_3m_d & -2q_2m_n - 2q_0m_d & -2q_3m_n + 2q_1m_d & 0 & 0 & 0 \\
-2q_3m_n + 2q_1m_d & 2q_2m_n + 2q_0m_d & 2q_1m_n + 2q_3m_d & -2q_0m_n + 2q_2m_d & 0 & 0 & 0 \\
2q_2m_n + 2q_0m_d & 2q_3m_n-2q_1m_d & 2q_0m_n-2q_2m_d & 2q_1m_n + 2q_3m_d & 0 & 0 & 0
\end{bmatrix}
拡張カルマンフィルタ
カルマンゲイン・状態値・共分散行列更新
\bf K_t = P_{t/t-1}H^T[H_tP_{t/t-1}H_t^T + R_t]^{-1} \\
\hat{x}_{t/t} = \hat{x}_{t/t-1} + K_t[y_t - h_t(\hat{x}_{t/t-1})] \\
P_{t/t} = P_{t/t-1} - K_t H_t P_{t/t-1} \\
状態値・行分散行列予測
\bf \hat{x}_{t+1/t} = f_t(\hat{x}_{t/t}) \\
P_{t+1/t} = F_tP_{t/t}F_t^T + G_tQ_tG_t^T
実装
kalman_filter.cpp
#include "ExtendedKalmanFilter.h"
#include "Eigen/Dense.h"
using namespace Eigen;
using namespace std;
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Vector3.h>
#include <ros.h>
Ekf::Ekf(double delta_t) {
this->_delta_t = delta_t;
_covariance_p = Eigen::Matrix4Xd::Zero(4, 4);
_covariance_r = Eigen::Matrix3Xd::Zero(3, 3);
_covariance_q = Eigen::Matrix4Xd::Zero(4, 4);
_covariance_q(0) = 1.00E-6;
_covariance_q(5) = 1.00E-6;
_covariance_q(10) = 1.00E-6;
_covariance_q(15) = 1.00E-6;
_covariance_r(0) = 2.0;
_covariance_r(4) = 2.0;
_covariance_r(8) = 2.0;
_covariance_p(0) = 0.5;
_covariance_p(5) = 0.5;
_covariance_p(10) = 0.5;
_covariance_p(15) = 0.5;
grav_acc = 9.79;
_state_value << 1.0, 0.0, 0.0, 0.0;
}
Ekf::~Ekf() {}
geometry_msgs::Quaternion Ekf::get_compensation_state(
const geometry_msgs::Vector3& linear_acc,
const geometry_msgs::Vector3& angular_vel) {
// clang-format off
Matrix<double, 3, 1> _gyro_sense;
_gyro_sense << angular_vel.x,
angular_vel.y,
angular_vel.z;
Matrix<double, 3, 1> _acc_geo_sense;
_acc_geo_sense << linear_acc.x,
linear_acc.y,
linear_acc.z;
// clang-format on
Matrix<double, 4, 1> predicted_state = _get_predicted_state(_gyro_sense);
Matrix<double, 4, 4> predicted_jacobian_F = _get_predicted_jacobian_F(_gyro_sense);
Matrix<double, 4, 4> predicted_covariance_p = _get_predicted_covariance_p(predicted_jacobian_F);
Matrix<double, 3, 4> observation_jacobian_H = _get_observation_jacobian_H(predicted_state);
Matrix<double, 3, 1> observation_state = _get_observation_state(predicted_state);
Matrix<double, 4, 3> kalman_gain = _get_kalman_gain(observation_jacobian_H, predicted_covariance_p);
_update_covariance_p(kalman_gain, observation_jacobian_H, predicted_covariance_p);
Matrix<double, 4, 1> com_quat = _get_compensation_state(predicted_state, kalman_gain, _acc_geo_sense, observation_state);
_state_value = com_quat;
double quat_norm = sqrt(com_quat(0) * com_quat(0) + com_quat(1) * com_quat(1) + com_quat(2) * com_quat(2) + com_quat(3) * com_quat(3));
geometry_msgs::Quaternion _compensation_quaternion;
_compensation_quaternion.w = com_quat(0) / quat_norm;
_compensation_quaternion.x = com_quat(1) / quat_norm;
_compensation_quaternion.y = com_quat(2) / quat_norm;
_compensation_quaternion.z = com_quat(3) / quat_norm;
return _compensation_quaternion;
}
Matrix<double, 4, 1> Ekf::_get_predicted_state(const Matrix<double, 3, 1>& gyro_sense) {
double omega_x_dt = 0.5 * _delta_t * gyro_sense(0);
double omega_y_dt = 0.5 * _delta_t * gyro_sense(1);
double omega_z_dt = 0.5 * _delta_t * gyro_sense(2);
Matrix<double, 4, 4> f;
f << 1, -omega_x_dt, -omega_y_dt, -omega_z_dt,
omega_x_dt, 1, omega_z_dt, -omega_y_dt,
omega_y_dt, -omega_z_dt, 1, omega_x_dt,
omega_z_dt, omega_y_dt, -omega_x_dt, 1;
Matrix<double, 4, 1> predicted_state;
predicted_state = f * _state_value;
return predicted_state;
}
Matrix<double, 4, 4> Ekf::_get_predicted_jacobian_F(const Matrix<double, 3, 1>& gyro_sense) {
double omega_x_dt = 0.5 * _delta_t * gyro_sense(0);
double omega_y_dt = 0.5 * _delta_t * gyro_sense(1);
double omega_z_dt = 0.5 * _delta_t * gyro_sense(2);
Matrix<double, 4, 4> predicted_jacobian_F;
predicted_jacobian_F << 1, -omega_x_dt, -omega_y_dt, -omega_z_dt,
omega_x_dt, 1, omega_z_dt, -omega_y_dt,
omega_y_dt, -omega_z_dt, 1, omega_x_dt,
omega_z_dt, omega_y_dt, -omega_x_dt, 1;
return predicted_jacobian_F;
}
Matrix<double, 4, 4> Ekf::_get_predicted_covariance_p(const Matrix<double, 4, 4>& predicted_jacobian_F) {
Matrix<double, 4, 4> predicted_covariance_p;
predicted_covariance_p = predicted_jacobian_F * _covariance_p * predicted_jacobian_F.transpose() + _covariance_q;
return predicted_covariance_p;
}
Matrix<double, 3, 4> Ekf::_get_observation_jacobian_H(const Matrix<double, 4, 1>& predicted_state) {
Matrix<double, 3, 4> observation_jacobian_H;
observation_jacobian_H << -predicted_state(2), predicted_state(3), -predicted_state(0), predicted_state(1),
predicted_state(1), predicted_state(0), predicted_state(3), predicted_state(2),
predicted_state(0), -predicted_state(1), -predicted_state(2), predicted_state(3);
return observation_jacobian_H * 2.0 * grav_acc;
}
Matrix<double, 3, 1> Ekf::_get_observation_state(const Matrix<double, 4, 1>& predicted_state) {
Matrix<double, 3, 1> observation_state;
// clang-format off
observation_state << 2 * (predicted_state(1) * predicted_state(3) - predicted_state(0) * predicted_state(2)) * grav_acc,
2 * (predicted_state(2) * predicted_state(3) + predicted_state(0) * predicted_state(1)) * grav_acc,
(predicted_state(0) * predicted_state(0) - predicted_state(1) * predicted_state(1) - predicted_state(2) * predicted_state(2) + predicted_state(3) * predicted_state(3)) * grav_acc;
// clang-format on
return observation_state;
}
Matrix<double, 4, 3> Ekf::_get_kalman_gain(const Matrix<double, 3, 4>& observation_jacobian_H,
const Matrix<double, 4, 4>& predicted_covariance_p) {
Matrix<double, 4, 3> kalman_gain;
kalman_gain = predicted_covariance_p * observation_jacobian_H.transpose() * (observation_jacobian_H * predicted_covariance_p * observation_jacobian_H.transpose() + _covariance_r).inverse();
return kalman_gain;
}
void Ekf::_update_covariance_p(const Matrix<double, 4, 3>& kalman_gain,
const Matrix<double, 3, 4>& observation_jacobian_H,
const Matrix<double, 4, 4>& predicted_covariance_p) {
_covariance_p = (Matrix<double, 4, 4>::Identity() - kalman_gain * observation_jacobian_H) * predicted_covariance_p;
}
Matrix<double, 4, 1> Ekf::_get_compensation_state(const Matrix<double, 4, 1>& predicted_state,
const Matrix<double, 4, 3>& kalman_gain,
const Matrix<double, 3, 1>& acc_geo_sense,
const Matrix<double, 3, 1>& observation_state) {
Matrix<double, 4, 1> compensation_state;
compensation_state = predicted_state + kalman_gain * (acc_geo_sense - observation_state);
return compensation_state;
}
評価
見た感じうまく推定できてるっぽいです。
自分で作ったやつでも動いた！ pic.twitter.com/5MX9cQjubA
— たけし (@NibiJim) January 24, 2021
補正をした後の値を更新するのを忘れいていた(_state_value = com_quat;の部分)時にハマってしまったので、一応madgwickフィルタのライブラリを使っても動かしました。
こちらは地磁気の値も使っても上手く動きました。
正確さと速度的にはmadgwickフィルタを使うことになりそうですが、気が向いたらちゃんと比較してみようと思います。
やっと姿勢推定ができるようになったので、とりあえず一安心です。
全く違う内容の卒業研究にも追われ、なかなか素早く進めることができていませんが、ここまで読んで頂きありがとうございました。
参考
https://www.jstage.jst.go.jp/article/jrsj1983/26/6/26_6_626/_pdf
https://www.jstage.jst.go.jp/article/sposun/22/2/22_255/_pdf/-char/en
https://www.analog.com/media/jp/analog-dialogue/volume-53/number-1/strapdown-inertial-navigation-system-based-on-an-imu-and-a-geomagnetic-sensor_jp.pdf
https://www.jstage.jst.go.jp/article/jje/50/4/50_182/_pdf
ドローン制御工学入門
https://watako-lab.com/2019/02/20/3axis_cmps/