 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. ## 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. 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. ## 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. 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 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: 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 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)