DriveWorks SDK Reference
3.0.4260 Release
For Test and Development only

Egomotion Interface

Detailed Description

Provides vehicle egomotion functionality.

The egomotion module provides implementations of motion models with different sensor modalities. Starting from a simple Ackerman-based odometry-only model, to a full-fledged fusion of inertial sensor information.

This module provides access to a history of motion estimates with a predefined cadence and length. At any point of time an access into the history can be made to retrieve previous estimates. The access into the history is timestamp-based. If an access falls between two history entries it will be interpolated.

In addition to history-based access, all motion models support prediction of the motion into the future.

Note
SW Release Applicability: These APIs are available in both NVIDIA DriveWorks and NVIDIA DRIVE Software releases.

Data Structures

struct  dwEgomotionLinearAccelerationFilterParams
 Defines egomotion linear acceleration filter parameters. More...
 
struct  dwEgomotionParameters
 Holds initialization parameters for the Egomotion module. More...
 
struct  dwEgomotionRelativeUncertainty
 Holds egomotion uncertainty estimates for a relative motion estimate. More...
 
struct  dwEgomotionResult
 Holds egomotion state estimate. More...
 
struct  dwEgomotionSensorCharacteristics
 Sensor measurement noise characteristics. More...
 
struct  dwEgomotionSuspensionParameters
 Suspension model type and parameters. More...
 
struct  dwEgomotionUncertainty
 Holds egomotion uncertainty estimates. More...
 

Modules

 Global Egomotion Interface
 Provides global location and orientation estimation functionality.
 
 RadarDopplerMotion Interface
 Provides estimation of the speed and heading of the radar observing doppler based radar measurements.
 

Typedefs

typedef struct dwEgomotionObject const * dwEgomotionConstHandle_t
 
typedef struct dwEgomotionObject * dwEgomotionHandle_t
 

Enumerations

enum  dwEgomotionDataField {
  DW_EGOMOTION_ROTATION = 1 << 0,
  DW_EGOMOTION_LIN_VEL_X = 1 << 1,
  DW_EGOMOTION_LIN_VEL_Y = 1 << 2,
  DW_EGOMOTION_LIN_VEL_Z = 1 << 3,
  DW_EGOMOTION_ANG_VEL_X = 1 << 4,
  DW_EGOMOTION_ANG_VEL_Y = 1 << 5,
  DW_EGOMOTION_ANG_VEL_Z = 1 << 6,
  DW_EGOMOTION_LIN_ACC_X = 1 << 7,
  DW_EGOMOTION_LIN_ACC_Y = 1 << 8,
  DW_EGOMOTION_LIN_ACC_Z = 1 << 9
}
 Defines flags that indicate validity of corresponding data in dwEgomotionResult and dwEgomotionUncertainty. More...
 
enum  dwEgomotionLinearAccelerationFilterMode {
  DW_EGOMOTION_ACC_FILTER_NO_FILTERING,
  DW_EGOMOTION_ACC_FILTER_SIMPLE
}
 Defines egomotion linear acceleration filter mode. More...
 
enum  dwEgomotionSpeedMeasurementType {
  DW_EGOMOTION_FRONT_SPEED = 0,
  DW_EGOMOTION_REAR_WHEEL_SPEED = 1
}
 Defines speed measurement types. More...
 
enum  dwEgomotionSuspensionModel {
  DW_EGOMOTION_SUSPENSION_RIGID_MODEL = 0,
  DW_EGOMOTION_SUSPENSION_TORSIONAL_SPRING_MODEL = 1
}
 Defines egomotion suspension model. More...
 
enum  dwMotionModel {
  DW_EGOMOTION_ODOMETRY = 1 << 0,
  DW_EGOMOTION_IMU_ODOMETRY = 1 << 1 | DW_EGOMOTION_ODOMETRY
}
 Defines the motion models. More...
 
enum  dwMotionModelMeasurement {
  DW_EGOMOTION_MEASURMENT_VELOCITY = 0,
  DW_EGOMOTION_MEASURMENT_STEERINGANGLE = 1,
  DW_EGOMOTION_MEASURMENT_STEERINGWHEELANGLE = 2
}
 Defines motion measurements. More...
 

Functions

