|
mirs_esp_pio
|
#include <Mirs2605Ekf.hpp>
公開型 | |
| enum | StateIdx { IX = 0 , IY = 1 , ITH = 2 , IV = 3 , IBG = 4 , N = 5 } |
公開メンバ関数 | |
| Mirs2605Ekf () | |
| void | reset () |
| void | setProcessNoise (float q_theta, float q_v, float q_bg) |
| プロセスノイズ対角を設定(既定値は室内低速向け)。 | |
| void | setMeasurementNoise (float r_vel, float r_yaw_rate, float r_mag) |
| 観測ノイズ(分散)を設定。 | |
| void | setInitialCovariance (float p_xy, float p_theta, float p_v, float p_bg) |
| 初期共分散の対角を設定(起動直後の不確かさ)。 | |
| void | predict (float gz_rad_s, float dt_sec) |
| 予測ステップ。ジャイロで theta/x/y を伝搬する。 | |
| void | updateVel (float v_enc, float r_override=-1.0f) |
| エンコーダ前進速度で v を補正する。 | |
| void | updateYawRate (float w_enc, float gz_rad_s, float r_override=-1.0f) |
| エンコーダ角速度でジャイロバイアスを補正する。 | |
| void | updateYawRateGated (float w_enc, float gz_rad_s, float v_enc, float v_gate=0.05f, float r_override=-1.0f) |
| 低速ゲート付きバイアス更新。 | |
| void | updateMagTheta (float yaw_mag_rad, float r_override=-1.0f) |
| 地磁気ヨーで theta を補正する(屋内磁気外乱時は呼ばないこと)。 | |
| void | updateZeroVel (float r_override=-1.0f) |
| 停止中拘束。v=0 の観測として扱う(ZUPT)。 | |
| float | x () const |
| float | y () const |
| float | theta () const |
| float | v () const |
| float | gyroBias () const |
| float | cov (int i, int j) const |
静的公開メンバ関数 | |
| static float | normalizeAngle (float a) |
| static float | angleResidual (float meas, float pred) |
| static float | magYawTiltCompensated (float mx, float my, float mz, float ax, float ay, float az) |
| 加速度でチルト補正した地磁気ヨーを求める。 | |
| static bool | isStationary (float ax, float ay, float az, float gz_rad_s, float v_enc, float acc_tol=0.6f, float gz_tol=0.03f, float v_tol=0.02f) |
| 停止判定。加速度ノルムがg付近かつ角速度・車輪速度が微小。 | |
Mirs2605Ekf.hpp の 25 行目に定義があります。
| 列挙値 | |
|---|---|
| IX | |
| IY | |
| ITH | |
| IV | |
| IBG | |
| N | |
Mirs2605Ekf.hpp の 27 行目に定義があります。
| Mirs2605Ekf::Mirs2605Ekf | ( | ) |
Mirs2605Ekf.cpp の 18 行目に定義があります。
|
static |
Mirs2605Ekf.cpp の 221 行目に定義があります。
| float Mirs2605Ekf::cov | ( | int | i, |
| int | j ) const |
Mirs2605Ekf.cpp の 69 行目に定義があります。
|
inline |
Mirs2605Ekf.hpp の 87 行目に定義があります。
|
static |
停止判定。加速度ノルムがg付近かつ角速度・車輪速度が微小。
Mirs2605Ekf.cpp の 246 行目に定義があります。
|
static |
加速度でチルト補正した地磁気ヨーを求める。
Mirs2605Ekf.cpp の 225 行目に定義があります。
|
static |
Mirs2605Ekf.cpp の 217 行目に定義があります。
| void Mirs2605Ekf::predict | ( | float | gz_rad_s, |
| float | dt_sec ) |
予測ステップ。ジャイロで theta/x/y を伝搬する。
| gz_rad_s | ジャイロz [rad/s](ロボットz軸と同符号のこと) |
| dt_sec | 周期 [s](<=0では何もしない。>0.1は0.1にクランプ) |
Mirs2605Ekf.cpp の 76 行目に定義があります。
| void Mirs2605Ekf::reset | ( | ) |
Mirs2605Ekf.cpp の 22 行目に定義があります。
| void Mirs2605Ekf::setInitialCovariance | ( | float | p_xy, |
| float | p_theta, | ||
| float | p_v, | ||
| float | p_bg ) |
初期共分散の対角を設定(起動直後の不確かさ)。
Mirs2605Ekf.cpp の 61 行目に定義があります。
| void Mirs2605Ekf::setMeasurementNoise | ( | float | r_vel, |
| float | r_yaw_rate, | ||
| float | r_mag ) |
観測ノイズ(分散)を設定。
| r_vel | エンコーダ速度 [(m/s)^2] |
| r_yaw_rate | エンコーダ角速度 [(rad/s)^2] |
| r_mag | 地磁気ヨー [rad^2] |
Mirs2605Ekf.cpp の 49 行目に定義があります。
| void Mirs2605Ekf::setProcessNoise | ( | float | q_theta, |
| float | q_v, | ||
| float | q_bg ) |
プロセスノイズ対角を設定(既定値は室内低速向け)。
| q_theta | 姿勢伝搬ノイズ [(rad^2/s)] |
| q_v | 速度ランダムウォーク [(m/s)^2/s] |
| q_bg | バイアスランダムウォーク [(rad/s)^2/s] |
Mirs2605Ekf.cpp の 37 行目に定義があります。
|
inline |
Mirs2605Ekf.hpp の 85 行目に定義があります。
| void Mirs2605Ekf::updateMagTheta | ( | float | yaw_mag_rad, |
| float | r_override = -1.0f ) |
地磁気ヨーで theta を補正する(屋内磁気外乱時は呼ばないこと)。
Mirs2605Ekf.cpp の 207 行目に定義があります。
| void Mirs2605Ekf::updateVel | ( | float | v_enc, |
| float | r_override = -1.0f ) |
エンコーダ前進速度で v を補正する。
Mirs2605Ekf.cpp の 181 行目に定義があります。
| void Mirs2605Ekf::updateYawRate | ( | float | w_enc, |
| float | gz_rad_s, | ||
| float | r_override = -1.0f ) |
エンコーダ角速度でジャイロバイアスを補正する。
真の角速度を w_enc とみなし、z = gz - w_enc を bg の観測として扱う(H = [0,0,0,0,1])。
Mirs2605Ekf.cpp の 186 行目に定義があります。
| void Mirs2605Ekf::updateYawRateGated | ( | float | w_enc, |
| float | gz_rad_s, | ||
| float | v_enc, | ||
| float | v_gate = 0.05f, | ||
| float | r_override = -1.0f ) |
低速ゲート付きバイアス更新。
|v_enc| が v_gate 超では更新しない。走行中のエンコーダ 角速度ノイズが bg 推定に混入するのを防ぐ(bgは停止付近で可観測)。 制御周期内の使用は本関数を使うこと。
Mirs2605Ekf.cpp の 192 行目に定義があります。
| void Mirs2605Ekf::updateZeroVel | ( | float | r_override = -1.0f | ) |
停止中拘束。v=0 の観測として扱う(ZUPT)。
Mirs2605Ekf.cpp の 212 行目に定義があります。
|
inline |
Mirs2605Ekf.hpp の 86 行目に定義があります。
|
inline |
Mirs2605Ekf.hpp の 83 行目に定義があります。
|
inline |
Mirs2605Ekf.hpp の 84 行目に定義があります。