ROS 2 センサー融合 SDK (FusionCore)

IMU、ホイールエンコーダー、GPS のデータを一つの信頼性の高い位置推定値に統合します。自己調整式のノイズ共分散機能を備え、Apache License 2.0 のもとで公開されています。

これはどのような問題を解決するのでしょうか？

移動ロボットは、複数のセンサー（IMU、ホイールエンコーダー、GPS）からの情報を元に「現在何处にあるか」を把握する必要があります。しかし、それぞれのセンサーには独自の弱点があります。IMU はドリフトを起こし、ホイールエンコーダーでは滑りが生じ、GPS の信号は一瞬で飛ぶことがあり得ます。これらの 3 つの異なるセンサーデータを賢く統合して、一つの信頼できる位置推定値に変換するソフトウェア—that is what we call a センサー融合パッケージです。

標準的な ROS パッケージである

robot_localization

fuse

ベンチマーク結果

ミシガン大学が公開する NCLT データセット上で、FusionCore と

robot_localization

FusionCore は 6 つのシリーズのうち 5 つで勝っています。 2012 年 11 月 4 日（秋、GPS 劣化）では、慣性コリスモード（ consecutiveremotions に起因する Q インフレーション）によりマルコバノイス異常検出門閾値が失効しましたが、これは GPS が長期間にわたり十分に劣化したため、蓄積されたドリフトを完全に回復しきれなかったことを示しています。RL-EKF は異常検出门閾値を持ちず、直ちに自己修正を試みます。RL-UKF は 6 つのシリーズすべてで NaN による発散 occurredしました。

詳細な方法論、設定ファイル、再現手順については

benchmarks/

なぜ FusionCore を選ぶべきか？

navsat_transform

mahalanobis_threshold

history_length

/diagnostics

~/reset

インストール

事前条件

• ROS 2 Jazzy Jalisco（主要対応）または ROS 2 Kilted（コミュニティテスト済み）

• colcon

  ワークスペース例 (

  ~/ros2_ws

  )

ワークスペースへのクローン

これは 4 つの独立した

ament_cmake

compass_msgs

fusioncore_core

fusioncore_ros

fusioncore_gazebo

package.xml

colcon

src/

package.xml

colcon

src/

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
git clone https://github.com/manankharwar/fusioncore.git
cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash
rosdep install --from-paths src --ignore-src -r -y
colcon build
source install/setup.bash

リアルロボット用ユーザー（Gazebo 不要の場合）

注意:

fusioncore_gazebo

ros_gz_sim

COLCON_IGNORE

touch ~/ros2_ws/src/fusioncore/fusioncore_gazebo/COLCON_IGNORE

これにより、

colcon

fusioncore_core

fusioncore_ros

テストの実行

cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash
colcon build --packages-select fusioncore_core --cmake-args -DBUILD_TESTING=ON
colcon test --packages-select fusioncore_core
colcon test-result --verbose

期待される出力: 39 テスト、0 エラー、0 フォールチャー、0 スキップ。

FusionCore の実行

ターミナル 1: ノードの起動

ros2 launch fusioncore_ros fusioncore.launch.py

ターミナル 2: ライフェサイクルノードの設定とアクティベート

ros2 lifecycle set /fusioncore configure
ros2 lifecycle set /fusioncore activate

100Hz で出力が確認できるか検証

ros2 topic hz /fusion/odom
# expected: average rate: 100.000

FusionCore は ROS 2 ライフェサイクルノードを使用しています。 まず構成を行う（パラメータを読み込み、TF ツリーを検証、変換を確認し）、その後アクティベートしてセンサーデータの処理を開始します。これにより、フィルタが悪い初期値や欠落する変換を含む状態で起動するのを防ぎます。

WSL2 に関する注記

ros2 lifecycle set

fusioncore_gazebo.launch.py

EmitEvent(ChangeState(...))

全機能の動作確認（実機ロボットなしで）

物理的なロボットを使わず、フェイクセンサーデータを使用して FusionCore の全ての機能をテストできます。

~/YOUR_WS

~/ros2_ws

~/fusioncore_ws

ターミナル 1: FusionCore の起動

source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash
ros2 launch fusioncore_ros fusioncore.launch.py

