Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 25 additions & 4 deletions app/autonomy/LocalizationMeasurements.js
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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,
);
Expand All @@ -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,
Expand Down
6 changes: 4 additions & 2 deletions app/simulation/sensors/SensorTypeRegistry.js
Original file line number Diff line number Diff line change
Expand Up @@ -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 },
];
Expand Down
18 changes: 18 additions & 0 deletions docs/run-manifests.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading