Odometry EKF Fusion

OdometryEkfFusion is a component in the odometry extension that fuses wheelbase and IMU measurements to obtain a robot-centric state estimate. It uses a Extended Kalman Filter and a differential drive kinematics model to track the states in [position_x, position_y, heading, linear_speed, angular_speed, linear_acceleration, angular_acceleration].

It outputs the state estimate as DifferentialBaseEgoMotion composites through a transmitter channel and additionally updates the transform between robot_frame and odometry_frame in the PoseTree.

• Wheelbase observation (Expecting: DifferentialBaseState)

• IMU observation (Expecting: ImuMessageParts)

• Output state (DifferentialBaseEgoMotion)

It uses an Extended Kalman Filter tracking the state between measurements, using an adaptive time horizon to incorporate sensor measurements with varying time delays. It rewinds the state estimate to the previous known estimate and incorporates latent sensor measurements. The time window can be set through the parameter history_length_sec and should be slightly higher than the expected sensor latency.

NvIsaacOdometryExtension

Extension containing odometry related components.

nvidia::isaac::AckermannMockOdometry

Tracks Ackermann base state from dynamics measurements.

Parameters:

name

Input channel

lookup_name

dynamics_in

description

The incoming dynamic state measurement channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Output channel

lookup_name

state_out

description

The outgoing full state estimation channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Memory pool

lookup_name

pool

description

Handle to the memory allocator pool used for data generation

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Atlas Frontend

lookup_name

atlas

description

Handle to atlas frontend to access pose tree

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Robot Frame

lookup_name

robot_frame

description

The name of the pose frame containing the pose information.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_STRING

default

N/A

name

Odometry Frame

lookup_name

odometry_frame

description

The name of the pose frame containing the odometry information.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_STRING

default

N/A

nvidia::isaac::AckermannMockUpdateOdometry

Takes in full dynamic state from the ground truth and directly updates the pose tree. Also publishes an odometry message consisting of values taken directly from the ground truth state.

Parameters:

name

Input channel

lookup_name

dynamics_in

description

The incoming dynamic state measurement channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Output channel

lookup_name

state_out

description

The outgoing full state estimation channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Memory pool

lookup_name

pool

description

Handle to the memory allocator pool used for data generation

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Atlas Frontend

lookup_name

atlas

description

Handle to atlas frontend to access pose tree

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Robot Frame

lookup_name

robot_frame

description

The name of the pose frame containing the pose information.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Odometry Frame

lookup_name

odometry_frame

description

The name of the pose frame containing the odometry information.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

nvidia::isaac::AckermannOdometry

Tracks Ackermann base state from dynamics measurements.

Parameters:

name

Input channel

lookup_name

in

description

The incoming dynamic state measurement channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Output channel

lookup_name

out

description

The outgoing full state estimation channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Memory pool

lookup_name

pool

description

Handle to the memory allocator pool used for data generation

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Atlas Frontend

lookup_name

atlas

description

Handle to atlas frontend to access pose tree

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Predition Model Noise

lookup_name

prediction_noise

description

The EKF prediction noise model: [position_x, position_y, linear_velocity, linear_acceleration, heading, curvature, curvature_derivative]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Observation Model Noise

lookup_name

observation_noise

description

The EKF observation noise model: [linear_velocity, linear_acceleration, curvature, curvature_derivative]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Robot Frame

lookup_name

robot_frame

description

The name of the pose frame containing the estimated pose information.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_STRING

default

N/A

name

Odometry Frame

lookup_name

odometry_frame

description

The name of the odometry frame.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_STRING

default

N/A

nvidia::isaac::DifferentialWheelOdometry

Tracks differential base state from dynamics measurements.

Parameters:

name

Input measurement

lookup_name

rx_observation

description

The incoming dynamic state measurement channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Output estimated state

lookup_name

tx_state

description

The outgoing estimated full state channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Memory pool

lookup_name

pool

description

Handle to the memory allocator pool used for data generation

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Atlas Frontend

lookup_name

atlas

description

Handle to atlas frontend to access pose tree

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Prediction Model Noise

lookup_name

prediction_noise

description

The EKF prediction noise model: [position_x, position_y, linear_velocity, angular_velocity, linear_acceleration, angular_acceleration]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Observation Model Noise

lookup_name

observation_noise

description

The EKF observation noise model: [linear_velocity, angular_velocity, linear_acceleration, angular_acceleration]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Robot Frame

lookup_name

robot_frame

description

The target frame of the estimated odometry transform.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Odometry Frame

lookup_name

odometry_frame

description

The source frame of the estimated odometry transform.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

nvidia::isaac::DifferentialImuWheelOdometry

Tracks differential base state from dynamics measurements and imu measurements.

Parameters:

name

Input Dynamics Measurement

lookup_name

rx_dynamics_observation

description

The incoming dynamics measurement channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Input Imu Measurement

lookup_name

rx_imu_observation

description

The incoming imu measurement channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Output Estimated State

lookup_name

tx_state

description

The outgoing estimated full state channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Allocator

lookup_name

allocator

description

Handle to the memory allocator used for data generation

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Atlas Frontend

lookup_name

atlas

description

Handle to atlas frontend to access pose tree

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Prediction Model Noise

lookup_name

prediction_noise

description

The EKF prediction noise model: [position_x, position_y, linear_speed, angular_speed, linear_acceleration, angular_acceleration]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Observation Model Noise

lookup_name