ターミナル 2: 構成とアクティベート

source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash

# 必要な TF 変換をパブリッシュ（バックグラウンドで動作し続ける）
ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id imu_link &
ros2 run tf2_ros static_transform_publisher --frame-id odom --child-frame-id base_link &
sleep 1

ros2 lifecycle set /fusioncore configure
sleep 1
ros2 lifecycle set /fusioncore activate

ターミナル 3: フェイクセンサーの送信

source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash

# 静止した状態 (重力が上向きの) IMU @ 100Hz のフェイクデータ
ros2 topic pub /imu/data sensor_msgs/msg/Imu "{
  header: {frame_id: 'base_link'},
  angular_velocity: {x: 0.0, y: 0.0, z: 0.0},
  linear_acceleration: {x: 0.0, y: 0.0, z: 9.81},
  orientation_covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
}" --rate 100 &

# 静止した状態 (50Hz) のフェイクホイールエンコーダー (ZUPT をトリガー)
ros2 topic pub /odom/wheels nav_msgs/msg/Odometry "{
  header: {frame_id: 'odom'},
  twist: {twist: {linear: {x: 0.0}, angular: {z: 0.0}}}
}" --rate 50 &

# ハミルトン、オンタリオの GPS @ 5Hz のフェイクデータ
ros2 topic pub /gnss/fix sensor_msgs/msg/NavSatFix "{
  header: {frame_id: 'base_link'},
  status: {status: 0},
  latitude: 43.2557,
  longitude: -79.8711,
  altitude: 100.0,
  position_covariance: [1.0, 0, 0, 0, 1.0, 0, 0, 0, 4.0],
  position_covariance_type: 2
}" --rate 5

ターミナル 4: 各機能の検証

ライブ中のトピックとサービスを確認:

source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash
ros2 topic list | grep fusion
ros2 service list | grep fusioncore

/fusion/odom

/fusion/pose

/fusioncore/reset

/fusion/pose

ros2 topic echo /fusion/pose --once

UKF から生成されたフル 6×6 の共分散行列を持つポーズメッセージが表示されるはずです。

/diagnostics

ros2 topic echo /diagnostics --once

4 つの状態エントリ（

fusioncore: IMU

fusioncore: Encoder

fusioncore: GNSS

fusioncore: Filter

OK

WARN

ZUPT をテスト: 静止時、速度はほぼゼロを保つべき:

ros2 topic echo /fusion/odom --field twist.twist.linear

IMU が動作しているにもかかわらず、値は本質的にゼロ (~1e-10) であるはずです。これは ZUPT がロボットが動かない間、IMU の速度ドリフトを抑制していることを確認します。

リセットサービス (

/fusioncore/reset

ros2 service call /fusioncore/reset std_srvs/srv/Trigger

期待される出力:

success: True

message: 'FusionCore filter reset. GPS reference cleared.'

リセットコールから 1 秒以内に、ノードログ（ターミナル 1）に以下が表示されるはずです：

Filter reset via ~/reset service.
GNSS reference set: lat=43.255700 lon=-79.871100

GPS 固定拒否の警告はなく、リセット直後に GPS が再融合されます。

センサートピック

サブスクライブするトピック

/imu/data

sensor_msgs/Imu

/odom/wheels

nav_msgs/Odometry

/gnss/fix

sensor_msgs/NavSatFix

/gnss/heading

sensor_msgs/Imu

gnss.azimuth_topic

compass_msgs/Azimuth

gnss.fix2_topic

sensor_msgs/NavSatFix

パブリッシュするトピック

/fusion/odom

nav_msgs/Odometry

/fusion/pose

geometry_msgs/PoseWithCovarianceStamped

/diagnostics

diagnostic_msgs/DiagnosticArray

/tf

TF

odom -> base_link

サービス

~/reset

std_srvs/Trigger

構成 (Configuration)

fusioncore:
  ros__parameters:
    base_frame: base_link
    odom_frame: odom
    publish_rate: 100.0

    imu.gyro_noise: 0.005       # rad/s: IMU データシートからの値
    imu.accel_noise: 0.1        # m/s²
    imu.has_magnetometer: false # 9 アキシス IMU (BNO08x, VectorNav, Xsens) の場合 true
                                # 6 アキシスの場合は false (ジャイロ積分によるヨードリフトあり)
    imu.remove_gravitational_acceleration: false  # 静止時、ロボットが Z 方向にドリフトする場合 true に設定
                                                   # 多くの IMU は生体力 (重力を含む) を報告するため

    encoder.vel_noise: 0.05     # m/s
    encoder.yaw_noise: 0.02     # rad/s

    gnss.base_noise_xy: 1.0     # メートル: HDOP に応じて自動的にスケーリング
    gnss.base_noise_z: 2.0      # メートル
    gnss.heading_noise: 0.02    # ラジアン (デュアンテナ用)
    gnss.max_hdop: 4.0          # これより悪い固定値を拒否
    gnss.min_satellites: 4
    gnss.min_fix_type: 1        # 最小固定品質：1=GPS, 2=DGPS, 3=RTK_FLOAT, 4=RTK_FIXED
                                # 注意：sensor_msgs/NavSatFix status=2 は RTK_FIXED に対応。
                                # RTK_FLOAT (3) は NavSatFix を通じて利用できないため、2 または 4 を使用してください。

    # アンテナレバーアーム：ボディフレームにおける base_link から主 GPS アンテナのオフセット
    # x=前方、y=左方、z=上 (メートル)。アンテナが base_link より上にある場合は 0 で維持。
    # レバーアーム補正は方位角が独立して検証された時にのみ有効になります。
    gnss.lever_arm_x: 0.0
    gnss.lever_arm_y: 0.0
    gnss.lever_arm_z: 0.0

    # 次点 GPS レシーバーのレバーアーム (gnss.fix2_topic を使用する場合)
    gnss.lever_arm2_x: 0.0
    gnss.lever_arm2_y: 0.0
    gnss.lever_arm2_z: 0.0

    # オプションの次点 GPS レシーバー
    gnss.fix2_topic: ""

    # 方位角トピック：1 つまたは両方を選択
    gnss.heading_topic: "/gnss/heading"   # sensor_msgs/Imu
    gnss.azimuth_topic: ""                # compass_msgs/Azimuth (推奨)

    # マハラノビス異常値拒否
    outlier_rejection: true
    outlier_threshold_gnss: 16.27   # chi2(3, 0.999): 3D 位置
    outlier_threshold_hdg: 10.83    # chi2(1, 0.999): 1D 方位角
    outlier_threshold_enc: 11.34    # chi2(3, 0.999): 3D エンコーダー
    outlier_threshold_imu: 15.09    # chi2(6, 0.999): 6D IMU

    # 適応型ノイズ共分散
    adaptive.imu: true
    adaptive.encoder: true
    adaptive.gnss: true
    adaptive.window: 50
    adaptive.alpha: 0.01

    ukf.q_position: 0.01
    ukf.q_orientation: 1.0e-9  # クォータニオン正規化のみ：これを増加させないでください
    ukf.q_velocity: 0.1
    ukf.q_angular_vel: 0.1
    ukf.q_acceleration: 1.0
    ukf.q_gyro_bias: 1.0e-5
    ukf.q_accel_bias: 1.0e-5

古い構成からアップグレード: もし YAML に

ukf.q_orientation: 0.01

1.0e-9

GPS 座標参照システム (CRS)

FusionCore は PROJ を使用して入力 GNSS 固定値の座標系を変換します。デフォルト設定は任意の標準 GPS レシーバー（WGS84 lat/lon → ECEF）に対応しています。レシーバーが異なる CRS を出力する場合のみ変更してください。

# PROJ 座標参照システム
input.gnss_crs: "EPSG:4326"              # 入力 NavSatFix メッセージの CRS
                                          # EPSG:4326 = WGS84 lat/lon (標準 GPS)
                                          # EPSG:32617 = UTM zone 17N (一部の RTK レシーバー)
output.crs: "EPSG:4978"                  # 内部計算用 CRS
                                          # EPSG:4978 = ECEF XYZ (デフォルト、世界的に有効)
output.convert_to_enu_at_reference: true  # output.crs が ECEF の場合 true
                                          # output.crs が既にローカル投影 CRS の場合 false
reference.use_first_fix: true            # ローカル ENU 起源を最初の GPS 固定値にアンカー
reference.x: 0.0                         # use_first_fix: false の場合、output.crs 内の固定原点
reference.y: 0.0
reference.z: 0.0

農業用 RTK 例: レシーバーが直接 UTM zone 17N (easting/northing) を出力する場合：

input.gnss_crs: "EPSG:32617"
output.crs: "EPSG:32617"
output.convert_to_enu_at_reference: false
reference.use_first_fix: true

FusionCore が「難しい問題」をどのように処理するか

IMU フレーム変換

IMU はほとんどが

base_link

frame_id

base_link

[WARN] Cannot transform IMU from imu_link to base_link.
Fix: ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id imu_link

起動時の TF 検証

configure

--- TF Validation ---
  [OK]      imu_link -> base_link
  [MISSING] base_link -> odom  Fix: ros2 run tf2_ros static_transform_publisher --frame-id odom --child-frame-id base_link
---------------------

マハラノビス異常値拒否

GPS 固定値を融合する前に、現在の状態推定値に対して該測量が統計的にどれほど不可能かを FusionCore は計算します。マハラノビス距離 $d^2 = \nu^T \cdot S^{-1} \cdot \nu$ をカイ二乗分布の 99.9 百分位数閾値と比較します。閾値を超える固定値はフィルタを更新せずに拒否されます。

この機能により、GPS ジャンプ、マルチパス誤差、エンコーダースリップスパイクを処理できます。フィルタ位置は拒否中も安定したままです：テストで 500m の GPS ジャンプを注入し、位置変化がゼロであることを観察することで確認されています。

GNSS 位置共分散はゲート評価前に床（flooring）されます。これにより、RTK クラスのレシーバー（典型的な $\sigma_{xy} \sim 3\text{mm}$ ）がフィルタがまだ RTK レベル精度に到達していないのに自己拒否を引き起こすのを防ぎます。

ゼロ速度更新 (ZUPT)

ロボットが静止している場合：エンコーダ速度 < 0.05 m/s、角速度 < 0.05 rad/s：FusionCore は非常に狭いノイズを持つゼロ速度疑似測定値を融合します。これにより、ロボットが止まっている間、IMU から速度推定値のドリフトが生じるのを防ぎます。すべての深刻な慣性航法システムはこの機能を実装しています。ZUPT がなければ、ロボットが動かないにもかかわらず、IMU ノイズが時間とともに仮想的な速度推定値に蓄積します。

適応型ノイズ共分散

FusionCore は各センサーについてイノベーション履歴の 50 メッセージ分のスライディングウィンドウを跟踪し、実際のコスパリアンスをデータから推定します。ノイズ行列 $R$ は、$\alpha=0.01$ の指数平滑平均を使用してゆっくりと推定された真値に向き更新されます。数分間の動作後、$R$ は自動的に実際のセンサー特性に収束します。手動 YAML 調整は不要です。

GPS アンテナオフセット (レバーアーム)

GPS アンテナが

base_link

base_link

各 GPS レシーバーは独自の独立したレバーアームを持ちます。主レシーバーは

gnss.lever_arm_x/y/z

gnss.lever_arm2_x/y/z

方位角可観測性 (Heading Observability)

FusionCore は独立した真のソースからのみ有効に設定される

heading_validated_

• DUAL_ANTENNA: デュアンテナ方位角メッセージが受信された場合
• IMU_ORIENTATION: 9 アキシス AHRS がフルオリエントを公開した場合（

  imu.has_magnetometer: true

  のみ有効；6 アキシス IMU はヨーでドリフトするためカウントされない）
• GPS_TRACK: ロボットが速度 $\ge$ 0.2 m/s で 5 メートル以上走行し、ヨーレート $\le$ 0.3 rad/s の場合

これら以前に、レバーアームは無効化されます。

GPS 固定品質ゲートリング

FusionCore は

sensor_msgs/NavSatFix.status

gnss.min_fix_type: 4   # RTK_FIXED を要求：基本的な GPS 固定値を完全に拒否

品質により固定値が拒否された場合、拒否ログは固定タイプと閾値を示します：

[WARN] GNSS fix rejected (fix_type=1, min=4, hdop=1.20, quality check or Mahalanobis gate)

重要:

sensor_msgs/NavSatFix

min_fix_type: 3

センサー単位の診断

FusionCore は 1Hz で

/diagnostics

rqt_robot_monitor

フィルタリセットサービス

ros2 service call /fusioncore/reset std_srvs/srv/Trigger

UKF 状態を再初期化し、GPS リファレンスアンカーをクリアします。ロボットは次の GPS 固定値で再度アンカー付けられます。ノード再起動は不要です。

メッセージ共分散

FusionCore はセンサーが実際にパブリッシュする共分散値を使用します。GPS:

position_covariance_type == 3

twist.covariance

orientation_covariance

遅延補償

FusionCore は 100 IMU メッセージ（100Hz で 1 秒分）のリングバッファを保持します。遅れた GPS 固定値が到着した際、修正タイムスタンプ前の最も近い状態スナップショットを復元し、正しい時間に修正値を再融合し、その後すべてのバッファされた IMU メッセージを現在時刻まで前方にリプレイします。これにより、遅延測定値における運動モデル近似誤差を排除します。

非ホロノミック地上制約 (Non-Holonomic Ground Constraint)

ホイール駆動の地上用ロボットについては、FusionCore は各エンコーダ更新で $V_Z = 0$ の疑似測定値を融合します。これにより、IMU ノイズによる垂直方向速度のドリフトを防ぎます。空飛ぶ車両や垂直方向に移動するロボットには使用しないでください。

センサードロップ検出

FusionCore は各センサーの最後の更新時間を独立して跟踪しています。センサーが

stale_timeout

get_status()

SensorHealth::STALE

compass_msgs/Azimuth

FusionCore は ROS 1 のみである upstream の

compass_msgs/Azimuth

シミュレーション

FusionCore は Gazebo Harmonic シミュレーションワールドを付属しており、物理ハードウェアなしでフル融合パイプラインのテストが可能です。室外環境に配置され、GPS 起源がオンタリオ州ハミルトンに設定された差分駆動ロボット（100Hz IMU と GPS）が含まれています。

Gazebo Harmonic の組み込み NavSat センサーには既知のバグ (gz-sim issue #2163) があり、定期的に完全に異なる座標で GPS 固定値を出力します。壊れたセンサーと戦うのではなく、シミュレーションは Gazebo の ground truth ワールドポーズから GPS を導出し、現実的なガウスノイズ（水平 0.5m、垂直 0.3m 1-sigma）を加算します。

シミュレーションの事前条件

Gazebo Harmonic と ROS-Gazebo ブリッジは

rosdep

sudo apt install ros-jazzy-ros-gz

シミュレーションの実行

cd ~/ros2_ws
source /opt/ros/jazzy/setup.bash
colcon build
source install/setup.bash
ros2 launch fusioncore_gazebo fusioncore_gazebo.launch.py

ロボットを運転して融合位置を見ます：

ターミナル 2: 円を描くように走行

source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.5}, angular: {z: 0.3}}" --rate 10

