 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 posesTutorial Level: INTERMEDIATE
Contents
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 Apr 25 20:02:14.493284 2024] [:error] [pid 9118] failed to exec() latex
where
latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 20:02:14.508914 2024] [:error] [pid 9119] 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 Apr 25 20:02:14.524638 2024] [:error] [pid 9120] failed to exec() latex
Or, in the matrix form
latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 20:02:14.540234 2024] [:error] [pid 9121] 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 Apr 25 20:02:14.557023 2024] [:error] [pid 9123] 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 Apr 25 20:02:14.575316 2024] [:error] [pid 9124] 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 Apr 25 20:02:14.590147 2024] [:error] [pid 9125] failed to exec() latex
each update. The linearized model in the form
latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 20:02:14.603340 2024] [:error] [pid 9126] failed to exec() latex
is thus
where H is the measurement model matrix and
latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 20:02:14.620754 2024] [:error] [pid 9127] failed to exec() latex
is the noise vector on the position measurement and
latex error! exitcode was 2 (signal 0), transscript follows: [Thu Apr 25 20:02:14.634653 2024] [:error] [pid 9128] failed to exec() latex
is the noise on the radial speed measurement.
