diff --git a/app/autonomy/LocalizationMeasurements.js b/app/autonomy/LocalizationMeasurements.js index d5156419..d2469986 100644 --- a/app/autonomy/LocalizationMeasurements.js +++ b/app/autonomy/LocalizationMeasurements.js @@ -1,3 +1,4 @@ +import { Vector3 } from "three"; import { composeRep103Poses, eulerToQuaternion, @@ -127,6 +128,7 @@ export function captureVehicleSnapshot(vehicle, captureTimeNs, previous = null, const bodyAngularVelocity = { x: 0, y: 0, z: yawRate }; let bodyAcceleration = { x: 0, y: 0, z: 0 }; + let bodyAngularAcceleration = { x: 0, y: 0, z: 0 }; if (previous?.captureTimeNs != null && captureTimeNs > previous.captureTimeNs) { const dt = (captureTimeNs - previous.captureTimeNs) / 1e9; if (dt > 0) { @@ -135,6 +137,11 @@ export function captureVehicleSnapshot(vehicle, captureTimeNs, previous = null, y: (bodyVelocity.y - previous.bodyVelocity.y) / dt, z: (bodyVelocity.z - previous.bodyVelocity.z) / dt, }; + bodyAngularAcceleration = { + x: 0, + y: 0, + z: (yawRate - Number(previous.bodyAngularVelocity?.z || 0)) / dt, + }; } } // The body frame rotates with the vehicle, so the derivative of body-frame @@ -149,6 +156,7 @@ export function captureVehicleSnapshot(vehicle, captureTimeNs, previous = null, orientation, bodyVelocity, bodyAngularVelocity, + bodyAngularAcceleration, bodyAcceleration, wheelbase, steeringAngle, @@ -156,6 +164,20 @@ export function captureVehicleSnapshot(vehicle, captureTimeNs, previous = null, }; } +/** + * Body-frame acceleration of a point at `offset` from base_link on a rigid body: + * a + alpha x r + omega x (omega x r). + */ +export function accelerationAtOffset(snapshot, offset) { + const r = new Vector3().copy(vec3(offset)); + const omega = new Vector3().copy(snapshot.bodyAngularVelocity || { x: 0, y: 0, z: 0 }); + const alpha = new Vector3().copy(snapshot.bodyAngularAcceleration || { x: 0, y: 0, z: 0 }); + const tangential = new Vector3().crossVectors(alpha, r); + const centripetal = new Vector3().crossVectors(omega, new Vector3().crossVectors(omega, r)); + const total = new Vector3().copy(snapshot.bodyAcceleration).add(tangential).add(centripetal); + return { x: total.x, y: total.y, z: total.z }; +} + export function sensorFrameRotation(sensorPose = {}) { const mountPose = { position: vec3(sensorPose.position), @@ -196,7 +218,7 @@ export function buildImuMeasurement(snapshot, sensorConfig, rng, state = {}) { const sensorRotation = sensorFrameRotation(sensorConfig.pose); const angularVelocity = vectorToSensorFrame(snapshot.bodyAngularVelocity, sensorRotation); const specificForce = specificForceRep103( - snapshot.bodyAcceleration, + accelerationAtOffset(snapshot, sensorConfig.pose?.position), sensorRotation, sensorConfig.calibration.gravity, snapshot.orientation,