ターミナル 3: 位置を確認

source /opt/ros/jazzy/setup.bash
source ~/YOUR_WS/install/setup.bash
ros2 topic echo /fusion/odom --field pose.pose.position

インテグレーションテスト

python3 ~/ros2_ws/src/fusioncore/fusioncore_gazebo/launch/integration_test.py

4 つの自動テスト：IMU ドリフト率、異常値拒否、ドリフト後の GPS 補正、完全円走行復帰。クリーンセッションではすべて通過します。

リアルロボット構成ファイル

ハードウェア構成は

fusioncore_ros/config/

clearpath_husky.yaml

bno085_custom.yaml

duatic_mecanum.yaml

ハードウェア構成を変更せずに、動作環境に合わせて GPS 信頼度を調整するには、環境プリセット（

env_open.yaml

env_urban.yaml

env_canopy.yaml

ロボットの追加構成を追加する場合は、Hardware Config Request を開くか PR を送信してください：

CONTRIBUTING.md

robot_localization からの移行

完全なパラメータマッピング、トピック変更、ステップバイステップ手順：

docs/migration_from_robot_localization.md

Nav2 と FusionCore の統合

FusionCore は Nav2 用のドロップインオドメトリソースです。必要なものをすべてパブリッシュしており、リマップ也不要、追加ノード也不要です。

FusionCore が Nav2 で使用する出力:

/fusion/odom

nav2_params.yaml

odom_topic

odom → base_link TF

/fusion/pose

/diagnostics

ステップ 1: Nav2 を FusionCore のオドメトリに向けたい

nav2_params.yaml

odom_topic

/fusion/odom

amcl:
  ros__parameters:
    odom_topic: /fusion/odom

bt_navigator:
  ros__parameters:
    odom_topic: /fusion/odom

velocity_smoother:
  ros__parameters:
    odom_topic: /fusion/odom

ステップ 2: FusionCore と Nav2 を同時にローンチしたい

fusioncore_nav2.launch.py

ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
  fusioncore_config:=/path/to/your/fusioncore.yaml \
  nav2_params:=/path/to/your/nav2_params.yaml

環境プリセットを使用する場合：

ros2 launch fusioncore_ros fusioncore_nav2.launch.py \
  fusioncore_config:=/path/to/your/fusioncore.yaml \
  nav2_params:=/path/to/your/nav2_params.yaml \
  env_config:=$(ros2 pkg prefix fusioncore_ros)/share/fusioncore_ros/config/env_urban.yaml

ローンチファイルは、2 秒後に FusionCore を構成し、

configure → inactive

odom → base_link

これで完了です。 追加ノード、座標変換、リマップは不要です。FusionCore の

