mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
Mirs2605Ekf クラス

#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 行目に定義があります。

列挙型メンバ詳解

◆ StateIdx

列挙値
IX 
IY 
ITH 
IV 
IBG 
N 

Mirs2605Ekf.hpp の 27 行目に定義があります。

構築子と解体子

◆ Mirs2605Ekf()

Mirs2605Ekf::Mirs2605Ekf ( )

Mirs2605Ekf.cpp の 18 行目に定義があります。

関数詳解

◆ angleResidual()

float Mirs2605Ekf::angleResidual ( float meas,
float pred )
static

Mirs2605Ekf.cpp の 221 行目に定義があります。

◆ cov()

float Mirs2605Ekf::cov ( int i,
int j ) const

Mirs2605Ekf.cpp の 69 行目に定義があります。

◆ gyroBias()

float Mirs2605Ekf::gyroBias ( ) const
inline

Mirs2605Ekf.hpp の 87 行目に定義があります。

◆ isStationary()

bool Mirs2605Ekf::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 )
static

停止判定。加速度ノルムがg付近かつ角速度・車輪速度が微小。

Mirs2605Ekf.cpp の 246 行目に定義があります。

◆ magYawTiltCompensated()

float Mirs2605Ekf::magYawTiltCompensated ( float mx,
float my,
float mz,
float ax,
float ay,
float az )
static

加速度でチルト補正した地磁気ヨーを求める。

戻り値
ヨー [rad](-PI..PI)。加速度ノルム異常時は mag の生ヨーを返す。

Mirs2605Ekf.cpp の 225 行目に定義があります。

◆ normalizeAngle()

float Mirs2605Ekf::normalizeAngle ( float a)
static

Mirs2605Ekf.cpp の 217 行目に定義があります。

◆ predict()

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 行目に定義があります。

◆ reset()

void Mirs2605Ekf::reset ( )

Mirs2605Ekf.cpp の 22 行目に定義があります。

◆ setInitialCovariance()

void Mirs2605Ekf::setInitialCovariance ( float p_xy,
float p_theta,
float p_v,
float p_bg )

初期共分散の対角を設定(起動直後の不確かさ)。

Mirs2605Ekf.cpp の 61 行目に定義があります。

◆ setMeasurementNoise()

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 行目に定義があります。

◆ setProcessNoise()

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 行目に定義があります。

◆ theta()

float Mirs2605Ekf::theta ( ) const
inline

Mirs2605Ekf.hpp の 85 行目に定義があります。

◆ updateMagTheta()

void Mirs2605Ekf::updateMagTheta ( float yaw_mag_rad,
float r_override = -1.0f )

地磁気ヨーで theta を補正する(屋内磁気外乱時は呼ばないこと)。

Mirs2605Ekf.cpp の 207 行目に定義があります。

◆ updateVel()

void Mirs2605Ekf::updateVel ( float v_enc,
float r_override = -1.0f )

エンコーダ前進速度で v を補正する。

Mirs2605Ekf.cpp の 181 行目に定義があります。

◆ updateYawRate()

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 行目に定義があります。

◆ updateYawRateGated()

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 行目に定義があります。

◆ updateZeroVel()

void Mirs2605Ekf::updateZeroVel ( float r_override = -1.0f)

停止中拘束。v=0 の観測として扱う(ZUPT)。

Mirs2605Ekf.cpp の 212 行目に定義があります。

◆ v()

float Mirs2605Ekf::v ( ) const
inline

Mirs2605Ekf.hpp の 86 行目に定義があります。

◆ x()

float Mirs2605Ekf::x ( ) const
inline

Mirs2605Ekf.hpp の 83 行目に定義があります。

◆ y()

float Mirs2605Ekf::y ( ) const
inline

Mirs2605Ekf.hpp の 84 行目に定義があります。


このクラス詳解は次のファイルから抽出されました: