AHRS

fibril_sim/ahrs は MuJoCo の orientation、gyroscope、accelerometer sensor を入力として、内部処理済み IMU/AHRS の出力をブラックボックスとして近似する Device です。 sample type ID は fibril_sim/ahrs_sample/v1 です。

fibril_sim/imu が比較的 raw な IMU を表すのに対し、fibril_sim/ahrs は内部補正やフィルタを持つセンサを対象にします。メーカー固有の Kalman filter を再実装せず、観測可能な出力ダイナミクスと残差誤差をパラメータ化します。

MuJoCo 側の要件

orientation sensor、gyroscope sensor、accelerometer sensor はすべて必須です。 orientation sensor は四次元の frame quaternion、gyroscope と accelerometer は三次元の対応する sensor type である必要があります。

最小設定

components:
  - id: ahrs
    type: fibril_sim/ahrs
    config:
      orientation_sensor: ahrs_quat
      gyroscope_sensor: ahrs_gyro
      accelerometer_sensor: ahrs_accel
      period_s: 0.01
      yaw_reference: initial

R6093U は specification/driver contract を device profile、複数の実機録画で再現した stochastic residual を empirical profile として選択できます。実測元が特定 robot project であっても、device family に閉じた residual は generic profile として保持し、installed-unit 固有差分だけを consuming project に残します。

R6093U本体は package://fibril_sim_models/mujoco/sensors/r6093u.xml のMJCF moduleとして再利用できます。 prefix="imu_" でattachすると imu_site、imu_sensor_quat、imu_sensor_gyro、imu_sensor_accel が生成されるため、visualとMuJoCo sensor endpointをrobotごとに書き直す必要がありません。 Component profileはこのmoduleとは独立して、acquisition cadenceやoutput semanticsだけを設定します。

components:
  - id: ahrs
    type: fibril_sim/ahrs
    config:
      profiles:
        device: r6093u_nominal
        empirical: r6093u_empirical
      orientation_sensor: ahrs_quat
      gyroscope_sensor: ahrs_gyro
      accelerometer_sensor: ahrs_accel

r6093u_nominal は acquisition cadence、bandwidth、座標規約、limit、resolution を提供します。 r6093u_empirical は複数の実機録画で training/held-out の再現性を確認した zero-mean stochastic residual を提供します。 fixed bias、scale、mounting correction のような installed-unit calibration は含みません。測定 provenance は fibril_sim_models/profiles/evidence/r6093u_empirical.md に置きます。

Configuration の項目

vector parameter は [x, y, z] の三要素で指定します。未同定の誤差項は既定値のまま無効化できます。

Field	Type	Required	Default	Meaning
`profiles.device`	string	no	none	`r6093u_nominal`
`profiles.empirical`	string	no	none	`r6093u_empirical` または consuming project が登録する installed-unit empirical profile
`orientation_sensor`	string	yes		MuJoCo frame quaternion sensor
`gyroscope_sensor`	string	yes		MuJoCo gyro sensor
`accelerometer_sensor`	string	yes		MuJoCo accelerometer sensor
`period_s`	double	profile 未指定時 yes	profile	acquisition period
`phase_s`	double	no	`0.0`	acquisition phase
`latency_s`	double	no	`0.0`	delivery latency
`jitter_s`	double	no	`0.0`	delivery jitter standard deviation
`loss_probability`	double	no	`0.0`	packet loss probability
`quantization`	string	no	`next_boundary`	schedule quantization
`yaw_reference`	string	no	`initial`	`initial` or `world`
`orientation_bandwidth_hz`	double	no	`0.0`	first-order effective orientation bandwidth; `0` disables filtering
`angular_velocity_bandwidth_hz`	double	no	`0.0`	first-order effective angular velocity bandwidth; `0` disables filtering
`linear_acceleration_bandwidth_hz`	double	no	`0.0`	first-order effective acceleration bandwidth; `0` disables filtering
`angular_velocity_scale`	double[3]	no	`[1, 1, 1]`	per-axis multiplicative scale
`linear_acceleration_scale`	double[3]	no	`[1, 1, 1]`	per-axis multiplicative scale
`angular_velocity_bias`	double[3]	no	`[0, 0, 0]`	post-dynamics residual bias in rad/s
`linear_acceleration_bias`	double[3]	no	`[0, 0, 0]`	post-dynamics residual bias in m/s²
`orientation_noise_stddev_rad`	double[3]	no	`[0, 0, 0]`	local roll/pitch/yaw residual noise stationary standard deviation
`angular_velocity_noise_stddev`	double[3]	no	`[0, 0, 0]`	post-dynamics residual Gaussian noise
`linear_acceleration_noise_stddev`	double[3]	no	`[0, 0, 0]`	post-dynamics residual Gaussian noise
`orientation_noise_correlation`	double[3]	no	`[0, 0, 0]`	per-axis AR(1) orientation residual correlation coefficient
`angular_velocity_noise_correlation`	double[3]	no	`[0, 0, 0]`	per-axis AR(1) residual-noise correlation coefficient
`linear_acceleration_noise_correlation`	double[3]	no	`[0, 0, 0]`	per-axis AR(1) residual-noise correlation coefficient
`orientation_yaw_sign`	double	no	`1.0`	MuJoCo yawからdevice orientation yawへの符号変換。`-1.0`または`1.0`のみ
`yaw_rate_bias_stddev_rad_s`	double	no	`0.0`	episodeごとにsampleし、gyro-Zとorientation yaw積分で共有するzero-mean residual rate biasの標準偏差
`orientation_resolution_rad`	double	no	`0.0`	local roll/pitch/yaw output quantization step
`angular_velocity_limit_rad_s`	double	no	`0.0`	symmetric output saturation; `0` disables it
`linear_acceleration_limit_m_s2`	double	no	`0.0`	symmetric output saturation; `0` disables it
`angular_velocity_resolution`	double	no	`0.0`	angular velocity quantization step
`linear_acceleration_resolution`	double	no	`0.0`	acceleration quantization step

bandwidth、limit、noise standard deviation、resolution は非負である必要があります。 scale は各軸で正値である必要があります。 noise correlation は絶対値が 1 未満である必要があります。 orientation_yaw_sign は -1.0 または 1.0 である必要があります。

出力モデル

angular velocity と acceleration は、まず scale を適用し、その後 first-order low-pass で effective output dynamics を近似します。 bandwidth を f_c、acquisition period を dt とすると、更新係数は次です。

1 2	`alpha = 1 - exp(-2pif_cdt) y[k] = y[k-1] + alpha (x[k] - y[k-1])`

filter state は最初の acquisition で現在値へ初期化するため、turn-on transient は生成しません。

その後、残差 bias と残差 Gaussian noise を加え、saturation と resolution を適用します。 noise は episode seed、Component ID、誤差源名から作る独立 named RNG stream を使用します。

noise correlation を rho、対応する noise standard deviation を定常標準偏差 sigma とすると、残差 noise は次の AR(1) process です。

1 2	`n[0] = sigma * N(0, 1) n[k] = rho * n[k-1] + sigma * sqrt(1-rho^2) * N(0, 1)`

rho = 0 は i.i.d. Gaussian noise と一致します。初回から定常分散で初期化するため、episode 開始時だけ noise variance が小さくなる transient は生成しません。

orientation residual は local roll/pitch/yaw の小角誤差として同じ AR(1) process を生成し、その誤差 quaternion を filtered orientation に右から合成します。 orientation_yaw_sign はMuJoCo frame quaternionをroll/pitch/yawへ変換した直後のyawに適用します。この変換はdevice orientationの座標規約だけを表し、gyroとaccelerationの軸や符号は変更しません。 yaw_rate_bias_stddev_rad_s が正の場合は、episode開始時に独立named RNGからconstant residual yaw-rate biasを1回sampleします。同じbiasをangular velocityのZ出力へ加え、orientation yawにも時間積分するため、processed AHRSのrate出力とangle driftの整合性を維持します。最初のsampleではdrift angleを0から開始するため、warm-upやalignment transientは生成しません。 orientation_resolution_rad が正の場合は、誤差合成後の roll/pitch/yaw を量子化してから quaternion に戻します。固定 orientation bias は別途持たず、既知姿勢による calibration がない profile ではゼロ平均の残差だけを再現します。

orientation は yaw_reference: initial の場合、最初の acquisition の yaw をゼロ基準として roll/pitch を維持します。 orientation_bandwidth_hz を指定した場合は quaternion を正規化しながら first-order に追従させます。

R6093U profile の指定

r6093u_nominal は specification と driver contract から再利用できる device-level parameter を提供します。 r6093u_empirical は device-family residual として再利用できる noise、correlation、episode-level yaw-rate drift dispersion を提供します。 installed-unit 固有の fixed bias、scale、mounting correction は consuming project の profile または calibration artifact に置きます。