 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 1 (signal 0), transscript follows: This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex) kpathsea: Running mktexfmt latex.fmt mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order): mktexfmt: /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes: mktexfmt: /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf /usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied I can't find the format file `latex.fmt'!
where
latex error! exitcode was 1 (signal 0), transscript follows: This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex) kpathsea: Running mktexfmt latex.fmt mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order): mktexfmt: /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes: mktexfmt: /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf /usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied I can't find the format file `latex.fmt'!
are the tracked object's position and velocity vectors, respectively. The process model is simply
latex error! exitcode was 1 (signal 0), transscript follows: This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex) kpathsea: Running mktexfmt latex.fmt mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order): mktexfmt: /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes: mktexfmt: /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf /usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied I can't find the format file `latex.fmt'!
Or, in the matrix form
latex error! exitcode was 1 (signal 0), transscript follows: This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex) kpathsea: Running mktexfmt latex.fmt mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order): mktexfmt: /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes: mktexfmt: /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf /usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied I can't find the format file `latex.fmt'!
we can write
where F is the state transition matrix, L is the process noise matrix, and
latex error! exitcode was 1 (signal 0), transscript follows: This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex) kpathsea: Running mktexfmt latex.fmt mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order): mktexfmt: /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes: mktexfmt: /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf /usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied I can't find the format file `latex.fmt'!
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 1 (signal 0), transscript follows: This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex) kpathsea: Running mktexfmt latex.fmt mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order): mktexfmt: /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes: mktexfmt: /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf /usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied I can't find the format file `latex.fmt'!
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 1 (signal 0), transscript follows: This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex) kpathsea: Running mktexfmt latex.fmt mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order): mktexfmt: /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes: mktexfmt: /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf /usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied I can't find the format file `latex.fmt'!
each update. The linearized model in the form
latex error! exitcode was 1 (signal 0), transscript follows: This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex) kpathsea: Running mktexfmt latex.fmt mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order): mktexfmt: /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes: mktexfmt: /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf /usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied I can't find the format file `latex.fmt'!
is thus
where H is the measurement model matrix and
latex error! exitcode was 1 (signal 0), transscript follows: This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex) kpathsea: Running mktexfmt latex.fmt mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order): mktexfmt: /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes: mktexfmt: /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf /usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied I can't find the format file `latex.fmt'!
is the noise vector on the position measurement and
latex error! exitcode was 1 (signal 0), transscript follows: This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex) kpathsea: Running mktexfmt latex.fmt mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order): mktexfmt: /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes: mktexfmt: /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf /usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied I can't find the format file `latex.fmt'!
is the noise on the radial speed measurement.
