Localization

Robot localization using unscented Kalman filter. This package does not stink :)

• Output state message param: /topic/sensor/odom_state
• Output frame: temp_odom

Run

• Node: roslaunch au_localization localizer.launch
• Tests: catkin_make run_tests_au_localization

File structure

• types.h - declares custom data-types
• ufk.h - generic UKF implementation
• ukf_helpers.h - implements UKF sigma point class and unscented transform function
• localization(.h .cpp) - implements a localization filter using UKF; process and measurement models defined here
• localization_ros(.h .cpp) - ROS wrapper for localization; implements all ROS inputs and outputs for localization
• localization_node.cpp - runs LocalizationRos class as a ros node

ROS architecture

All sensor measurements are transformed to robot's base_link frame and added to a temporal priority queue. The UKF filter is periodically called by a timer (rate defined by filter frequency param). All past measurements in the queue are then sequentially passed to the filter. The node then publishes the state estimate, and odom->base_link transform.

At startup, the node waits for an IMU message to initialize the filter state.

Note: robot pose is w.r.t. world; robot velocity is w.r.t. robot (base_link)

Unscented Kalman Filter

State

The robot state is defined as a 15 element vector

Note: φ = roll; θ = pitch; ψ = yaw

Predict Step

State transition model (F)

The state transition model is used to make predict next robot state from previous state. A 3D kinematics model with constant linear acceleration and constant angular velocity is used.

(Reference: Section 2.2.1 from Handbook of Marine Craft Hydrodynamics and Motion Control, First Edition. Thor I. Fossen.)

Linear

Angular

Note: this transformation is undefined if θ=+/-90 degrees

Measurement Step

Measurement model (H)

A simple model is used where state is simply reduced to match measurement (aka drop state variables that are not part of the measurement).

E.g. for depth sensor: the model simply returns state z.

note: for IMU the wrapping for orientation angles is handled as well