...
The high dynamics filter works well for scenarios with agressive driving maneuvers such as drifting and cornering. During such maneuvers the turning motion of the wheels generally doesn’t directly correspond with the direction of the vehicle. Therefore for this filter doesn’t rely on wheel velocity measurements for localization like the low-dynamics (LD) filter. This filter uses information from the wheels to determine if the car has come to a full stop.
...
fusedVehiclePose(2D pose): Output equivalent to the LD filter output. Includes position in meters relative to starting point, global position (lon, lat) and heading.fusedPose(3D pose): relative to starting point, x, y (in meters) + z (height in meters) + 3D orientation quaternionglobalFusedPose:globally referenced 3D position (longitude, latitude, height in meters) + 3D orientation quaternion in ENU frame
...
You can find more information regarding the ENU coordinate system here.
Notes on IMU Arrangement
The used For the car frame is we use the Volkswagen (VW) coordinate frame convention:
...
And the orientation quaternion woud be [w, x, y, z, w] = [ 0, 0, 0, 1 , 0 ] which can be specified in the configuration like below:
| Code Block |
|---|
"imuToCarRotation": {
"w": 0,
"x": 0,
"y": 0,
"z": 1
} |
Check this page for more information on how to calculate the orientation quaternion.
Note: The orientation of the IMU for a unity quaternion [ 1, 0, 0, 0 ] would be
| Code Block |
|---|
IMU mounting
x: backwards
y: right
z: up |
Configuration Block
Insert the following configuration block into your config.json file in the sinks section to activate the HD filter. The filter's node name is gnssImuFusion.
| Code Block |
|---|
..
"sinks": {
..,
"gnssImuFusion": {
"echoFusedPose": false,
"endpoint": "tcp://*:8803",
"fuser": {
"fitModel": "ModelGnssImu",
"accelError": 0.01,
"omegaError": 0.02,
"measurementError": 0.05,
"imuToCarRotation": {
"w": 1,
"x": 0,
"y": 0,
"z": 0
},
}
"outputWhenFilterNotReady": false,
"retainStateWhenStopped": true,
}
.. |
See below a description of the parameter options for the HD filter.
...
Parameter name
...
Description
...
Default
"smoothFit": true,
"globalOrigin": {
"longitude": 139.73226516666668,
"latitude": 35.657632
}
}
}
.. |
See below a description of the parameter options for the HD filter.
Parameter name | Description | Default |
|---|---|---|
echoFusedPose |
| false |
endpoint | Output port for the fusion result | 8801 |
fitModel | Model to use for fusion. | ModelGnssImu |
accelError | Acceleration error for Kalman filter. Keep default value. | 0.01 |
omegaError | Omega error for Kalman filter. Keep default value. | 0.02 |
measurementError | Measurement error for Kalman filter. Keep default value. | 0.05 |
imuToCarRotation | Orientation quaternion of IMU relative to car frame | 1, 0, 0, 0 |
smoothFit | Enable this option to prevent filter output from jumping between IMU data and GPS measurement. Keep enabled. | true |
singleEndpoint |
| true |
poseEndpoint | Output port for the |
|
globalPoseEndpoint | Output port for the |
|
outputRawGnssData | Publishes raw Gnss data position instead of the fusion output. Useful for debugging. | false |
outputWhenFilterNotReady | Publishes a temporary raw Gnss data output while the filter is initializing. Useful for a minimal sanity check before moving the vehicle. | false |
retainStateWhenStopped | If the vehicle is detected to be stationary by looking at data from CAN bus, then the current filter state (position, rotation etc.) will be automatically retained until motion is detected. | true |
globalOrigin | A global position as longitude and latitude: For | { 0, 0 } |
Output Data Format
FusedVehiclePose
...