DW_API_PUBLIC dwStatus dwEgomotion_addIMUMeasurement (const dwIMUFrame *imu, dwEgomotionHandle_t obj)
 Adds an IMU frame to the egomotion module. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_addOdometry (dwMotionModelMeasurement measuredType, float32_t measuredValue, dwTime_t timestamp, dwEgomotionHandle_t obj)
 Notifies the egomotion module of a new odometry measurement. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_addVehicleState (const dwVehicleIOState *state, dwEgomotionHandle_t obj)
 Notifies the egomotion module of a changed vehicle state. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_applyRelativeTransformation (dwTransformation3f *newVehicle2World, const dwTransformation3f *poseOld2New, const dwTransformation3f *oldVehicle2World)
 Applies the estimated relative motion as returned by dwEgomotion_computeRelativeTransformation() to a given vehicle pose. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_computeRelativeTransformation (dwTransformation3f *poseAtoB, dwEgomotionRelativeUncertainty *uncertainty, dwTime_t timestamp_a, dwTime_t timestamp_b, dwEgomotionConstHandle_t obj)
 Computes the relative transformation between two timestamps and the uncertainty of this transform. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_computeSteeringAngleFromIMU (float32_t *steeringAngle, float32_t *inverseSteeringR, const dwIMUFrame *imuMeasurement, dwEgomotionConstHandle_t obj)
 Computes steering angle of the vehicle based on IMU measurement. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_estimate (dwEgomotionResult *pose, dwEgomotionUncertainty *uncertainty, dwTime_t timestamp_us, dwEgomotionConstHandle_t obj)
 Estimates the state for a given timestamp. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_getEstimation (dwEgomotionResult *result, dwEgomotionConstHandle_t obj)
 Gets the latest state estimate. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_getEstimationTimestamp (dwTime_t *timestamp, dwEgomotionConstHandle_t obj)
 Gets the timestamp of the latest state estimate. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_getHistoryElement (const dwEgomotionResult **pose, const dwEgomotionUncertainty **uncertainty, size_t index, dwEgomotionConstHandle_t obj)
 Returns an element from the motion history that is currently available. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_getHistorySize (size_t *num, dwEgomotionConstHandle_t obj)
 Returns the number of elements currently stored in the history. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_getMotionModel (dwMotionModel *model, dwEgomotionConstHandle_t obj)
 Returns the type of the motion model used. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_getUncertainty (dwEgomotionUncertainty *result, dwEgomotionConstHandle_t obj)
 Gets the latest state estimate uncertainties. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_hasEstimation (bool *result, dwEgomotionConstHandle_t obj)
 Check whether has state estimate. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_initialize (dwEgomotionHandle_t *obj, const dwEgomotionParameters *params, dwContextHandle_t ctx)
 Initializes the egomotion module. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_initParamsFromRig (dwEgomotionParameters *params, dwConstRigHandle_t rigConfiguration, const char *imuSensorName, const char *canSensorName)
 Initialize egomotion parameters from a provided RigConfiguration. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_initParamsFromRigByIndex (dwEgomotionParameters *params, dwConstRigHandle_t rigConfiguration, uint32_t imuSensorIdx, uint32_t canSensorIdx)
 Same as dwEgomotion_initParamsFromRig however uses sensor indices in rigConfiguration instead of their names. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_release (dwEgomotionHandle_t obj)
 Releases the egomotion module. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_reset (dwEgomotionHandle_t obj)
 Resets the state estimate and all history of the egomotion module. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_steeringAngleToSteeringWheelAngle (float32_t *steeringWheelAngle, float32_t steeringAngle, dwEgomotionHandle_t obj)
 Convert steering angle to steering wheel angle. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_steeringWheelAngleToSteeringAngle (float32_t *steeringAngle, float32_t steeringWheelAngle, dwEgomotionHandle_t obj)
 Convert steering wheel angle to steering angle. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_update (dwTime_t timestamp_us, dwEgomotionHandle_t obj)
 Runs the motion model estimation for a given timestamp. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_updateIMUExtrinsics (const dwTransformation3f *imuToRig, dwEgomotionHandle_t obj)
 This method updates the IMU extrinsics to convert from the IMU coordinate system to the vehicle rig coordinate system. More...
 
DW_API_PUBLIC dwStatus dwEgomotion_updateVehicle (const dwVehicle *vehicle, dwEgomotionHandle_t obj)
 This method updates the egomotion module with an updated vehicle. More...
 

Data Structure Documentation

◆ dwEgomotionLinearAccelerationFilterParams

struct dwEgomotionLinearAccelerationFilterParams
Data Fields
float32_t accelerationFilterTimeConst Time constant of the IMU acceleration measurements.
float32_t measurementNoiseStdevAcceleration Standard deviation of measurement noise in acceleration [m/s^2].
float32_t measurementNoiseStdevSpeed Standard deviation of measurement noise in speed [m/s].
dwEgomotionLinearAccelerationFilterMode mode Linear acceleration filter mode. Default (0): no filtering.
float32_t processNoiseStdevAcceleration Square root of continuous time process noise covariance in acceleration [m/s^2 * 1/sqrt(s)].
float32_t processNoiseStdevSpeed Square root of continuous time process noise covariance in speed [m/s * 1/sqrt(s)].

◆ dwEgomotionParameters

struct dwEgomotionParameters
Data Fields
bool automaticUpdate Automatically update state estimation.

In general to update motion estimation, a call to dwEgomotion_update() is required. When automaticUpdate is set, the motion estimation update is triggered by the addition of new sensor measurements. The exact update timestamp is dependent on the sensor type and motion model implementation. The downside of this method is that motion estimates requested for timestamps that do not correspond to the timestamps at which automatic update happened, might potentially be less accurate as interpolation or extrapolation to the requested timestamps is needed.

Note
when the automatic update is active, dwEgomotion_update() will not update the filter state and throw a DW_NOT_SUPPORTED exception instead.
bool estimateInitialOrientation When enabled, initial rotation will be estimated from accelerometer measurements.

When disabled, initial rotation is assumed to be identity, i.e. vehicle standing on flat, horizontal ground.

Note
only available for DW_EGOMOTION_IMU_ODOMETRY motion model. Ignored when DW_EGOMOTION_ODOMETRY is used.
float32_t gyroscopeBias[3] Initial gyroscope biases, if known at initialization time.

Gyroscope biases are estimated internally at run-time, however it can be beneficial if the filter is initialized with already known biases. If unknown, leave biases zero-initialized.

