diff --git a/app/autonomy/LocalizationMeasurements.js b/app/autonomy/LocalizationMeasurements.js index 7a46c6d..0a30fa0 100644 --- a/app/autonomy/LocalizationMeasurements.js +++ b/app/autonomy/LocalizationMeasurements.js @@ -33,6 +33,17 @@ export function gaussianSample(rng) { return Math.sqrt(-2 * Math.log(left)) * Math.cos(2 * Math.PI * rng.next()); } +/** + * Per-sample standard deviation, on every axis, for white noise given as a + * datasheet noise density (units/√Hz): sigma = density / √dt. Without a + * density, returns `perSample` unchanged. + */ +export function noiseStdDevFromDensity(density, perSample, dtSeconds) { + if (density == null) return perSample; + const sigma = Math.max(0, Number(density)) / Math.sqrt(Math.max(Number.EPSILON, dtSeconds)); + return { x: sigma, y: sigma, z: sigma }; +} + export function sampleAxisNoise(rng, stdDev, bias = { x: 0, y: 0, z: 0 }, drift = { x: 0, y: 0, z: 0 }) { const perturb = (axis) => (Number(bias[axis] || 0) + Number(drift[axis] || 0)) + gaussianSample(rng) * Number(stdDev[axis] || 0); @@ -252,15 +263,25 @@ export function buildImuMeasurement(snapshot, sensorConfig, rng, state = {}) { ), }; + const angularStdDev = noiseStdDevFromDensity( + noise.angularVelocityNoiseDensity ?? sensorConfig.calibration.angularVelocityNoiseDensity, + noise.angularVelocityStdDev || sensorConfig.calibration.angularVelocityStdDev || { x: 0, y: 0, z: 0 }, + dt, + ); + const accelerationStdDev = noiseStdDevFromDensity( + noise.linearAccelerationNoiseDensity ?? sensorConfig.calibration.linearAccelerationNoiseDensity, + noise.linearAccelerationStdDev || sensorConfig.calibration.linearAccelerationStdDev || { x: 0, y: 0, z: 0 }, + dt, + ); const angularNoise = sampleAxisNoise( rng, - noise.angularVelocityStdDev || sensorConfig.calibration.angularVelocityStdDev || { x: 0, y: 0, z: 0 }, + angularStdDev, turnOnBias.angular || turnOnBias, nextDrift.angular, ); const accelerationNoise = sampleAxisNoise( rng, - noise.linearAccelerationStdDev || sensorConfig.calibration.linearAccelerationStdDev || { x: 0, y: 0, z: 0 }, + accelerationStdDev, turnOnBias.acceleration || { x: 0, y: 0, z: 0 }, nextDrift.acceleration, ); @@ -281,8 +302,8 @@ export function buildImuMeasurement(snapshot, sensorConfig, rng, state = {}) { angularVelocity: measuredAngular, linearAcceleration: measuredAcceleration, orientationCovariance: unavailableOrientationCovariance(), - angularVelocityCovariance: diagonalCovariance(noise.angularVelocityStdDev || sensorConfig.calibration.angularVelocityStdDev || { x: 0, y: 0, z: 0 }), - linearAccelerationCovariance: diagonalCovariance(noise.linearAccelerationStdDev || sensorConfig.calibration.linearAccelerationStdDev || { x: 0, y: 0, z: 0 }), + angularVelocityCovariance: diagonalCovariance(angularStdDev), + linearAccelerationCovariance: diagonalCovariance(accelerationStdDev), }, nextState: { turnOnBias, diff --git a/app/simulation/sensors/SensorTypeRegistry.js b/app/simulation/sensors/SensorTypeRegistry.js index 10162f0..64ff1b0 100644 --- a/app/simulation/sensors/SensorTypeRegistry.js +++ b/app/simulation/sensors/SensorTypeRegistry.js @@ -159,8 +159,10 @@ function axisCalibration(source = {}, fallback = 0) { const imuRunFields = [ { label: "Gravity (m/s²)", path: ["calibration", "gravity"], control: "number", min: 0, step: 0.001 }, { label: "IMU topic ID", path: ["outputs", "imuTopicId"], control: "text", advanced: true }, - { label: "Angular velocity σ (rad/s)", path: ["calibration", "angularVelocityStdDev", "x"], control: "number", min: 0, step: 0.0001, advanced: true }, - { label: "Linear acceleration σ (m/s²)", path: ["calibration", "linearAccelerationStdDev", "z"], control: "number", min: 0, step: 0.001, advanced: true }, + { label: "Gyro noise density (rad/s/√Hz)", path: ["calibration", "angularVelocityNoiseDensity"], control: "number", min: 0, step: 0.00001, advanced: true }, + { label: "Accelerometer noise density (m/s²/√Hz)", path: ["calibration", "linearAccelerationNoiseDensity"], control: "number", min: 0, step: 0.0001, advanced: true }, + { label: "Angular velocity σ per sample (rad/s), without density", path: ["calibration", "angularVelocityStdDev", "x"], control: "number", min: 0, step: 0.0001, advanced: true }, + { label: "Linear acceleration σ per sample (m/s²), without density", path: ["calibration", "linearAccelerationStdDev", "z"], control: "number", min: 0, step: 0.001, advanced: true }, { label: "Angular saturation (rad/s)", path: ["calibration", "angularVelocitySaturation"], control: "number", min: 0, advanced: true }, { label: "Acceleration saturation (m/s²)", path: ["calibration", "linearAccelerationSaturation"], control: "number", min: 0, advanced: true }, ]; diff --git a/docs/run-manifests.md b/docs/run-manifests.md index a8c36ae..b6d2cf8 100644 --- a/docs/run-manifests.md +++ b/docs/run-manifests.md @@ -356,10 +356,28 @@ appearance/analytic readback. | Sensor | Rate | Key calibration | Units | | --- | --- | --- | --- | +| IMU | 100 Hz | `gravity`, `angularVelocityNoiseDensity`, `linearAccelerationNoiseDensity` (or per-sample `angularVelocityStdDev`, `linearAccelerationStdDev`), drift τ, saturation | rad/s/√Hz, m/s²/√Hz (rad/s, m/s² per sample) | +| GNSS | 10 Hz | `datum` (lat/lng/alt), `positionNoiseEnu`, `faults.dropoutProbability`, `faults.outageProbability`, multipath τ | deg, m | | IMU | 100 Hz | `gravity`, per-axis `angularVelocityStdDev`, `linearAccelerationStdDev`, drift τ, saturation | rad/s, m/s² | | GNSS | 10 Hz | `datum` (lat/lng/alt), `positionNoiseEnu`, `faults.dropoutProbability`, `faults.outageProbability`, `faults.outageDurationSec` (0 or absent: outage lasts to the end of the run), multipath τ | deg, m | | Wheel odometry | 50 Hz | `wheelRadius`, `ticksPerRevolution`, `trackWidth`, `slipFactor`, pose/twist noise | m, ticks/rev | +IMU white noise is set from datasheet noise densities. Each sample gets +σ = density / √Δt, where Δt is the time since the previous capture. Raising +the rate then raises per-sample noise, and noise averaged over a fixed time +stays the same, as on a real sensor. Each density is one number for all +three axes. Without a density, the IMU uses the per-sample +`angularVelocityStdDev` or `linearAccelerationStdDev`, which does not scale +with rate. Default runs set no density. The per-sample defaults +(0.002 rad/s, 0.02 m/s²) equal 0.0002 rad/s/√Hz and 0.002 m/s²/√Hz at 100 Hz. + +Datasheet conversions: + +- gyro °/s/√Hz × π/180 = rad/s/√Hz +- gyro angle random walk °/√h × π/180 / 60 = rad/s/√Hz +- accelerometer µg/√Hz × 9.80665e-6 = m/s²/√Hz +- accelerometer velocity random walk m/s/√h ÷ 60 = m/s²/√Hz + GNSS treats map/odom position as ENU offset from the manifest datum. Wheel odometry integrates an independent measured `odom → base_link` estimate; oracle truth on `/oracle/vehicle/odometry` remains isolated for scoring. ## Headless profiles, spaces, and sensor backends