Skip to content

Latest commit

 

History

History
85 lines (55 loc) · 4.16 KB

README.md

File metadata and controls

85 lines (55 loc) · 4.16 KB

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.

ros_architecture

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

state

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

rotation_1

rotation_2

predict_position

predict_velocity

Angular

predict_orientation

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