uint32_t historySize Number of state estimates to keep in the history (if 0 specified default of 1000 is used).

Any call to dwEgomotion_update(), or automatic update, adds an estimate into the history.

dwTransformation3f imu2rig IMU extrinsics.

Transformation from the IMU coordinate system to the vehicle rig coordinate system.

Note
the quality of the estimated motion is highly depended on the accuracy of the extrinsics.
float32_t lateralSlipCoefficient Lateral slip coefficient [rad*s^2/m].

Used in linear function mapping lateral acceleration [m/s^2] to slip angle [rad], such that slipAngle = lateralSlipCoefficient * lateralAcceleration. If 0, default slip coefficient of -2.83e-3 [rad*s^2/m] is used.

Note
only available for DW_EGOMOTION_IMU_ODOMETRY motion model. Ignored when DW_EGOMOTION_ODOMETRY is used.
dwEgomotionLinearAccelerationFilterParams linearAccelerationFilterParameters Linear acceleration filter parameters.
Note
only available for DW_EGOMOTION_IMU_ODOMETRY motion model. Ignored when other motion models are used.
dwMotionModel motionModel Specifies the motion model to be used for pose estimation.
dwEgomotionSensorCharacteristics sensorParameters Sensor parameters, containing information about sensor characteristics.

If the struct is zero initialized, default assumptions about sensor parameters are made.

See also
dwEgomotionSensorCharacteristics
dwEgomotionSpeedMeasurementType speedMeasurementType Defines which velocity readings from dwVehicleIOState shall be used for egomotion estimation.
dwEgomotionSuspensionParameters suspension Suspension model parameters.

The model is used internally to compensate for vehicle body rotation due to acceleration and resulting rotational suspension effects. If the struct is zero initialized, suspension will not be modeled and accounted for.

Note
only available for DW_EGOMOTION_IMU_ODOMETRY motion model. Ignored when other motion models are used.
dwVehicle vehicle Vehicle parameters to setup the model.
Note
The validity of the parameters will be verified at initialization time and an error will be returned back if vehicle parameters are found to be not plausible.

◆ dwEgomotionRelativeUncertainty

struct dwEgomotionRelativeUncertainty
Data Fields
dwMatrix3f rotation a 3x3 covariance of the rotation (order: roll, pitch, yaw) [rad]
dwTime_t timeInterval relative motion time interval [us]
dwMatrix3f translation a 3x3 covariance of the translation (x,y,z) [m]
bool valid indicates whether uncertainty estimates are valid or not

◆ dwEgomotionResult

struct dwEgomotionResult
Data Fields
float32_t angularVelocity[3] Rotation speed in body frame measured in [rad/s].
float32_t linearAcceleration[3] Linear acceleration measured in body frame in [m/s^2].
float32_t linearVelocity[3] Linear velocity in body frame measured in [m/s] at the origin.

For motion models capable of estimating slip angle imposed lateral velocity, y component will be populated.

Note
this represents instanteneous linear velocity
dwQuaternionf rotation Rotation represented as quaternion (x,y,z,w).

Valid when DW_EGOMOTION_ROTATION is set.

dwTime_t timestamp Timestamp of egomotion state estimate [us].
int32_t validFlags Bitwise combination of dwEgomotionDataField flags.

◆ dwEgomotionSensorCharacteristics

struct dwEgomotionSensorCharacteristics
Data Fields
float32_t accNoiseDensityMicroG Expected zero mean measurement noise of the linear accelerometer, also known as Noise Density [ug/sqrt(Hz)] A default value of 100 micro-g per sqrt(Hz) will be assumed if no parameter, i.e.

0 or nan, passed

float32_t gyroBiasRange If known this value in [rad/s] shall indicate standard deviation of the expected bias range of the gyroscope sensor.

Usually temperature controlled/calibrated gyroscopes vary around the mean by few tens of a radian. If 0 is given, it will be assumed the standard deviation around the bias mean is about +-0.05 [rad/s], ~ +- 3deg/s

float32_t gyroDriftRate Expected gyroscope drift rate in [deg/s].

A default value of 0.025 [deg/s] will be assumed if no parameter, i.e. 0 or nan, passed

float32_t gyroNoiseDensityDeg Expected zero mean measurement noise of the gyroscope, also known as Noise Density [deg/s/sqrt(Hz)] A default value of 0.015 [deg/s/sqrt(Hz)] will be assumed if no parameter, i.e.

0 or nan, passed

float32_t imuSamplingRateHz If known this entry shall indicate expected sampling rate in [Hz] of the IMU sensor.

A default value of 100Hz is used if no parameter passed

float32_t velocityFactor CAN velocity correction factor which is read from can properties in rig file.

When dwEgomotionParameters::speedMeasurementType == DW_EGOMOTION_FRONT_SPEED then received speed measurements are multiplied by this factor to obtain (approximately) true vehicle speed, e.g. due to non-nominal wheel diameters.

Note
A default value of 1 is assumed if no parameter is passed
dwTime_t velocityLatency CAN velocity latency in microseconds which is read from can properties in rig file.

◆ dwEgomotionSuspensionParameters

struct dwEgomotionSuspensionParameters
Data Fields
dwEgomotionSuspensionModel model Suspension model to use.

If left zero-intialized, a rigid suspension system is assumed (i.e. no suspension modeling).