odom → base_link

fromLL

GPS ウェイポイントナビゲーションのために (nav2_waypoint_follower with fromLL)

# FusionCore は GPS 固定値を取得した時点で fromLL サービスを公開
ros2 service call /fromLL fusioncore_ros/srv/FromLL \
  "{ll_point: {latitude: 43.2557, longitude: -79.8711, altitude: 0.0}}"

アーキテクチャ

fusioncore/
├── fusioncore_core/              # 純粋な C++17 数学ライブラリ。ROS 依存性なし。
│   ├── include/fusioncore/
│   │   ├── ukf.hpp               # 非縮退カルマンフィルタ：45 シグマポイント
│   │   ├── state.hpp             # 22 次元状態ベクトル (クォータニオンオリエンテーション)
│   │   ├── fusioncore.hpp        # パブリック API: FusionCore, FusionCoreConfig
│   │   └── sensors/
│   │       ├── imu.hpp           # 生 IMU + オリエント測量モデル
│   │       ├── encoder.hpp       # ホイールエンコーダー測量モデル
│   │       └── gnss.hpp          # GPS: ECEF, レバーアーム, 共分散, 品質ゲートリング
│   └── src/
│       ├── ukf.cpp               # UKF: シグマポイント、予測、更新、測量予測
│       └── fusioncore.cpp        # マネージャー：異常値拒否、適応ノイズ、
│                                 #          スナップショット、可観測性、遅延補償
├── fusioncore_ros/               # ROS 2 Jazzy ラッパー
│   ├── src/fusion_node.cpp       # ライフェサイクルノード：全センサーコールバック、TF 検証、
│   │                             #   ZUPT, 診断、/fusion/pose, リセットサービス
│   ├── config/fusioncore.yaml    # デフォルト構成
│   ├── config/duatic_mecanum.yaml
│   └── launch/fusioncore.launch.py
└── fusioncore_gazebo/             # シミュレーションワールド
    ├── worlds/fusioncore_test.sdf
    ├── models/fusioncore_robot/
    ├── launch/fusioncore_gazebo.launch.py
    ├── launch/gz_pose_to_gps.py
    └── launch/integration_test.py

技術的な詳細

