mirs_esp_pio
読み取り中…
検索中…
一致する文字列を見つけられません
Mirs2605Ekf.hpp ファイル

車輪とIMUの値から、計測のばらつきを考慮して機体の位置・向きを推定する。 [詳解]

#include <stdint.h>

[ソースコード]

クラス

class  Mirs2605Ekf

詳解

車輪とIMUの値から、計測のばらつきを考慮して機体の位置・向きを推定する。

差動二輪用・ESP32向け軽量EKF(計算層・ヒープ不使用)

状態: [x, y, theta, v, bg](位置・姿勢・前進速度・ジャイロバイアス)。 角速度 w 自体は状態に持たず、入力 gz から (gz - bg) として求める。 5x5 共分散1個だけ保持し、更新は全てスカラ逐次更新のため 行列逆行列・動的確保・Eigen は不要。15ms制御周期で完結する。

融合:

  • predict(gz, dt): ジャイロで姿勢・位置を伝搬
  • updateVel(v_enc): エンコーダ前進速度で v を補正
  • updateYawRate(w_enc, gz): エンコーダ角速度で bg を補正
  • updateMagTheta(yaw_mag): 地磁気ヨーで theta を補正
  • updateZeroVel(): 停止検出時の v=0 拘束(加速度計由来のZUPT用)

座標の約束: x前・y左・theta反時計回り(ROS REP-103準拠)。 BMX055の軸とロボット軸が一致していることが前提。 不一致・マウント回転・磁気 declination は呼び出し側で補正すること。

Mirs2605Ekf.hpp に定義があります。