float32_t torsionalSpringPitchDampingRatio Level of damping relative to critical damping around the pitch axis of the vehicle [dimensionless].

Typical passenger car suspension systems have a damping ratio in the range of [0.2, 0.6]. Default value is 0.6 if left zero-initialized.

float32_t torsionalSpringPitchNaturalFrequency Torsional spring model parameters.

Used if model == DW_EGOMOTION_SUSPENSION_TORSIONAL_SPRING_MODEL. If left zero-initizialized, a default set of parameters will be used.

These model parameters are suitable for a simple damped torsional spring model with external driving torque resulting from vehicle linear accelerations, described by the following ODE:

\[ //! I \ddot{\theta} + C \dot{\theta} + k \theta = \tau //! \]

where:

  • I vehicle inertia around y axis [kg m^2]
  • C angular damping constant [J s rad^-1]
  • k torsion spring constant [N m rad^-1]
  • \( \tau \) driving torque [N m], function of linear acceleration, vehicle mass and height of center of mass.
Note
if selected, this model requires accurate vehicle mass, inertia and height of center of mass in the dwVehicle struct provided as part of the dwEgomotionParameters.

Frequency at which the suspension system tends to oscillate around the pitch axis of the vehicle in the absence of an external driving force or damping [Hz]. Typical passenger car suspension systems have a natural frequency in the range of [0.5, 1.5] Hz. Default value is 1.25Hz if left zero-initialized.

◆ dwEgomotionUncertainty

struct dwEgomotionUncertainty
Data Fields
float32_t angularVelocity[3]
float32_t linearAcceleration[3]
float32_t linearVelocity[3]
dwMatrix3f rotation
dwTime_t timestamp Timestamp of egomotion uncertainty estimate [us].
int64_t validFlags

Typedef Documentation

◆ dwEgomotionConstHandle_t

typedef struct dwEgomotionObject const* dwEgomotionConstHandle_t

Definition at line 80 of file Egomotion.h.

◆ dwEgomotionHandle_t

typedef struct dwEgomotionObject* dwEgomotionHandle_t

Definition at line 79 of file Egomotion.h.

Enumeration Type Documentation

◆ dwEgomotionDataField

Defines flags that indicate validity of corresponding data in dwEgomotionResult and dwEgomotionUncertainty.

Enumerator
DW_EGOMOTION_ROTATION 

indicates validity of rotation,

See also
limitations of selected dwMotionModel
DW_EGOMOTION_LIN_VEL_X 

indicates validity of linearVelocity[0]

DW_EGOMOTION_LIN_VEL_Y 

indicates validity of linearVelocity[1]

DW_EGOMOTION_LIN_VEL_Z 

indicates validity of linearVelocity[2]

DW_EGOMOTION_ANG_VEL_X 

indicates validity of angularVelocity[0]

DW_EGOMOTION_ANG_VEL_Y 

indicates validity of angularVelocity[1]

DW_EGOMOTION_ANG_VEL_Z 

indicates validity of angularVelocity[2]

DW_EGOMOTION_LIN_ACC_X 

indicates validity of linearAcceleration[0]

DW_EGOMOTION_LIN_ACC_Y 

indicates validity of linearAcceleration[1]

DW_EGOMOTION_LIN_ACC_Z 

indicates validity of linearAcceleration[2]

Definition at line 392 of file Egomotion.h.

◆ dwEgomotionLinearAccelerationFilterMode

Defines egomotion linear acceleration filter mode.

Enumerator
DW_EGOMOTION_ACC_FILTER_NO_FILTERING 

no filtering of the output linear acceleration

DW_EGOMOTION_ACC_FILTER_SIMPLE 

simple low-pass filtering of the acceleration

Definition at line 213 of file Egomotion.h.

◆ dwEgomotionSpeedMeasurementType

Defines speed measurement types.

Enumerator
DW_EGOMOTION_FRONT_SPEED 

Indicates that speed is linear speed [m/s] measured at front wheels (along steering direction).

Steering angle [rad] is used internally to compute longitudinal speed.

  • Positive speed corresponds to a forward motion of the vehicle.
  • Positive steering angle corresponds to a left turn.
Note
: estimation quality is dependent on measurement accuracy, resolution and sampling rate. We recommend the following:
  • speed signal at >=50Hz sampling rate and resolution of 0.02 m/s or higher.
  • steering angle signal at >=50Hz sampling rate and resolution of 0.01 deg or higher.

Provide front speed measurement and steering angle with the dwEgomotion_addOdometry() API, or with the dwEgomotion_addVehicleState() API where the dwVehicleIOState struct contains speed, steeringAngle, speedTimestamp and steeringTimestamp.

DW_EGOMOTION_REAR_WHEEL_SPEED 

Indicates that speeds are angular speeds [rad/s] measured at rear wheels.

  • Positive angular speeds correspond to a forward motion of the vehicle.
Note
This mode requires valid dwEgomotionParameters.dwVehicle.wheelRadius otherwise incorrect estimation of the longitudinal speed will be made.
It is expected that both rear wheel speed measurements are not far apart in time, otherwise degradation in estimation quality is expected.
: estimation quality is dependent on measurement accuracy, resolution and sampling rate. We recommend the following:
  • speed signal at >=50Hz sampling rate and resolution of 0.05 rad/s or higher.

Provide rear wheel speed measurements with the dwEgomotion_addVehicleState() API where the dwVehicleIOState struct contains wheelSpeed and wheelSpeedTimestamp.

Definition at line 172 of file Egomotion.h.

◆ dwEgomotionSuspensionModel

Defines egomotion suspension model.

Enumerator
DW_EGOMOTION_SUSPENSION_RIGID_MODEL 

No suspension model. Equivalent to perfectly rigid suspension.

DW_EGOMOTION_SUSPENSION_TORSIONAL_SPRING_MODEL 

Models suspension as single-axis damped torsional spring.

Definition at line 221 of file Egomotion.h.

◆ dwMotionModel

Defines the motion models.

Enumerator
DW_EGOMOTION_ODOMETRY 

Given odometry information, estimates motion of the vehicle using a bicycle model.

The following parameters are required for this model:

  • dwEgomotionParameters
    • vehicle
      • wheelbase, mass, inertia3D, frontCorneringStiffness, rearCorneringStiffness, centerOfMassToRearAxle, wheelRadius (1), steeringWheelToSteeringMap (3), maxSteeringWheelAngle (3), frontSteeringOffset (3)
    • speedMeasurementType

The following odometry measurements are required for this model:

This model is capable of providing the following estimates:

Uncertainty estimates are not supported at this time.

DW_EGOMOTION_IMU_ODOMETRY 

Fuses odometry model with IMU measurements to estimate motion of the vehicle.

The following parameters are required for this model:

  • dwEgomotionParameters
    • vehicle
      • wheelRadius (1), steeringWheelToSteeringMap (3), maxSteeringWheelAngle (3), wheelbase (4), mass (4), centerOfMassHeight (4), inertia3D (4)
    • imu2rig
    • speedMeasurementType

The following odometry measurements are required for this model:

This model is capable of providing the following estimates:

Uncertainty estimates are provided for all state estimates listed above.

Definition at line 83 of file Egomotion.h.

◆ dwMotionModelMeasurement

Defines motion measurements.

Enumerator
DW_EGOMOTION_MEASURMENT_VELOCITY 

Vehicle velocity [m/s].

DW_EGOMOTION_MEASURMENT_STEERINGANGLE 

Steering angle [rad].

DW_EGOMOTION_MEASURMENT_STEERINGWHEELANGLE 

Steering wheel angle [rad].

Definition at line 163 of file Egomotion.h.

Function Documentation

◆ dwEgomotion_addIMUMeasurement()

DW_API_PUBLIC dwStatus dwEgomotion_addIMUMeasurement ( const dwIMUFrame imu,
dwEgomotionHandle_t  obj 
)

Adds an IMU frame to the egomotion module.

The IMU frame shall contain either linear acceleration or angular velocity measurements for X, Y and Z axes or both at once; the frame will be discarded otherwise.

Parameters
[in]imuIMU measurement.
[in]objEgomotion handle.
Returns
DW_INVALID_ARGUMENT - if timestamp is not greater than last measurement timestamp, or given frame is invalid.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_NOT_SUPPORTED - if the given measurement is not supported by the egomotion model.
DW_SUCCESS

◆ dwEgomotion_addOdometry()

DW_API_PUBLIC dwStatus dwEgomotion_addOdometry ( dwMotionModelMeasurement  measuredType,
float32_t  measuredValue,
dwTime_t  timestamp,
dwEgomotionHandle_t  obj 
)

Notifies the egomotion module of a new odometry measurement.

Note
measurement timestamp must be strictly increasing; i.e. take place after the previous measurement timestamp.
Parameters
[in]measuredTypeType of measurement. For example: velocity, steering angle.
[in]measuredValueValue that was measured. For example: 3.6 m/s, 0.1 rad.
[in]timestampTimestamp for the measurement.
[in]objEgomotion handle.
Returns
DW_INVALID_ARGUMENT - if timestamp is not greater than last measurement timestamp, measuredType is not valid, or measuredValue is not finite.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_NOT_SUPPORTED - if odometry measurements are not supported by the egomotion model or if egomotion speedMeasurementType is not DW_EGOMOTION_FRONT_SPEED.
DW_SUCCESS
Note
When velocity is passed to this module, it is assumed the velocity is measured on front wheels in the steering direction. See dwEgomotionSpeedMeasurementType::DW_EGOMOTION_FRONT_SPEED

◆ dwEgomotion_addVehicleState()

DW_API_PUBLIC dwStatus dwEgomotion_addVehicleState ( const dwVehicleIOState state,
dwEgomotionHandle_t  obj 
)

Notifies the egomotion module of a changed vehicle state.

In case relevant new information is contained in this state then it gets consumed, otherwise state is ignored.

Parameters
[in]stateNew VehicleIOState which contains potentially new information for egomotion consumption
[in]objEgomotion handle.
Returns
DW_INVALID_HANDLE - if provided, the egomotion handle is invalid.
DW_NOT_SUPPORTED - if underlying egomotion handle does not support vehicle state.
DW_SUCCESS

◆ dwEgomotion_applyRelativeTransformation()

DW_API_PUBLIC dwStatus dwEgomotion_applyRelativeTransformation ( dwTransformation3f newVehicle2World,
const dwTransformation3f poseOld2New,
const dwTransformation3f oldVehicle2World 
)

Applies the estimated relative motion as returned by dwEgomotion_computeRelativeTransformation() to a given vehicle pose.

Parameters
[out]newVehicle2WorldTransformation representing new pose of a vehicle after applying the relative motion.
[in]poseOld2NewRelative motion between two timestamps.
[in]oldVehicle2WorldCurrent pose of a vehicle.
Returns
DW_INVALID_ARGUMENT - if any of the given arguments is nullptr
DW_SUCCESS

◆ dwEgomotion_computeRelativeTransformation()

DW_API_PUBLIC dwStatus dwEgomotion_computeRelativeTransformation ( dwTransformation3f poseAtoB,
dwEgomotionRelativeUncertainty uncertainty,
dwTime_t  timestamp_a,
dwTime_t  timestamp_b,
dwEgomotionConstHandle_t  obj 
)

Computes the relative transformation between two timestamps and the uncertainty of this transform.

Parameters
[out]poseAtoBTransformation mapping a point at time a to a point at time b.
[out]uncertaintyRotational and translational uncertainty of transformation (optional, ignored if nullptr provided)
[in]timestamp_aTimestamp corresponding to beginning of transformation.
[in]timestamp_bTimestamp corresponding to end of transformation.
[in]objEgomotion handle.
Note
validity of uncertainty estimates is indicated by dwEgomotionRelativeUncertainty::valid
uncertainty estimates are currently only supported by DW_IMU_ODOMETRY motion model.
Returns
DW_INVALID_ARGUMENT - if any required argument is invalid.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_NOT_AVAILABLE - if there is currently not enough data available to make a prediction or requested timestamps are outside of available range (
See also
dwEgomotion_estimate).
DW_SUCCESS

◆ dwEgomotion_computeSteeringAngleFromIMU()

DW_API_PUBLIC dwStatus dwEgomotion_computeSteeringAngleFromIMU ( float32_t steeringAngle,
float32_t inverseSteeringR,
const dwIMUFrame imuMeasurement,
dwEgomotionConstHandle_t  obj 
)

Computes steering angle of the vehicle based on IMU measurement.

The computation will take the yaw rate measurement from the IMU and combine it with currently estimated speed and wheel base. Speed used for estimation will be the reported by dwEgomotion_getEstimation().

Parameters
[out]steeringAngleSteering angle estimated from imu yaw rate. This parameter is optional.
[out]inverseSteeringRInverse radius of the arc driven by the vehicle. This parameter is optional.
[in]imuMeasurementCurrent IMU measurement with IMU measured in vehicle coordinate system
[in]objEgomotion handle.
Returns
DW_INVALID_ARGUMENT - if any of the provided pointer arguments is nullptr DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_NOT_AVAILABLE - if there is currently no estimation available
DW_SUCCESS
Note
Depending on the underlying model the estimation might differ. Default implementation is to output steering angle and inverse radius as derived using bicycle model.

◆ dwEgomotion_estimate()

DW_API_PUBLIC dwStatus dwEgomotion_estimate ( dwEgomotionResult pose,
dwEgomotionUncertainty uncertainty,
dwTime_t  timestamp_us,
dwEgomotionConstHandle_t  obj 
)

Estimates the state for a given timestamp.

This method does not modify internal state and can only be used to extrapolate motion into the future or interpolate motion between estimates the past.

Parameters
[out]poseStruct where to store estimate state.
[out]uncertaintyStruct where to store estiamted state uncertainties. Can be nullptr if not used.
[in]timestamp_usTimestamp to estimate state for.
[in]objEgomotion handle.
Returns
DW_NOT_AVAILABLE - if there is currently not enough data available to provide an estimate, or requested timestamp is too far from available estimates.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_SUCCESS
Note
Interpolation into the past can only happen within the available history range.
Interpolation interval is limited to 5 seconds, if interval between two successive poses is larger, the function will return DW_NOT_AVAILABLE.
Extrapolation into the future is limited to 2.5 seconds and the function will return DW_NOT_AVAILABLE if the extrapolation interval is larger.

◆ dwEgomotion_getEstimation()

DW_API_PUBLIC dwStatus dwEgomotion_getEstimation ( dwEgomotionResult result,
dwEgomotionConstHandle_t  obj 
)

Gets the latest state estimate.

Parameters
[out]resultA pointer to the dwEgomotionResult struct containing state estimate.
[in]objEgomotion handle.
Returns
DW_INVALID_ARGUMENT - if the provided pointer is nullptr.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_NOT_AVAILABLE - if there is currently no estimation available.
DW_SUCCESS

◆ dwEgomotion_getEstimationTimestamp()

DW_API_PUBLIC dwStatus dwEgomotion_getEstimationTimestamp ( dwTime_t timestamp,
dwEgomotionConstHandle_t  obj 
)

Gets the timestamp of the latest state estimate.

The timestamp will be updated after each egomotion filter update.

Parameters
[out]timestampPointer to be filled with timestamp in [usec] of latest available state estimate.
[in]objEgomotion handle.
Returns
DW_INVALID_ARGUMENT - if the provided timestamp pointer is nullptr.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_NOT_AVAILABLE - if there is currently no estimation available.
DW_SUCCESS

◆ dwEgomotion_getHistoryElement()

DW_API_PUBLIC dwStatus dwEgomotion_getHistoryElement ( const dwEgomotionResult **  pose,
const dwEgomotionUncertainty **  uncertainty,
size_t  index,
dwEgomotionConstHandle_t  obj 
)

Returns an element from the motion history that is currently available.

Parameters
[out]poseReturn state estimate at the requested index in history (can be null if not interested in data).
[out]uncertaintyReturn estimate uncertainty at the requested index in history (can be null if not interested in data).
[in]indexIndex into the history, in the range [0; dwEgomotion_getHistorySize), with 0 being latest estimate and last element pointing to oldest estimate.
[in]objEgomotion handle.
Returns
DW_INVALID_HANDLE - if the egomotion handle is invalid
DW_INVALID_ARGUMENT - if the index is outside of available history
DW_SUCCESS

◆ dwEgomotion_getHistorySize()

DW_API_PUBLIC dwStatus dwEgomotion_getHistorySize ( size_t *  num,
dwEgomotionConstHandle_t  obj 
)

Returns the number of elements currently stored in the history.

Parameters
[out]numA pointer to the number of elements in the history.
[in]objEgomotion handle.
Returns
DW_INVALID_ARGUMENT - if the provided pointer is nullptr.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_SUCCESS

◆ dwEgomotion_getMotionModel()

DW_API_PUBLIC dwStatus dwEgomotion_getMotionModel ( dwMotionModel model,
dwEgomotionConstHandle_t  obj 
)

Returns the type of the motion model used.

Parameters
[out]modelType of the motion model which is used by the instance specified by the handle.
[in]objEgomotion handle.
Returns
DW_INVALID_ARGUMENT - if the provided pointer is nullptr.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_SUCCESS

◆ dwEgomotion_getUncertainty()

DW_API_PUBLIC dwStatus dwEgomotion_getUncertainty ( dwEgomotionUncertainty result,
dwEgomotionConstHandle_t  obj 
)

Gets the latest state estimate uncertainties.

Parameters
[out]resultA pointer to the dwEgomotionUncertainty struct containing estimated state uncertainties.
[in]objEgomotion handle.
Returns
DW_INVALID_ARGUMENT - if the provided pointer is nullptr.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_NOT_AVAILABLE - if there is currently no estimation available.
DW_SUCCESS
Note
Not all values do have valid uncertainties. Check dwEgomotionUncertainty.validFlags.

◆ dwEgomotion_hasEstimation()

DW_API_PUBLIC dwStatus dwEgomotion_hasEstimation ( bool *  result,
dwEgomotionConstHandle_t  obj 
)

Check whether has state estimate.

Parameters
[out]resultA pointer to the bool.
[in]objEgomotion handle.
Returns
DW_INVALID_ARGUMENT - if the provided pointer is nullptr.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_SUCCESS

◆ dwEgomotion_initialize()

DW_API_PUBLIC dwStatus dwEgomotion_initialize ( dwEgomotionHandle_t obj,
const dwEgomotionParameters params,
dwContextHandle_t  ctx 
)

Initializes the egomotion module.

Parameters
[out]objA pointer to the egomotion handle for the created module.
[in]paramsA pointer to the configuration parameters of the module.
[in]ctxSpecifies the handler to the context under which the Egomotion module is created.
Returns
DW_INVALID_ARGUMENT - if provided egomotion handle or parameters are invalid.
DW_INVALID_HANDLE - if the provided DriveWorks context handle is invalid.
DW_SUCCESS

◆ dwEgomotion_initParamsFromRig()

DW_API_PUBLIC dwStatus dwEgomotion_initParamsFromRig ( dwEgomotionParameters params,
dwConstRigHandle_t  rigConfiguration,
const char *  imuSensorName,
const char *  canSensorName 
)

Initialize egomotion parameters from a provided RigConfiguration.

This will read out vehicle as well as all relevant sensor parameters and apply them on top of default egomotion parameters.

Parameters
[out]paramsPointer to a parameter struct to be filled out with vehicle and sensor parameters
[in]rigConfigurationHandle to a rig configuration to retrieve parameters from
[in]imuSensorNamename of the IMU sensor to be used (optional, can be null)
[in]canSensorNamename of the CAN sensor to be used (optional, can be null)
Returns
DW_INVALID_ARGUMENT - if provided params pointer or rig handle are invalid
DW_FILE_INVALID - if provided sensor could not be found in the rig config
DW_SUCCESS
Note
Clears any existing parameters set in params.
If a sensor name is null, no sensor specific parameters will be extracted from the configuration for this sensor.
Following parameters are extracted from the rig configuration: CAN sensor:

◆ dwEgomotion_initParamsFromRigByIndex()

DW_API_PUBLIC dwStatus dwEgomotion_initParamsFromRigByIndex ( dwEgomotionParameters params,
dwConstRigHandle_t  rigConfiguration,
uint32_t  imuSensorIdx,
uint32_t  canSensorIdx 
)

Same as dwEgomotion_initParamsFromRig however uses sensor indices in rigConfiguration instead of their names.

Parameters
[out]paramsPointer to a parameter struct to be filled out with default data
[in]rigConfigurationHandle to a rig configuration to retrieve parameters from
[in]imuSensorIdxIndex of the IMU sensor to be retrieved (optional, can be (uint32_t)-1)
[in]canSensorIdxIndex of the CAN sensor to be retrieved (optional, can be (uint32_t)-1)
Note
Clears any existing parameters set in params.
Returns
DW_INVALID_ARGUMENT - if provided params pointer or rig handle are invalid
DW_FILE_INVALID - if provided sensor could not be found in the rig config
DW_SUCCESS

◆ dwEgomotion_release()

DW_API_PUBLIC dwStatus dwEgomotion_release ( dwEgomotionHandle_t  obj)

Releases the egomotion module.

Note
This method renders the handle unusable.
Parameters
[in]objEgomotion handle to be released.
Returns
DW_INVALID_HANDLE - if the provided egomotion handle is invalid.
DW_SUCCESS

◆ dwEgomotion_reset()

DW_API_PUBLIC dwStatus dwEgomotion_reset ( dwEgomotionHandle_t  obj)

Resets the state estimate and all history of the egomotion module.

All consecutive motion estimates will be relative to the (new) origin.

Parameters
[in]objEgomotion handle to be reset.
Returns
DW_INVALID_HANDLE - if the provided egomotion handle is invalid.
DW_SUCCESS

◆ dwEgomotion_steeringAngleToSteeringWheelAngle()

DW_API_PUBLIC dwStatus dwEgomotion_steeringAngleToSteeringWheelAngle ( float32_t steeringWheelAngle,
float32_t  steeringAngle,
dwEgomotionHandle_t  obj 
)

Convert steering angle to steering wheel angle.

Parameters
[out]steeringWheelAnglesteering wheel angle [radian]
[in]steeringAnglesteering angle [radian]
[in]objSpecifies the egomotion module handle.
Returns
DW_INVALID_ARGUMENT - if any of the provided pointer arguments is nullptr.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_NOT_SUPPORTED - if underlying egomotion handle does not support steering angle to steering wheel angle conversion. The egomotion model has to be DW_EGOMOTION_ODOMETRY or DW_EGOMOTION_IMU_ODOMETRY.
DW_SUCCESS

◆ dwEgomotion_steeringWheelAngleToSteeringAngle()

DW_API_PUBLIC dwStatus dwEgomotion_steeringWheelAngleToSteeringAngle ( float32_t steeringAngle,
float32_t  steeringWheelAngle,
dwEgomotionHandle_t  obj 
)

Convert steering wheel angle to steering angle.

Parameters
[out]steeringAnglesteering angle [radian]
[in]steeringWheelAnglesteering wheel angle [radian]
[in]objSpecifies the egomotion module handle.
Returns
DW_INVALID_ARGUMENT - if any of the provided pointer arguments is nullptr.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_NOT_SUPPORTED - if underlying egomotion handle does not support steering wheel angle to steering angle conversion. The egomotion model has to be DW_EGOMOTION_ODOMETRY or DW_EGOMOTION_IMU_ODOMETRY.
DW_SUCCESS

◆ dwEgomotion_update()

DW_API_PUBLIC dwStatus dwEgomotion_update ( dwTime_t  timestamp_us,
dwEgomotionHandle_t  obj 
)

Runs the motion model estimation for a given timestamp.

The internal state is modified. The motion model advances to the given timestamp. To retrieve the result of the estimation, use dwEgomotion_getEstimation().

This method allows the user to update the egomotion filter when required, for a specific timestamp, using all sensor data available up to this timestamp.

When the automatic update period is active (automaticUpdate in dwEgomotionParameters is set), dwEgomotion_update() will not update the filter state and throw a DW_NOT_SUPPORTED exception instead.

Parameters
[in]timestamp_usTimestamp for which to estimate vehicle state.
[in]objEgomotion handle.
Returns
DW_INVALID_ARGUMENT - if given timestamp is not greater than last update timestamp.
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_NOT_SUPPORTED - if the automatic estimation update period is active (i.e. non-zero).
DW_SUCCESS
Note
The provided timestamp must be larger than the previous timestamp used when calling dwEgomotion_estimate().
A full set of new sensor measurements should be added before calling this method; otherwise the state estimate might be based on older, extrapolated sensor measurements.
Updating internal state does not guarantee that a motion estimation is available, i.e. there might be not enough data to provide an estimate. Use dwEgomotion_getEstimation() and/or dwEgomotion_computeRelativeTransformation() to query estimation if it is available.

◆ dwEgomotion_updateIMUExtrinsics()

DW_API_PUBLIC dwStatus dwEgomotion_updateIMUExtrinsics ( const dwTransformation3f imuToRig,
dwEgomotionHandle_t  obj 
)

This method updates the IMU extrinsics to convert from the IMU coordinate system to the vehicle rig coordinate system.

Parameters
[in]imuToRigTransformation from the IMU coordinate system to the vehicle rig coordinate system.
[in]objEgomotion handle.
Returns
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_INVALID_ARGUMENT - if provided imuToRig is nullptr.
DW_NOT_SUPPORTED - if given egomotion instance does not support change of this parameter.
DW_SUCCESS

◆ dwEgomotion_updateVehicle()

DW_API_PUBLIC dwStatus dwEgomotion_updateVehicle ( const dwVehicle vehicle,
dwEgomotionHandle_t  obj 
)

This method updates the egomotion module with an updated vehicle.

Parameters
[in]vehicleUpdated vehicle which may contain updated vehicle parameters.
[in]objEgomotion handle.
Returns
DW_INVALID_HANDLE - if the given egomotion handle is invalid.
DW_INVALID_ARGUMENT - if provided vehicle is nullptr.
DW_NOT_SUPPORTED - if given egomotion instance does not support change of this parameter.
DW_SUCCESS