observation_noise

description

The EKF observation noise model: [linear_speed, angular_speed, linear_acceleration, angular_acceleration]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Robot Frame

lookup_name

robot_frame

description

The target frame of the estimated odometry transform.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Odometry Frame

lookup_name

odometry_frame

description

The source frame of the estimated odometry transform.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Imu Linear Acceleration Weight

lookup_name

imu_linear_acceleration_weight

description

Used for calculating the weighted average between the imu observation and the differential dynamics observation.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_FLOAT64

default

N/A

name

Imu Angular Speed Weight

lookup_name

imu_angular_speed_weight

description

Used for calculating the weighted average between the imu observation and the dynamics observation.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_FLOAT64

default

N/A

name

Use Imu

lookup_name

use_imu

description

Whether the imu shall be used for the estimation.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_BOOL

default

N/A

nvidia::isaac::odometry_3d::OdometryEkfFusionAttitudeEstimator

Codelet fusing imu and wheelbase data for 3d state estimation.

Parameters:

name

Input Imu Measurement

lookup_name

rx_imu_observation

description

The incoming imu measurement channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Input Dynamics Measurement

lookup_name

rx_wheelbase_observation

description

The incoming dynamics measurement channel

flags

GXF_PARAMETER_FLAGS_OPTIONAL

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Output Differential Base State

lookup_name

tx_differential_base_state

description

The outgoing estimated state as DifferentialBaseStateComposite.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Prediction Noise Covariance

lookup_name

prediction_noise_covariance

description

The EKF prediction covariance per state and timestep [sin(θ), sin(φ)cos(θ), vx, yaw_rate]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Wheelbase Observation Noise Covariance

lookup_name

wheelbase_observation_noise_covariance

description

Expected noise covariance on the wheelbase observation [yaw_rate, linear_velocity]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Composite Schema Server

lookup_name

composite_schema_server

description

Handle to the composite schema server

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Pose Tree

lookup_name

pose_tree

description

Handle to the pose tree

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Robot Frame

lookup_name

robot_frame

description

The target frame of the estimated odometry transform.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Odometry Frame

lookup_name

odometry_frame

description

The source frame of the estimated odometry transform.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Allocator

lookup_name

allocator

description

The allocator

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

nvidia::isaac::odometry::differential_base::OdometryEkfFusion

Odometry fusion using extended kalman filter.

Parameters:

name

Input Dynamics Measurement

lookup_name

rx_wheelbase_observation

description

The incoming dynamics measurement channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Input Imu Measurement

lookup_name

rx_imu_observation

description

The incoming imu measurement channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Input Pose 3D

lookup_name

rx_pose3d_observation

description

The incoming pose3d measurement channel

flags

GXF_PARAMETER_FLAGS_OPTIONAL

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Output Estimated State

lookup_name

tx_state

description

The outgoing estimated full state channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Allocator

lookup_name

allocator

description

Handle to the memory allocator used for data generation

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Pose Tree

lookup_name

pose_tree

description

Handle to the pose tree

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Composite Schema Server

lookup_name

composite_schema_server

description

Handle to the composite schema server

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Prediction Model Noise

lookup_name

prediction_model_noise

description

The EKF prediction noise model: [position_x, position_y, heading, linear_speed, angular_speed, linear_acceleration, angular_acceleration]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Imu Observation Noise

lookup_name

imu_observation_noise

description

The EKF observation noise model: [linear_speed, angular_speed]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Wheelbase Observation Noise

lookup_name

wheelbase_observation_noise

description

The EKF observation noise model: [linear_speed, angular_speed, linear_acceleration, angular_acceleration]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Pose2d Observation Noise

lookup_name

pose2d_observation_noise

description

Pose2d observation noise model: [position_x, position_y, heading]

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_CUSTOM

default

N/A

name

Robot Frame

lookup_name

robot_frame

description

The target frame of the estimated odometry transform.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Odometry Frame

lookup_name

odometry_frame

description

The source frame of the estimated odometry transform.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

History Length

lookup_name

history_length_s

description

History lenght of observations taken into the update step

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_FLOAT64

default

0.1

name

Fuse Imu Observations

lookup_name

fuse_imu_observations

description

Imu observations are used to observe the state estimate

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_BOOL

default

True

name

Fuse Wheelbase Observations

lookup_name

fuse_wheelbase_observations

description

Wheelbase observations are used to observe the state estimate

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_BOOL

default

True

name

Fuse Pose Observations

lookup_name

fuse_pose_observations

description

Pose observations are used to observe the state estimate

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_BOOL

default

False

nvidia::isaac::PoseDynamicsCombiner

Combines a pose and a dynamics measurement to an odometry estimate.

Parameters:

name

Input Dynamics Measurement

lookup_name

rx_dynamics_observation

description

The incoming dynamics measurement channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Output Estimated Odometry

lookup_name

tx_odometry

description

The outgoing estimated odometry channel

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Allocator

lookup_name

allocator

description

N/A

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Pose Tree

lookup_name

pose_tree

description

N/A

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Composite Schema Server

lookup_name

composite_schema_server

description

N/A

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Odometry Frame

lookup_name

odometry_frame

description

The source frame of the estimated odometry transform.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A

name

Robot Frame

lookup_name

robot_frame

description

The target frame of the estimated odometry transform.

flags

GXF_PARAMETER_FLAGS_NONE

type

GXF_PARAMETER_TYPE_HANDLE

default

N/A