(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Tracking object Cartesian pose

Description: Guide on filtering raw detections into tracked object poses

Tutorial Level: INTERMEDIATE

Overview

In the previous tutorial, we introduced a tracking filter to solve the problem of object tracking from raw detections across successive radar scans. That filter operated in spherical coordinates (the same as radar detections), however, which means the filtered speed remains along the measurement direction.

This follows from the fact that the radar can only measure the component of velocity along the unit vector in the direction of the detected object, thus ignoring the fact that we can use detected range/angles over successive frames to resolve the true Cartesian velocity.

cartesian_tracking_filter.gif

Background

In this tutorial, we're considering the same problem as in the previous one - tracking objects through successive radar scans. Consider again the figure below. In the top row, the detections evolve over time as the objects move around; even as the cross one another's paths, assuming the framerate is high enough, it's not terribly difficult to "connect the dots" in range-doppler space to track them. The second row is much more realistic, however - there are many returns from each object, plus background noise, which make it difficult to tell where the objects move and even how many distinct objects are present.

target_tracking_hard.png

The difference here, as mentioned above, is that we can perform tracking in the Cartesian reference frame to estimate the full 3d velocity of objects, and hence estimate their pose under the simplifying assumption that the objects always move along their forward direction.

pose_tracking.png

Theory and Implementation

As for the original tracking filter (see previous tutorial), the Cartesian pose tracking filter is a bank of simple Kalman filters operating on a tracked detection's state - here in Cartesian coordinates. The flowchart below describes roughly how the filter works.

cartesian_tracking_filter_flowchart.png

Essentially, a main thread continually updates the filters in the bank using the process model, while new radar data triggers a callback in which detections (targets) are evaluated against the existing filters in the bank. If a target is consistent with a filter's state, it passes through that filter's validation gate and triggers a Kalman filter update. If a target is not consistent with any filter, it is pushed back as a new filter. If a filter has not been updated within a specified timeout, it is removed from the bank.

The state of each instance of the filter is

latex error! exitcode was 2 (signal 0), transscript follows:

[Thu Dec 02 07:31:45.813735 2021] [:error] [pid 23218] failed to exec() latex

where

latex error! exitcode was 2 (signal 0), transscript follows:

[Thu Dec 02 07:31:45.833919 2021] [:error] [pid 23219] failed to exec() latex

are the tracked object's position and velocity vectors, respectively. The process model is simply

latex error! exitcode was 2 (signal 0), transscript follows:

[Thu Dec 02 07:31:45.854205 2021] [:error] [pid 23220] failed to exec() latex

Or, in the matrix form

latex error! exitcode was 2 (signal 0), transscript follows:

[Thu Dec 02 07:31:45.874501 2021] [:error] [pid 23221] failed to exec() latex

we can write

filter_cartesian_process_model.png

where F is the state transition matrix, L is the process noise matrix, and

latex error! exitcode was 2 (signal 0), transscript follows:

[Thu Dec 02 07:31:45.899356 2021] [:error] [pid 23222] failed to exec() latex

is the process noise vector on velocity.

The measurement model is more complicated for the Cartesian state filter because we cannot measure the full state directly as before.

We can measure the Cartesian position computed from range and angles of raw detections:

filter_cartesian_pos_meas.png

However, instead of measuring Cartesian velocity, we measure the radial speed s which is the projection of the Cartesian velocity onto the radial direction:

latex error! exitcode was 2 (signal 0), transscript follows:

[Thu Dec 02 07:31:45.923941 2021] [:error] [pid 23223] failed to exec() latex

The speed measurement is nonlinear since it involves a product of position and velocity, and therefore the filter becomes an Extended Kalman Filter (EKF) with the measurement model linearized around the current state estimate

latex error! exitcode was 2 (signal 0), transscript follows:

[Thu Dec 02 07:31:45.944036 2021] [:error] [pid 23224] failed to exec() latex

each update. The linearized model in the form

latex error! exitcode was 2 (signal 0), transscript follows:

[Thu Dec 02 07:31:45.963047 2021] [:error] [pid 23225] failed to exec() latex

is thus

filter_cartesian_meas_model.png

where H is the measurement model matrix and

latex error! exitcode was 2 (signal 0), transscript follows:

[Thu Dec 02 07:31:45.984456 2021] [:error] [pid 23226] failed to exec() latex

is the noise vector on the position measurement and

latex error! exitcode was 2 (signal 0), transscript follows:

[Thu Dec 02 07:31:46.004097 2021] [:error] [pid 23227] failed to exec() latex

is the noise on the radial speed measurement.

Wiki: ainstein_radar/Tutorials/Tracking object Cartesian pose (last edited 2020-02-13 19:04:12 by nrotella-ainstein)