Global Egomotion Workflow

Workflow

Initialization

To initialize the global egomotion module, certain configuration parameters can be set, for example the position of the GNSS antenna in the rig coordinate system. Other parameters include noise and drift characteristics, or the size of the internal history storing state estimates. A typical initialization code sequence will look like this:

// read vehicle configuration
dwRigHandle_t rig;
dwRig_initializeFromFile(&rig, sdkContext, "path/to/rig.json");
    
// initialize global egomotion parameters
dwGlobalEgomotionParameters params{};
dwGlobalEgomotion_initParamsFromRig(&params, rig, "gps_sensor_name");
 
// other parameters left 0-initialized will be ignored and default values used instead
// see API documentation
 
// initialize global egomotion module
dwGlobalEgomotion_initialize(&globalEgomotion, &params, sdkContext);    

Run-time filter update and global position and orientation estimation

At run-time the global egomotion filter has to be filled with measurements from the GNSS sensor, as well as motion estimates from a relative egomotion module. The following example assumes that such a relative egomotion module with handle relativeEgomotion has been initialized and is being updated elsewhere; see Relative Egomotion Workflow for more details.

while(true)
{
    // read GPS measurements
    if (hasGPSMeasurement())
    {
        dwGlobalEgomotion_addGPSMeasurement(getGPSMeasurement(), globalEgomotion);
    }
 
    ...
 
    // update global egomotion module with latest relative egomotion estimates
    {
        dwEgomotionResult state;
        dwEgomotionUncertainty uncertainty;
        
        if (dwEgomotion_getEstimation(&state, relativeEgomotion) == DW_SUCCESS &&
            dwEgomotion_getUncertainty(&uncertainty, relativeEgomotion) == DW_SUCCESS)
            {
                dwGlobalEgomotion_addRelativeMotion(&state, &uncertainty, globalEgomotion);
            }
    }
 
    ...
 
    // get latest global state estimate
    {
        dwGlobalEgomotionResult result;
        dwGlobalEgomotionUncertainty uncertainty;
        dwGlobalEgomotion_getEstimate(&result, &uncertainty, globalEgomotion);
        
        // position estimate in WGS-84 as well as orientation relative to ENU coordinate system
        // can now be accessed.
    }
    
    ...
    
    // interpolate global state estimate to specific timestamp
    // extrapolation is supported but limited to a short time interval, see API doc.
    {
        dwTime_t requestedTimestamp = ...;
        dwGlobalEgomotionResult result;
        dwGlobalEgomotionUncertainty uncertainty;
        
        dwGlobalEgomotion_computeEstimate(&result, &uncertainty, requestedTimestamp, globalEgomotion);
        
        // position estimate in WGS-84 as well as orientation relative to ENU coordinate system
        // at `requestedTimestamp` can now be accessed.
    }
}

Before using any state estimates, always verify their validity by inspecting the return codes of the various APIs as well as the flags contained within the state and uncertainty structures.

This workflow is demonstrated in the following sample: Egomotion Sample