• フィルタ: 非縮退カルマンフィルタ (Unscented Kalman Filter), 45 シグマポイント
• 状態ベクトル: 22 次元：位置 (x,y,z), オリエンテーション (クォータニオン qw,qx,qy,qz), リニア速度, アングラリベロシティ, リニアアクセレーション, ジャイロバイアス (x,y,z), アクセルバイアス (x,y,z)
• GPS 座標系: PROJ を通じて構成可能：デフォルト ECEF (EPSG:4978, 世界的に有効); UTM ゾーンを含む任意の PROJ 互換入力 CRS をサポート
• バイアス推定: 連続的なオンライン推定、校正不要
• GPS 品質スケーリング: HDOP/VDOP によるノイズ共分散のスケーリング、または利用可能な場合フル 3x3 メッセージ共分散使用
• 異常値拒否: センサー次元ごとにカイ二平方ゲートリング（99.9 百分位数）でのマハラノビス適用
• 適応型ノイズ: スライディングウィンドウイノベーション跟踪、指数平滑平均 R 更新
• 遅延補償: IMU リングバッファリプレイによる逆時差推定 (retrodiction) up to 500ms
• ZUPT: スタンバイ時の自動ゼロ速度更新
• 出力レート: 100Hz
• 言語: C++17
• ライセンス: Apache 2.0

ステータス

機能している・テスト済み：

• ハードウェアテスト進行中：産業用メカヌームマニピュレータ (Duatic), 農業用 RTK ロボット (オンタリオ州南部)
• UKF コア:

  colcon test

  を通じて 39 のユニットテストが通過
• UKF 数値的安定性：P シメトリゼーション + アイデンティティシフト Cholesky リペア + アングラリベロシティ分散キャップ
• IMU + エンコーダー + GPS 融合
• 自動 IMU バイアス推定
• ECEF GPS 変換と品質に応じたノイズスケーリング
• デュアンテナ方位角：

  sensor_msgs/Imu

  と

  compass_msgs/Azimuth

  の両方対応
• TF を経由した IMU フレーム変換
• 起動時の TF 検証（正確な修正コマンド付き）
• GPS レバーアームと可観測性ガード：主および次点レシーバー用の独立パラメータ
• フル 3x3 GPS 共分散サポート
• ホイールオドメトリ共分散サポート
• 複数 GPS レシーバー対応
• 方位角可観測性跟踪：DUAL_ANTENNA / IMU_ORIENTATION / GPS_TRACK
• GPS 固定品質ゲートリング：構成可能な

  gnss.min_fix_type

  (GPS / DGPS / RTK_FIXED)
• マハラノビス異常値拒否：テストで GPS ジャンプが拒否されたことが確認
• 適応型ノイズ共分散：イノベーション履歴からの自動 R 推定
• GPS 遅延補償：フル IMU リプレイによる逆時差推定 up to 500ms
• 非ホロノミック地上制約：ホイールロボット用の VZ=0 疑似測定値
• ゼロ速度更新 (ZUPT): エンコーダ速度 < 0.05 m/s の時の自動アクティブ
• センサー単位の診断：1Hz の

  /diagnostics

  で異常値カウントと方位角ステータス付き

• /fusion/pose

  : Nav2 / AMCL / slam_toolbox 用の PoseWithCovarianceStamped
• フィルタリセットサービス：

  ~/reset

  がフィルタと GPS リファレンスをクリアし、ノード再起動不要
• センサードロップ検出：センサー単位のステールネス跟踪（SensorHealth enum 経由）
• PROJ CRS 座標変換：PROJ ライブラリを介して入力/出力 CRS を構成可能 (WGS84, UTM, ECEF, 任意の EPSG コード)
• ROS 2 Jazzy ライフェサイクルノード 100Hz
• Gazebo Harmonic シミュレーションワールド

既知の制限：

• GNSS アンテナレバーアームは固定値として知られており、データから推定されない
• Gazebo シミュレーションでは、Gazebo 物理による残留 y 軸ドリフト (~0.3m) が発生することがある：フィルタエラーではない
• メカヌームドライブの側方速度は運動モデルで予測されない

ロードマップ：

• アッケルマンおよび全方向ステアリング運動モデル
• メカヌームドライブ運動モデル
• TF header.frame_id から GNSS レバーアームを自動推導

ライセンス

Apache 2.0。BSD-3 が提供していない明確な特許ライセンス授与を含む。商業的に安全です。

サポート

イシューは 24 時間以内に回答します。GitHub イシューを開くか、ROS Discourse の元ディスカッションを見つけてください。

このプロジェクトは、2024 年 12 月のコミュニティスレッドで

robot_localization