+----------------+-----+----------+-----------+---+---+-----------------+
| **REPORT       |     |          |           | * |   |                 |
| DOCUMENTATION  |     |          |           | F |   |                 |
| PAGE**         |     |          |           | o |   |                 |
|                |     |          |           | r |   |                 |
|                |     |          |           | m |   |                 |
|                |     |          |           | A |   |                 |
|                |     |          |           | p |   |                 |
|                |     |          |           | p |   |                 |
|                |     |          |           | r |   |                 |
|                |     |          |           | o |   |                 |
|                |     |          |           | v |   |                 |
|                |     |          |           | e |   |                 |
|                |     |          |           | d |   |                 |
|                |     |          |           | * |   |                 |
|                |     |          |           |   |   |                 |
|                |     |          |           | * |   |                 |
|                |     |          |           | O |   |                 |
|                |     |          |           | M |   |                 |
|                |     |          |           | B |   |                 |
|                |     |          |           | N |   |                 |
|                |     |          |           | o |   |                 |
|                |     |          |           | . |   |                 |
|                |     |          |           | 0 |   |                 |
|                |     |          |           | 7 |   |                 |
|                |     |          |           | 0 |   |                 |
|                |     |          |           | 4 |   |                 |
|                |     |          |           | - |   |                 |
|                |     |          |           | 0 |   |                 |
|                |     |          |           | 1 |   |                 |
|                |     |          |           | 8 |   |                 |
|                |     |          |           | 8 |   |                 |
|                |     |          |           | * |   |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
| Public         |     |          |           |   |   |                 |
| reporting      |     |          |           |   |   |                 |
| burden for     |     |          |           |   |   |                 |
| this           |     |          |           |   |   |                 |
| collection of  |     |          |           |   |   |                 |
| information is |     |          |           |   |   |                 |
| estimated to   |     |          |           |   |   |                 |
| average 1 hour |     |          |           |   |   |                 |
| per response,  |     |          |           |   |   |                 |
| including the  |     |          |           |   |   |                 |
| time for       |     |          |           |   |   |                 |
| reviewing      |     |          |           |   |   |                 |
| instructions,  |     |          |           |   |   |                 |
| searching      |     |          |           |   |   |                 |
| existing data  |     |          |           |   |   |                 |
| sources,       |     |          |           |   |   |                 |
| gathering and  |     |          |           |   |   |                 |
| maintaining    |     |          |           |   |   |                 |
| the data       |     |          |           |   |   |                 |
| needed, and    |     |          |           |   |   |                 |
| completing and |     |          |           |   |   |                 |
| reviewing the  |     |          |           |   |   |                 |
| collection of  |     |          |           |   |   |                 |
| information.   |     |          |           |   |   |                 |
| Send comments  |     |          |           |   |   |                 |
| regarding this |     |          |           |   |   |                 |
| burden         |     |          |           |   |   |                 |
| estimate or    |     |          |           |   |   |                 |
| any other      |     |          |           |   |   |                 |
| aspect of this |     |          |           |   |   |                 |
| collection of  |     |          |           |   |   |                 |
| information,   |     |          |           |   |   |                 |
| including      |     |          |           |   |   |                 |
| suggestions    |     |          |           |   |   |                 |
| for reducing   |     |          |           |   |   |                 |
| this burden,   |     |          |           |   |   |                 |
| to Washington  |     |          |           |   |   |                 |
| Headquarters   |     |          |           |   |   |                 |
| Services,      |     |          |           |   |   |                 |
| Directorate    |     |          |           |   |   |                 |
| for            |     |          |           |   |   |                 |
| Information    |     |          |           |   |   |                 |
| Operations and |     |          |           |   |   |                 |
| Reports, 1215  |     |          |           |   |   |                 |
| Jefferson      |     |          |           |   |   |                 |
| Davis Highway, |     |          |           |   |   |                 |
| Suite 1204,    |     |          |           |   |   |                 |
| Arlington, VA  |     |          |           |   |   |                 |
| 22202-4302,    |     |          |           |   |   |                 |
| and to the     |     |          |           |   |   |                 |
| Office of      |     |          |           |   |   |                 |
| Management and |     |          |           |   |   |                 |
| Budget,        |     |          |           |   |   |                 |
| Paperwork      |     |          |           |   |   |                 |
| Reduction      |     |          |           |   |   |                 |
| Project        |     |          |           |   |   |                 |
| (0704-0188),   |     |          |           |   |   |                 |
| Washington, DC |     |          |           |   |   |                 |
| 20503.         |     |          |           |   |   |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
| 1\. AGENCY USE |     | 2\.      |           | 3 |   |                 |
| ONLY (Leave    |     | REPORT   |           | \ |   |                 |
| blank)         |     | DATE     |           | . |   |                 |
|                |     |          |           | R |   |                 |
|                |     | December |           | E |   |                 |
|                |     | 2004     |           | P |   |                 |
|                |     |          |           | O |   |                 |
|                |     |          |           | R |   |                 |
|                |     |          |           | T |   |                 |
|                |     |          |           | T |   |                 |
|                |     |          |           | Y |   |                 |
|                |     |          |           | P |   |                 |
|                |     |          |           | E |   |                 |
|                |     |          |           | A |   |                 |
|                |     |          |           | N |   |                 |
|                |     |          |           | D |   |                 |
|                |     |          |           | D |   |                 |
|                |     |          |           | A |   |                 |
|                |     |          |           | T |   |                 |
|                |     |          |           | E |   |                 |
|                |     |          |           | S |   |                 |
|                |     |          |           | C |   |                 |
|                |     |          |           | O |   |                 |
|                |     |          |           | V |   |                 |
|                |     |          |           | E |   |                 |
|                |     |          |           | R |   |                 |
|                |     |          |           | E |   |                 |
|                |     |          |           | D |   |                 |
|                |     |          |           |   |   |                 |
|                |     |          |           | F |   |                 |
|                |     |          |           | i |   |                 |
|                |     |          |           | n |   |                 |
|                |     |          |           | a |   |                 |
|                |     |          |           | l |   |                 |
|                |     |          |           | R |   |                 |
|                |     |          |           | e |   |                 |
|                |     |          |           | s |   |                 |
|                |     |          |           | e |   |                 |
|                |     |          |           | a |   |                 |
|                |     |          |           | r |   |                 |
|                |     |          |           | c |   |                 |
|                |     |          |           | h |   |                 |
|                |     |          |           | R |   |                 |
|                |     |          |           | e |   |                 |
|                |     |          |           | p |   |                 |
|                |     |          |           | o |   |                 |
|                |     |          |           | r |   |                 |
|                |     |          |           | t |   |                 |
|                |     |          |           |   |   |                 |
|                |     |          |           | A |   |                 |
|                |     |          |           | p |   |                 |
|                |     |          |           | r |   |                 |
|                |     |          |           | i |   |                 |
|                |     |          |           | l |   |                 |
|                |     |          |           | 2 |   |                 |
|                |     |          |           | 0 |   |                 |
|                |     |          |           | 0 |   |                 |
|                |     |          |           | 2 |   |                 |
|                |     |          |           | - |   |                 |
|                |     |          |           | - |   |                 |
|                |     |          |           | M |   |                 |
|                |     |          |           | a |   |                 |
|                |     |          |           | r |   |                 |
|                |     |          |           | c |   |                 |
|                |     |          |           | h |   |                 |
|                |     |          |           | 2 |   |                 |
|                |     |          |           | 0 |   |                 |
|                |     |          |           | 0 |   |                 |
|                |     |          |           | 4 |   |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
| 4\. TITLE AND  |     |          |           |   | 5 |                 |
| SUBTITLE       |     |          |           |   | \ |                 |
|                |     |          |           |   | . |                 |
| Integrated     |     |          |           |   | F |                 |
| Collision      |     |          |           |   | U |                 |
| Warning System |     |          |           |   | N |                 |
| Final          |     |          |           |   | D |                 |
| Technical      |     |          |           |   | I |                 |
| Report         |     |          |           |   | N |                 |
|                |     |          |           |   | G |                 |
|                |     |          |           |   | N |                 |
|                |     |          |           |   | U |                 |
|                |     |          |           |   | M |                 |
|                |     |          |           |   | B |                 |
|                |     |          |           |   | E |                 |
|                |     |          |           |   | R |                 |
|                |     |          |           |   | S |                 |
|                |     |          |           |   |   |                 |
|                |     |          |           |   | F |                 |
|                |     |          |           |   | e |                 |
|                |     |          |           |   | d |                 |
|                |     |          |           |   | e |                 |
|                |     |          |           |   | r |                 |
|                |     |          |           |   | a |                 |
|                |     |          |           |   | l |                 |
|                |     |          |           |   | I |                 |
|                |     |          |           |   | D |                 |
|                |     |          |           |   | \ |                 |
|                |     |          |           |   | # |                 |
|                |     |          |           |   | 2 |                 |
|                |     |          |           |   | 5 |                 |
|                |     |          |           |   | 0 |                 |
|                |     |          |           |   | 9 |                 |
|                |     |          |           |   | 6 |                 |
|                |     |          |           |   | 9 |                 |
|                |     |          |           |   | 4 |                 |
|                |     |          |           |   | 4 |                 |
|                |     |          |           |   | 9 |                 |
|                |     |          |           |   | 0 |                 |
|                |     |          |           |   | 0 |                 |
|                |     |          |           |   | 0 |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
| 6\. AUTHOR(S)  |     |          |           |   |   |                 |
|                |     |          |           |   |   |                 |
| University of  |     |          |           |   |   |                 |
| California     |     |          |           |   |   |                 |
| PATH, Carnegie |     |          |           |   |   |                 |
| Mellon         |     |          |           |   |   |                 |
| University     |     |          |           |   |   |                 |
| Robotics       |     |          |           |   |   |                 |
| Institute      |     |          |           |   |   |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
| 7\. PERFORMING |     |          |           |   | 8 |                 |
| ORGANIZATION   |     |          |           |   | \ |                 |
| NAME(S) AND    |     |          |           |   | . |                 |
| ADDRESS(ES)    |     |          |           |   | P |                 |
|                |     |          |           |   | E |                 |
| University of  |     |          |           |   | R |                 |
| California     |     |          |           |   | F |                 |
| PATH Carnegie  |     |          |           |   | O |                 |
| Mellon         |     |          |           |   | R |                 |
| University     |     |          |           |   | M |                 |
| Robotics       |     |          |           |   | I |                 |
| Institute      |     |          |           |   | N |                 |
|                |     |          |           |   | G |                 |
| 5000 Forbes    |     |          |           |   | O |                 |
| Ave            |     |          |           |   | R |                 |
|                |     |          |           |   | G |                 |
| Pittsburgh, PA |     |          |           |   | A |                 |
| 15213          |     |          |           |   | N |                 |
|                |     |          |           |   | I |                 |
|                |     |          |           |   | Z |                 |
|                |     |          |           |   | A |                 |
|                |     |          |           |   | T |                 |
|                |     |          |           |   | I |                 |
|                |     |          |           |   | O |                 |
|                |     |          |           |   | N |                 |
|                |     |          |           |   | R |                 |
|                |     |          |           |   | E |                 |
|                |     |          |           |   | P |                 |
|                |     |          |           |   | O |                 |
|                |     |          |           |   | R |                 |
|                |     |          |           |   | T |                 |
|                |     |          |           |   | N |                 |
|                |     |          |           |   | U |                 |
|                |     |          |           |   | M |                 |
|                |     |          |           |   | B |                 |
|                |     |          |           |   | E |                 |
|                |     |          |           |   | R |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
| 9\.            |     |          |           |   | 1 |                 |
| SPONSOR        |     |          |           |   | 0 |                 |
| ING/MONITORING |     |          |           |   | \ |                 |
| AGENCY NAME(S) |     |          |           |   | . |                 |
| AND            |     |          |           |   | S |                 |
| ADDRESS(ES)    |     |          |           |   | P |                 |
|                |     |          |           |   | O |                 |
| U.S.           |     |          |           |   | N |                 |
| Department of  |     |          |           |   | S |                 |
| T              |     |          |           |   | O |                 |
| ransportation, |     |          |           |   | R |                 |
| Federal        |     |          |           |   | I |                 |
| Transit        |     |          |           |   | N |                 |
| Administration |     |          |           |   | G |                 |
|                |     |          |           |   | / |                 |
| 400 Seventh    |     |          |           |   | M |                 |
| Street, S. W.  |     |          |           |   | O |                 |
|                |     |          |           |   | N |                 |
| Washington, D. |     |          |           |   | I |                 |
| C. 20590       |     |          |           |   | T |                 |
|                |     |          |           |   | O |                 |
|                |     |          |           |   | R |                 |
|                |     |          |           |   | I |                 |
|                |     |          |           |   | N |                 |
|                |     |          |           |   | G |                 |
|                |     |          |           |   | \ |                 |
|                |     |          |           |   | A |                 |
|                |     |          |           |   | G |                 |
|                |     |          |           |   | E |                 |
|                |     |          |           |   | N |                 |
|                |     |          |           |   | C |                 |
|                |     |          |           |   | Y |                 |
|                |     |          |           |   | R |                 |
|                |     |          |           |   | E |                 |
|                |     |          |           |   | P |                 |
|                |     |          |           |   | O |                 |
|                |     |          |           |   | R |                 |
|                |     |          |           |   | T |                 |
|                |     |          |           |   | N |                 |
|                |     |          |           |   | U |                 |
|                |     |          |           |   | M |                 |
|                |     |          |           |   | B |                 |
|                |     |          |           |   | E |                 |
|                |     |          |           |   | R |                 |
|                |     |          |           |   |   |                 |
|                |     |          |           |   | F |                 |
|                |     |          |           |   | T |                 |
|                |     |          |           |   | A |                 |
|                |     |          |           |   | - |                 |
|                |     |          |           |   | P |                 |
|                |     |          |           |   | A |                 |
|                |     |          |           |   | - |                 |
|                |     |          |           |   | 2 |                 |
|                |     |          |           |   | 6 |                 |
|                |     |          |           |   | - |                 |
|                |     |          |           |   | 7 |                 |
|                |     |          |           |   | 0 |                 |
|                |     |          |           |   | 0 |                 |
|                |     |          |           |   | 6 |                 |
|                |     |          |           |   | - |                 |
|                |     |          |           |   | 0 |                 |
|                |     |          |           |   | 4 |                 |
|                |     |          |           |   | . |                 |
|                |     |          |           |   | 1 |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
| 11\.           |     |          |           |   |   |                 |
| SUPPLEMENTARY  |     |          |           |   |   |                 |
| NOTES          |     |          |           |   |   |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
| 12a.           |     |          |           |   | 1 |                 |
| DISTRIBUTIO    |     |          |           |   | 2 |                 |
| N/AVAILABILITY |     |          |           |   | b |                 |
| STATEMENT \--  |     |          |           |   | . |                 |
| Available from |     |          |           |   | D |                 |
| the National   |     |          |           |   | I |                 |
| Technical      |     |          |           |   | S |                 |
| Information    |     |          |           |   | T |                 |
| Service/NTIS,  |     |          |           |   | R |                 |
| Springfield,   |     |          |           |   | I |                 |
| Virginia,      |     |          |           |   | B |                 |
| 22161. Phone   |     |          |           |   | U |                 |
| 703.605.6000,  |     |          |           |   | T |                 |
| Fax            |     |          |           |   | I |                 |
| 703.605.6900,  |     |          |           |   | O |                 |
| Email          |     |          |           |   | N |                 |
| \[orders@ntis. |     |          |           |   | C |                 |
| fedworld.gov\] |     |          |           |   | O |                 |
|                |     |          |           |   | D |                 |
|                |     |          |           |   | E |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
| 13\. ABSTRACT  |     |          |           |   |   |                 |
| (Maximum 200   |     |          |           |   |   |                 |
| words)         |     |          |           |   |   |                 |
|                |     |          |           |   |   |                 |
| Based on the   |     |          |           |   |   |                 |
| foundation of  |     |          |           |   |   |                 |
| the frontal    |     |          |           |   |   |                 |
| and side       |     |          |           |   |   |                 |
| collision      |     |          |           |   |   |                 |
| warning        |     |          |           |   |   |                 |
| systems, the   |     |          |           |   |   |                 |
| Frontal        |     |          |           |   |   |                 |
| Collision      |     |          |           |   |   |                 |
| Warning System |     |          |           |   |   |                 |
| (FCWS) and     |     |          |           |   |   |                 |
| Side Collision |     |          |           |   |   |                 |
| Warning System |     |          |           |   |   |                 |
| (SCWS) teams   |     |          |           |   |   |                 |
| joined efforts |     |          |           |   |   |                 |
| to improve the |     |          |           |   |   |                 |
| collision      |     |          |           |   |   |                 |
| warning        |     |          |           |   |   |                 |
| algorithms.    |     |          |           |   |   |                 |
| The objective  |     |          |           |   |   |                 |
| of the ICWS    |     |          |           |   |   |                 |
| Program is to  |     |          |           |   |   |                 |
| study how      |     |          |           |   |   |                 |
| frontal and    |     |          |           |   |   |                 |
| side collision |     |          |           |   |   |                 |
| warning system |     |          |           |   |   |                 |
| might          |     |          |           |   |   |                 |
| interface with |     |          |           |   |   |                 |
| each other,    |     |          |           |   |   |                 |
| and to develop |     |          |           |   |   |                 |
| prototype ICWS |     |          |           |   |   |                 |
| systems on two |     |          |           |   |   |                 |
| buses, one at  |     |          |           |   |   |                 |
| Samtrans and   |     |          |           |   |   |                 |
| the other at   |     |          |           |   |   |                 |
| PAT. The       |     |          |           |   |   |                 |
| prototype ICWS |     |          |           |   |   |                 |
| buses have     |     |          |           |   |   |                 |
| been in        |     |          |           |   |   |                 |
| revenue        |     |          |           |   |   |                 |
| operation in   |     |          |           |   |   |                 |
| the Bay\       |     |          |           |   |   |                 |
| Area and       |     |          |           |   |   |                 |
| Pittsburgh to  |     |          |           |   |   |                 |
| collect field  |     |          |           |   |   |                 |
| operational    |     |          |           |   |   |                 |
| data and       |     |          |           |   |   |                 |
| driver         |     |          |           |   |   |                 |
| responses. The |     |          |           |   |   |                 |
| results of the |     |          |           |   |   |                 |
| ICWS design,   |     |          |           |   |   |                 |
| build, and     |     |          |           |   |   |                 |
| integration    |     |          |           |   |   |                 |
| efforts as     |     |          |           |   |   |                 |
| well as an     |     |          |           |   |   |                 |
| analysis of    |     |          |           |   |   |                 |
| early data     |     |          |           |   |   |                 |
| collections to |     |          |           |   |   |                 |
| evolve the     |     |          |           |   |   |                 |
| warning        |     |          |           |   |   |                 |
| algorithms are |     |          |           |   |   |                 |
| documented in  |     |          |           |   |   |                 |
| this final     |     |          |           |   |   |                 |
| technical      |     |          |           |   |   |                 |
| report.        |     |          |           |   |   |                 |
| Evaluation and |     |          |           |   |   |                 |
| performance    |     |          |           |   |   |                 |
| analysis are   |     |          |           |   |   |                 |
| currently      |     |          |           |   |   |                 |
| being          |     |          |           |   |   |                 |
| finalized and  |     |          |           |   |   |                 |
| will be issued |     |          |           |   |   |                 |
| in a separate  |     |          |           |   |   |                 |
| report.        |     |          |           |   |   |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
| 14\. SUBJECT   |     |          |           |   |   | 15\. NUMBER OF  |
| TERMS          |     |          |           |   |   | PAGES           |
|                |     |          |           |   |   |                 |
| ICWS,          |     |          |           |   |   |                 |
| Collision      |     |          |           |   |   |                 |
| Warning,       |     |          |           |   |   |                 |
| Transit bus    |     |          |           |   |   |                 |
| safety         |     |          |           |   |   |                 |
+----------------+-----+----------+-----------+---+---+-----------------+
|                |     |          |           |   |   | 16\. PRICE CODE |
+----------------+-----+----------+-----------+---+---+-----------------+
| 17\. SECURITY  | 1   |          | 19\.      |   |   | 20\. LIMITATION |
| C              | 8\. |          | SECURITY  |   |   | OF ABSTRACT     |
| LASSIFICATION\ | SE  |          | CLASSI    |   |   |                 |
| OF REPORT      | CUR |          | FICATION\ |   |   |                 |
|                | ITY |          | OF        |   |   |                 |
| Unclassified   | CLA |          | ABSTRACT  |   |   |                 |
|                | SSI |          |           |   |   |                 |
|                | FIC |          | Unc       |   |   |                 |
|                | ATI |          | lassified |   |   |                 |
|                | ON\ |          |           |   |   |                 |
|                | OF  |          |           |   |   |                 |
|                | T   |          |           |   |   |                 |
|                | HIS |          |           |   |   |                 |
|                | P   |          |           |   |   |                 |
|                | AGE |          |           |   |   |                 |
|                |     |          |           |   |   |                 |
|                | Unc |          |           |   |   |                 |
|                | las |          |           |   |   |                 |
|                | sif |          |           |   |   |                 |
|                | ied |          |           |   |   |                 |
+----------------+-----+----------+-----------+---+---+-----------------+

> NSN 7540-01-280-5500 Standard Form 298 (Rev. 2-89)

Prescribed by ANSI Std. 239-18298-102

**Integrated Collision Warning System**

**Final Technical Report (Final Draft)**

December 2004

*Prepared by:*

+-----------------------------------+-----------------------------------+
| *Carnegie Mellon University*      | *University of California at      |
|                                   | Berkeley*                         |
| *Robotics Institute*              |                                   |
|                                   | *PATH Program*                    |
| *5000 Forbes Ave*                 |                                   |
|                                   | *1357 South 46^th^ Street*        |
| *Pittsburgh, PA 15213*            |                                   |
|                                   | *Richmond, CA 94804*              |
+-----------------------------------+-----------------------------------+

Prepared for:

*U.S. Department of Transportation*

*Federal Transit Administration*

*Washington, DC 20590*

Report Number:

FTA-PA-26-7006-04.1

**Disclaimer Notice**

[**List of Figures** i](#__RefHeading___Toc97001779)

[**List of Tables** i](#__RefHeading___Toc97001780)

[**Acknowledgements** i](#__RefHeading___Toc97001781)

[**Executive Summary** ii](#__RefHeading___Toc97001781)

[**1** **Introduction** 9](#introduction)

[**1.1.** **Background** 9](#background)

[**1.2.** **Scope** 10](#scope)

[1.2.1 Transit bus and object sensing
11](#transit-bus-and-object-sensing)

[1.2.2 ICWS Algorithms - Signal and data processing
12](#icws-algorithms---signal-and-data-processing)

[1.2.3 Driver-vehicle interface (DVI) 13](#driver-vehicle-interface-dvi)

[**1.3.** **Organization of Content** 14](#organization-of-content)

[**2** **Integrated Collision Warning System**
15](#integrated-collision-warning-system)

[**2.1.** **System Description** 15](#system-description)

[**2.2.** **Integrated ICWS** 17](#integrated-icws)

[**2.3.** **Sensing Needs** 19](#sensing-needs)

[**3** **System Overview** 20](#system-overview)

[**3.1.** **FCWS System Overview** 20](#fcws-system-overview)

[3.1.1 The goals of the FCWS 20](#the-goals-of-the-fcws)

[3.1.2 FCWS functions 20](#fcws-functions)

[3.1.3 FCWS system hardware 22](#fcws-system-hardware)

[3.1.4 FCWS system algorithms 22](#fcws-system-algorithms)

[**3.2.** **SCWS System Overview** 27](#scws-system-overview)

[3.2.1 SCWS data acquisition and communication
28](#scws-data-acquisition-and-communication)

[**4** **Hardware Development** 30](#hardware-development)

[**4.1.** **FCWS Obstacle Detection Sensors**
30](#fcws-obstacle-detection-sensors)

[4.1.1 FCWS obstacle sensors 30](#fcws-obstacle-sensors)

[4.1.2 FCWS Host-bus sensors 33](#fcws-host-bus-sensors)

[4.1.3 FCWS Battery / Ignition monitoring and shutdown circuitry
34](#fcws-battery-ignition-monitoring-and-shutdown-circuitry)

[**4.2.** **SCWS Side Sensors** 35](#scws-side-sensors)

[4.2.1 SCWS Laser Scanner 35](#scws-laser-scanner)

[4.2.2 Laser scanner retraction system
36](#laser-scanner-retraction-system)

[**4.3.** **SCWS Curb Detector** 37](#scws-curb-detector)

[4.3.1 Sensor fusion of curb detector, bus state and video
41](#sensor-fusion-of-curb-detector-bus-state-and-video)

[**4.4.** **PC-104 Platforms** 47](#pc-104-platforms)

[4.4.1 SCWS PC-104 platforms 47](#scws-pc-104-platforms)

[4.4.2 FCWS PC-104 platforms 48](#fcws-pc-104-platforms)

[**4.5.** **Digital Video Recorder PC-104 Platforms**
50](#__RefHeading___Toc97001814)

[4.5.1 SCWS Digital Video Recorder PC-104 platform
50](#scws-digital-video-recorder-pc-104-platform)

[4.5.2 FCWS Digital Video Recorder PC-104 platforms
51](#fcws-digital-video-recorder-pc-104-platforms)

[4.5.3 SCWS timing synchronization 52](#scws-timing-synchronization)

[4.5.4 FCWS timing synchronization 53](#fcws-timing-synchronization)

[4.5.5 FCWS / SCWS data synchronization
54](#fcws-scws-data-synchronization)

[4.5.6 FCWS / SCWS data protocol 54](#fcws-scws-data-protocol)

[**5** **System Software** 58](#system-software)

[**5.1.** **SCWS Software Architecture Development**
58](#scws-software-architecture-development)

[5.1.1 Inter-process communications 58](#inter-process-communications)

[5.1.2 Vehicle state propagation 60](#vehicle-state-propagation)

[5.1.3 Data flow 62](#data-flow)

[5.1.4 Integration with the FCWS 66](#integration-with-the-fcws)

[**5.2.** **FCWS Software Introduction**
66](#fcws-software-introduction)

[5.2.1 FCWS Software structure 68](#fcws-software-structure)

[5.2.2 FCWS Initialization 69](#fcws-initialization)

[5.2.3 FCWS Loop body 72](#fcws-loop-body)

[5.2.4 FCWS Synchronization 76](#fcws-synchronization)

[5.2.5 FCWS Program exit 76](#fcws-program-exit)

[**6** **Algorithm Development** 77](#algorithm-development)

[**6.1.** **Object Tracking Using Scanning Laser Rangefinders**
77](#object-tracking-using-scanning-laser-rangefinders)

[6.1.1 Input / Output example 77](#input-output-example)

[6.1.2 Sensor characteristics 78](#sensor-characteristics)

[6.1.3 The tracking problem 80](#the-tracking-problem)

[6.1.4 Tracker structure and algorithms
86](#tracker-structure-and-algorithms)

[6.1.5 Evaluation 107](#evaluation)

[6.1.6 Summary 109](#summary)

[**6.2.** **FCWS Warning Algorithm** 109](#fcws-warning-algorithm)

[6.2.1 FCWS Algorithm structure 111](#fcws-algorithm-structure)

[6.2.2 FCWS Data structure 112](#fcws-data-structure)

[6.2.3 FCWS Tracking algorithm 118](#__RefHeading___Toc97001844)

[6.2.4 FCWS Host vehicle state estimation
126](#__RefHeading___Toc97001845)

[6.2.5 FCWS Motion decoupling 129](#__RefHeading___Toc97001846)

[6.2.6 FCWS Target state estimation 131](#__RefHeading___Toc97001847)

[6.2.7 FCWS Threat assessment 135](#__RefHeading___Toc97001848)

[6.2.8 Warning signal generation 136](#__RefHeading___Toc97001849)

[6.2.9 FCWS Further improvement 137](#__RefHeading___Toc97001850)

[6.2.10 FCWS Suggestions 143](#__RefHeading___Toc97001851)

[6.2.11 FCWS Summary 143](#__RefHeading___Toc97001852)

[**6.3.** **SCWS Warning algorithm** 145](#__RefHeading___Toc97001853)

[6.3.1 Under-bus warning 146](#__RefHeading___Toc97001854)

[6.3.2 Notification that a collision occurred
146](#__RefHeading___Toc97001855)

[6.3.3 Frequency of alarms 147](#__RefHeading___Toc97001856)

[**6.4.** **False Alarms** 148](#__RefHeading___Toc97001857)

[6.4.1 Sources of false positive alarms
149](#__RefHeading___Toc97001858)

[6.4.2 Statistics of false positive alarms
151](#__RefHeading___Toc97001859)

[6.4.3 Sources of false negative alarms
152](#__RefHeading___Toc97001860)

[6.4.4 Reduction of nuisance alarms through curb detection
152](#__RefHeading___Toc97001861)

[**6.5.** **System Faults and Recovery**
152](#__RefHeading___Toc97001862)

[6.5.1 SCWS System faults and recovery 152](#__RefHeading___Toc97001863)

[6.5.2 FCWS System faults and recovery 154](#__RefHeading___Toc97001864)

[6.5.3 FCWS Faults categorization 155](#__RefHeading___Toc97001865)

[6.5.4 FCWS Fault detection 156](#__RefHeading___Toc97001866)

[6.5.5 FCWS Faults reporting and system recovery
164](#__RefHeading___Toc97001867)

[6.5.6 FCWS Summary 165](#__RefHeading___Toc97001868)

[**6.6.** **FCWS Simulation Playback Tools**
167](#__RefHeading___Toc97001869)

[6.6.1 The FCWS Data playback tool 167](#__RefHeading___Toc97001870)

[6.6.2 The FCWS Simulator Tool 168](#__RefHeading___Toc97001871)

[6.6.3 The FCWS Video Data Marking Tool
170](#__RefHeading___Toc97001872)

[6.6.4 FCWS Analysis Procedure 174](#__RefHeading___Toc97001873)

[**6.7.** 175](#__RefHeading___Toc97001874)

[**6.8.** **SCWS Data replay tools** 175](#__RefHeading___Toc97001875)

[**7** **DVI Development** 181](#__RefHeading___Toc97001876)

[**7.1.** **Background: Transit Collision Warning Nuances**
181](#__RefHeading___Toc97001877)

[**7.2.** **Guiding Concepts** 181](#__RefHeading___Toc97001878)

[**7.3.** **Warning Design** 183](#__RefHeading___Toc97001879)

[**7.4.** **Interface Design and Placement**
184](#__RefHeading___Toc97001880)

[**7.5.** **Examples of DVI Behavior** 186](#__RefHeading___Toc97001881)

[**7.6.** **Plans for DVI Evaluation** 190](#__RefHeading___Toc97001882)

[**8** **Data Analysis and Evaluation**
192](#__RefHeading___Toc97001883)

[**8.1.** **FCWS Data Analysis** 192](#__RefHeading___Toc97001884)

[8.1.1 FCWS Three-Step Quantitive Approach
193](#__RefHeading___Toc97001885)

[8.1.2 FCWS Warning scenarios categorization
193](#__RefHeading___Toc97001886)

[8.1.3 FCWS Summary 203](#__RefHeading___Toc97001887)

[**8.2.** **SCWS Data Analysis** 203](#__RefHeading___Toc97001888)

[8.2.1 Driver behavior analysis 203](#__RefHeading___Toc97001889)

[8.2.2 System debugging and development
208](#__RefHeading___Toc97001890)

[**9** **Calibration and Testing** 209](#__RefHeading___Toc97001891)

[**9.1.** **SICK Laser Scanner** 209](#__RefHeading___Toc97001892)

[9.1.1 SICK resolution and accuracy 209](#__RefHeading___Toc97001893)

[9.1.2 Definition of terms 209](#__RefHeading___Toc97001894)

[9.1.3 Error characterization 210](#__RefHeading___Toc97001895)

[9.1.4 Experimental confirmation of resolution
210](#__RefHeading___Toc97001896)

[9.1.5 Experimental confirmation of accuracy
212](#__RefHeading___Toc97001897)

[9.1.6 Summary 213](#__RefHeading___Toc97001898)

[**9.2.** **Calibration of Scanner Position and Orientation**
213](#__RefHeading___Toc97001899)

[9.2.1 Calibration by overlay 214](#__RefHeading___Toc97001900)

[9.2.2 Calibration by residual speed of fixed objects
214](#__RefHeading___Toc97001901)

[**9.3.** **Automatic External Calibration of a Laser Scanner**
214](#__RefHeading___Toc97001902)

[9.3.1 Calibration approach 215](#__RefHeading___Toc97001903)

[9.3.2 Example implementation 217](#__RefHeading___Toc97001904)

[9.3.3 Special case: bicycle model 219](#__RefHeading___Toc97001905)

[9.3.4 Extracting the best value from a distribution
220](#__RefHeading___Toc97001906)

[**9.4.** **Accuracy of Velocities Measured by DATMO**
222](#__RefHeading___Toc97001907)

[9.4.1 General test procedure 222](#__RefHeading___Toc97001908)

[9.4.2 Quantitative results of line-to-line matching
229](#__RefHeading___Toc97001909)

[**9.5.** **Quantitative Evaluation and Testing of FCWS**
236](#__RefHeading___Toc97001910)

[9.5.1 Test Objectives 237](#__RefHeading___Toc97001911)

[9.5.2 Considerations for Designing the Tests
238](#__RefHeading___Toc97001912)

[9.5.3 Hardware and Software Setup 238](#__RefHeading___Toc97001913)

[9.5.4 Known Driving Environment 240](#__RefHeading___Toc97001914)

[9.5.5 Preliminary Test 243](#__RefHeading___Toc97001915)

[9.5.6 Crows Landing Test 244](#__RefHeading___Toc97001916)

[9.5.7 Data Analysis 251](#__RefHeading___Toc97001917)

[9.5.8 Future work 254](#__RefHeading___Toc97001918)

[**10** **Transit CWS Simulator** 255](#__RefHeading___Toc97001919)

[**10.1.** **The SamTrans simulator** 255](#__RefHeading___Toc97001920)

[**10.2.** **PATH CWS/FAAC Simulator Integration**
258](#__RefHeading___Toc97001921)

[**10.3.** **Summary** 261](#__RefHeading___Toc97001922)

[**11** **Recommendations** 262](#__RefHeading___Toc97001923)

[**11.1.** **Develop ICWS Markets and Industrial Partnerships**
262](#__RefHeading___Toc97001924)

[**11.2.** **Conduct Field Operational Tests**
262](#__RefHeading___Toc97001925)

[**11.3.** **Human Factor Studies Using Samtrans Driving Simulator**
263](#__RefHeading___Toc97001926)

[**11.4.** **Finalize Performance Specifications**
264](#__RefHeading___Toc97001927)

[**11.5.** **Hardware and Software integration of ICWS**
265](#__RefHeading___Toc97001928)

[11.5.1 Eliminate Duplication of Hardware
265](#__RefHeading___Toc97001929)

[11.5.2 Combine / Eliminate Processors 266](#__RefHeading___Toc97001930)

[11.5.3 Eliminate Video 266](#__RefHeading___Toc97001931)

[11.5.4 Commercialize Laser Scanners 267](#__RefHeading___Toc97001932)

[11.5.5 Integrate a Rear Collision Warning System
272](#__RefHeading___Toc97001933)

[11.5.6 Training 272](#__RefHeading___Toc97001934)

[**11.6.** **Areas for Future Research**
273](#__RefHeading___Toc97001935)

[11.6.1 Transit bus data 273](#__RefHeading___Toc97001936)

[11.6.2 Unify the FCWS and SCWS Tracking and Warning Algorithms
274](#__RefHeading___Toc97001937)

[11.6.3 Integrate ICWS with other electronic vehicle systems
274](#__RefHeading___Toc97001938)

[11.6.4 Improvements to the object tracking algorithms (DATMO)
275](#__RefHeading___Toc97001939)

[11.6.5 Improvements to FCWS warning algorithm
275](#__RefHeading___Toc97001940)

[11.6.6 Sensor Fusion 276](#__RefHeading___Toc97001941)

[11.6.7 Develop an under the bus sensor
276](#__RefHeading___Toc97001942)

[**Appendix A:** **Acronym Definitions**
278](#__RefHeading___Toc97001943)

[**Appendix B:** **Related Documents** 280](#__RefHeading___Toc97001944)

[**Appendix C:** **Published Papers** 282](#__RefHeading___Toc97001945)

[Appendix D: Conversion Tables 285](#__RefHeading___Toc97001946)

[]{#__RefHeading___Toc97001779 .anchor}List of Figures

[Figure 1 - Basic ICWS Algorithms 9](#_Ref25394492)

[Figure 2 Configuration of Integrated Collision Warning System
14](#__RefHeading___Toc96687033)

[Figure 3 System Architecture 14](#_Ref18924925)

[Figure 4 Three stages towards a commercial product
16](#__RefHeading___Toc96687035)

[Figure 5 Integrated system spatial coverage illustration
17](#__RefHeading___Toc96687036)

[Figure 6 Functions of frontal collision warning system
19](#__RefHeading___Toc96687037)

[Figure 7 FCWS system architecture 19](#__RefHeading___Toc96687038)

[Figure 8 JDL data fusion process model 21](#__RefHeading___Toc96687039)

[Figure 9 The architecture of the transit FCWS warning algorithm
25](#__RefHeading___Toc96687040)

[Figure 10 Layout of sensors, cameras and HMI
28](#__RefHeading___Toc96687041)

[Figure 11. Interface between the engineering computer and host-bus
sensors 32](#__RefHeading___Toc96687042)

[Figure 12. SICK Laser Scanner and Retraction Assembly
33](#__RefHeading___Toc96687043)

[Figure 13 Schematic of a laser line striper.
35](#__RefHeading___Toc96687044)

[Figure 14 A LLS mounted on a vehicle looking to the side at various
objects. 36](#__RefHeading___Toc96687045)

[Figure 15 LLS mounted in the front bumper of the bus.
37](#__RefHeading___Toc96687046)

[Figure 16 Profile of the road and curb observed by the laser line
striper. 38](#__RefHeading___Toc96687047)

[Figure 17 Curb alongside the bus 39](#__RefHeading___Toc96687048)

[Figure 18 Example of three points chosen for the calibration.
42](#__RefHeading___Toc96687049)

[Figure 19. FCWS System Architecture 47](#__RefHeading___Toc96687050)

[Figure 20. FCWS Computer enclosure on the bus (top view)
48](#__RefHeading___Toc96687051)

[Figure 21. FCWS Video recorder-camera interface
49](#__RefHeading___Toc96687052)

[Figure 22 FCWS Warning signal definition
51](#__RefHeading___Toc96687053)

[Figure 23 FCWS Data acquisition program
65](#__RefHeading___Toc96687054)

[Figure 24 FCWS Software flow chart 66](#__RefHeading___Toc96687055)

[Figure 25 FCWS RADAR file format 71](#__RefHeading___Toc96687056)

[Figure 26 FCWSr file format 72](#__RefHeading___Toc96687057)

[Figure 27 FCWS Host-bus sensor file format
73](#__RefHeading___Toc96687058)

[Figure 28 Check time to open a new set of files
74](#__RefHeading___Toc96687059)

[Figure 29: Tracker input (one frame) 75](#__RefHeading___Toc96687060)

[Figure 30: Tracker output example 76](#__RefHeading___Toc96687061)

[Figure 31: Scanner angular resolution 77](#__RefHeading___Toc96687062)

[Figure 32: Shape change 81](#__RefHeading___Toc96687063)

[Figure 33: Occlusion 82](#__RefHeading___Toc96687064)

[Figure 34: Vegetation 83](#__RefHeading___Toc96687065)

[Figure 35: Weak returns 84](#__RefHeading___Toc96687066)

[Figure 36: Corner fitting 88](#__RefHeading___Toc96687067)

[Figure 37: Features 90](#__RefHeading___Toc96687068)

[Figure 38: Incidence angle and point spacing
91](#__RefHeading___Toc96687069)

[Figure 39: Tracker dynamics 105](#__RefHeading___Toc96687070)

[Figure 40: Track history matching 106](#__RefHeading___Toc96687071)

[Figure 41 FCWS Algorithm structure 110](#__RefHeading___Toc96687072)

[Figure 42 FCWS Track file structure 111](#__RefHeading___Toc96687073)

[Figure 43 Linked list of tracks and historical data
113](#__RefHeading___Toc96687074)

[Figure 44 Non-holonomic bicycle model 125](#__RefHeading___Toc96687075)

[Figure 45 FCWS Warning scenario snap shot (without side recognition)
136](#__RefHeading___Toc96687076)

[Figure 46 FCWS Warning scenario snap shot (with side recognition)
137](#__RefHeading___Toc96687077)

[Figure 47 FCWS Strategy of side recognition
138](#__RefHeading___Toc96687078)

[Figure 48 Following distance constraint
138](#__RefHeading___Toc96687079)

[Figure 49 Following distance constraint (before and after)
139](#__RefHeading___Toc96687080)

[Figure 50 The creeping warning 140](#__RefHeading___Toc96687081)

[Figure 51 The trajectories of bus and object shown in world coordinate
frame . 143](#__RefHeading___Toc96687082)

[Figure 52 Probability of collision plotted versus time
144](#__RefHeading___Toc96687083)

[Figure 53 - Duration and frequency of SCWS warnings
146](#__RefHeading___Toc96687084)

[Figure 54 SCWS Status Debugging GUI 152](#__RefHeading___Toc96687085)

[Figure 55 FCWS Fault categorization 154](#__RefHeading___Toc96687086)

[Figure 56 Speed raw data 157](#__RefHeading___Toc96687087)

[Figure 57 Kalman filter residual 157](#__RefHeading___Toc96687088)

[Figure 58 Trace of the covariance matrix
158](#__RefHeading___Toc96687089)

[Figure 59 Speed raw data 159](#__RefHeading___Toc96687090)

[Figure 60 Kalman filter residual 159](#__RefHeading___Toc96687091)

[Figure 61 Trace of the covariance matrix
160](#__RefHeading___Toc96687092)

[Figure 62 Detect DVI fault 161](#__RefHeading___Toc96687093)

[Figure 63 Detection scheme 161](#__RefHeading___Toc96687094)

[Figure 64 FCWS Fault detection architecture
164](#__RefHeading___Toc96687095)

[Figure 65 The simulation tool 166](#__RefHeading___Toc96687096)

[Figure 66 The updated playback tool 168](#__RefHeading___Toc96687097)

[Figure 67 FCWS Video Data Marking Tool
169](#__RefHeading___Toc96687098)

[Figure 68 Mark tool sub-window 172](#__RefHeading___Toc96687099)

[Figure 69 Example 1 of Overhead View and Overlaid data on video
176](#__RefHeading___Toc96687100)

[Figure 70 Example 2 with bicycle 177](#__RefHeading___Toc96687101)

[Figure 71 Integrated DVI 183](#__RefHeading___Toc96687102)

[Figure 72 The DVI control box 184](#__RefHeading___Toc96687103)

[Figure 73 Component diagram of LED assemblies
189](#__RefHeading___Toc96687104)

[Figure 74 The goal of field data analysis
190](#__RefHeading___Toc96687105)

[Figure 75 Warning scenario snap shot 193](#__RefHeading___Toc96687106)

[Figure 76 Technical variables 194](#__RefHeading___Toc96687107)

[Figure 77 Target vehicle Decelerating 195](#__RefHeading___Toc96687108)

[Figure 78 Technical variables 196](#__RefHeading___Toc96687109)

[Figure 79 Warning scenario snap shot. 197](#__RefHeading___Toc96687110)

[Figure 80 Nuisance warning ends 198](#__RefHeading___Toc96687111)

[Figure 81 Trajectories of the bus and the targets around
199](#__RefHeading___Toc96687112)

[Figure 82 Problem: stationary objects along curved road
200](#__RefHeading___Toc96687113)

[Figure 83 Problem: Overhead obstacles 200](#__RefHeading___Toc96687114)

[Figure 84 Data flow for driver behavior analysis
202](#__RefHeading___Toc96687115)

[Figure 85 Fusion of new data from test algorithms with real data
206](#__RefHeading___Toc96687116)

[Figure 86 Distances to a straight object measured by the laser scanner
209](#__RefHeading___Toc96687117)

[Figure 87 Error comparisons for the laser scanner.
210](#__RefHeading___Toc96687118)

[Figure 88 Distance to the target measured by the SICK versus measured
by tape. 211](#__RefHeading___Toc96687119)

[Figure 89 Vehicle and sensor coordinate frames
214](#__RefHeading___Toc96687120)

[Figure 90 Moving vehicle and sensor coordinate frames
214](#__RefHeading___Toc96687121)

[Figure 91 Path and the velocities recorded by the vehicle and the
sensor in the fixed coordinate frame. 215](#__RefHeading___Toc96687122)

[Figure 92 Distributions of Δx, Δy, and φ.
218](#__RefHeading___Toc96687123)

[Figure 93 The left side shows the scans projected into a global
reference frame. 222](#__RefHeading___Toc96687124)

[Figure 94 Velocity in x and y direction and the speed determined by
tracking and by vehicle state. 223](#__RefHeading___Toc96687125)

[Figure 95 On the left side is a single laser scan segmented into
different objects. 224](#__RefHeading___Toc96687126)

[Figure 96 Velocity measurement of a stationary car passed by a bus.
225](#__RefHeading___Toc96687127)

[Figure 97 Same as Figure 96 but now for a situation where the bus is
turning left. 226](#__RefHeading___Toc96687128)

[Figure 98 Three consecutive scans, blue, red, and green.
227](#__RefHeading___Toc96687129)

[Figure 99 Velocity measurement of a stationary car passed by a bus
229](#__RefHeading___Toc96687130)

[Figure 100 Same as Figure 99 but for a situation which gives worse
error. 230](#__RefHeading___Toc96687131)

[Figure 101 Distribution of the error in velocity.
231](#__RefHeading___Toc96687132)

[Figure 102 Fifth wheel to measure true ground speed and string pot
237](#__RefHeading___Toc96687133)

[Figure 103 Using string port to detect true inter-vehicle distance
on-the-fly 238](#__RefHeading___Toc96687134)

[Figure 104 View of the Static Objects from the Bus
239](#__RefHeading___Toc96687135)

[Figure 105 Detecting parked vehicles on both sides
240](#__RefHeading___Toc96687136)

[Figure 106 Moving (front vehicle) and static objects
240](#__RefHeading___Toc96687137)

[Figure 107 A Ground Coordinate System 242](#__RefHeading___Toc96687138)

[Figure 108 No string for vehicle following
243](#__RefHeading___Toc96687139)

[Figure 109 String Pot and wireless communication are used
244](#__RefHeading___Toc96687140)

[Figure 110 Parked car testing scenario
245](#__RefHeading___Toc96687141)

[Figure 111 Park car door open test scenario
246](#__RefHeading___Toc96687142)

[Figure 112 Side Moving Target Direction
247](#__RefHeading___Toc96687143)

[Figure 113 Cut-in and cut-out to test lateral movement detection
248](#__RefHeading___Toc96687144)

[Figure 114 Crash Test; No string is used
249](#__RefHeading___Toc96687145)

[Figure 115 LIDAR/RADAR target lateral position measurement and
estimation \[m\] 250](#__RefHeading___Toc96687146)

[Figure 116 String (true) distance vs. LIDAR/RADAR distance estimation
\[m\] 251](#__RefHeading___Toc96687147)

[Figure 117 LIDAR/RADAR target speed estimation vs. fifth wheel \[m/s\]
251](#__RefHeading___Toc96687148)

[Figure 118 Simulator set-up from the back
254](#__RefHeading___Toc96687149)

[Figure 119 Trainer/Experimenter workstation
255](#__RefHeading___Toc96687150)

[Figure 120 Driver Seat with forward view
255](#__RefHeading___Toc96687151)

[Figure 121 Simulated view of the interior of the bus
256](#__RefHeading___Toc96687152)

[Figure 122 PATH simulator software architecture
257](#__RefHeading___Toc96687153)

[Figure 123 DVI light bar. 258](#__RefHeading___Toc96687154)

[]{#__RefHeading___Toc97001780 .anchor}List of Tables

[Table 1. JDL data fusion process model 22](#__RefHeading___Toc96686994)

[Table 2. Location and orientation of obstacle detection sensors
29](#__RefHeading___Toc96686995)

[Table 3. Locations of cameras 30](#__RefHeading___Toc96686996)

[Table 4. LIDAR specifications 30](#__RefHeading___Toc96686997)

[Table 5. RADAR specification 31](#__RefHeading___Toc96686998)

[Table 6. Range and Resolution of Laser Line Striper
36](#__RefHeading___Toc96686999)

[Table 7. Configuration of Left and Right SCWS Computers
45](#__RefHeading___Toc96687000)

[Table 8. Configuration of SCWS Digital Video Recorder
48](#__RefHeading___Toc96687001)

[Table 9. Video board specifications 50](#__RefHeading___Toc96687002)

[Table 10. Parameter Values 53](#__RefHeading___Toc96687003)

[Table 11. Data sent from the FCWS to the SCWS
54](#__RefHeading___Toc96687004)

[Table 12. Data sent from the SCWS to the FCWS
55](#__RefHeading___Toc96687005)

[Table 13. FCWS File pointers -- sensors
67](#__RefHeading___Toc96687006)

[Table 14. FCWS System signals 67](#__RefHeading___Toc96687007)

[Table 15. FCWS Database variables -- sensors
68](#__RefHeading___Toc96687008)

[Table 16. FCWS Sensor data pointers 69](#__RefHeading___Toc96687009)

[Table 17. FCWS File name format 70](#__RefHeading___Toc96687010)

[Table 18. Features and improvements of three generations of FCWS
algorithms 108](#__RefHeading___Toc96687011)

[Table 19. FCWS Host vehicle state variable allocation
115](#__RefHeading___Toc96687012)

[Table 20. FCWS Object state variable allocation
116](#__RefHeading___Toc96687013)

[Table 21. FCWS Sensitivity, threshold and Warning level
133](#__RefHeading___Toc96687014)

[Table 22. FCWS Warning display 135](#__RefHeading___Toc96687015)

[Table 23. SCWS Alarm Frequency 145](#__RefHeading___Toc96687016)

[Table 24. SCWS Alarm Duration 145](#__RefHeading___Toc96687017)

[Table 25. Standard analysis procedure and main variables
173](#__RefHeading___Toc96687018)

[Table 26. Mapping of DVI side subcomponents to warnings
186](#__RefHeading___Toc96687019)

[Table 27. Warning scenario category 192](#__RefHeading___Toc96687020)

[Table 28. Evaluation Metrics (MOE\'s) 205](#__RefHeading___Toc96687021)

[Table 29. Values for sensor orientation, Δx, and Δy
217](#__RefHeading___Toc96687022)

[Table 30. Standard deviations of three matching methods for a
stationary car 226](#__RefHeading___Toc96687023)

[Table 31. Standard deviations of three different matching methods
226](#__RefHeading___Toc96687024)

[Table 32. Line matching algorithm errors vs other methods
228](#__RefHeading___Toc96687025)

[Table 33. Errors from the three different methods
230](#__RefHeading___Toc96687026)

[Table 34 Objects and obstructions of
Interest](#__RefHeading___Toc96687027) 233

[]{#__RefHeading___Toc97001781 .anchor}Acknowledgements

This report presents the results of a research effort undertaken by the
Carnegie Mellon University Robotics Institute and the University of
California PATH Program under funding provided by the Federal Transit
Administration under Federal ID \# 250969449000. The direction of Brian
Cronin is gratefully acknowledged.

Special thanks are also due to the state transportation agencies for
providing additional funding and contractual assistance. Specifically,
the California Department of Transportation (Caltrans) and the
Pennsylvania Department of Transportation (PennDOT) were instrumental in
the progress of this work.

Also this work would not have been possible without the cooperation of
the local transit agencies. Specifically, the Port Authority of
Allegheny County (PAT) and the San Mateo County Transit District
(Samtrans)

We would also like to acknowledge the work of Clever Devices. We have
learned a lot and hopefully applied the lessons learned from their work
of designing and installing their obstacle detection systems on transit
buses.

The feedback of Eric Traube of Mitretek has also been very beneficial to
the effort of this research and evaluation program.

# Executive Summary {#executive-summary .unnumbered}

This final technical report documents technical developments conducted
under the Integrated Collision Warning System Program (ICWS). It is a
continuation of the development programs for the individual frontal and
side collision warning systems for transit buses. The goal of the ICWS
program is to integrate the advanced frontal and side collision warning
systems into a unified collision warning system. A single Driver Vehicle
Interface (DVI) is being developed that can effectively display warnings
from both frontal and side collision warning systems and signal the
driver in a manner that is effective in helping the driver avoid
crashes.

Vehicle collisions have been a significant concern for transit
operators. They not only result in property damage, service
interruptions and personal injuries, but also affect transit efficiency,
revenue and public perception.  In addition to collision damage,
passenger falls resulting from emergency maneuvers also contribute to an
increased potential for passenger injuries and liability. A transit
collision ripples through the agency and consumes additional resources
to settle claims and results in significant loss of good will. Transit
operators and industry stakeholders actively seek solutions to avoid
collisions and have recommended that studies be conducted under the US
DOT's Intelligent Vehicle Initiative (IVI) to develop transit collision
warning technologies. The primary goal of the Transit IVI program is to
develop technical and performance specifications for collision warning
systems which can identify hazards that may potentially lead to
collisions in complex urban environments and warn drivers accordingly.
Based on the recommendations, Federal Transit Administration initiated
the Transit IVI Program in 2000. As part of the Transit IVI Program,
substantial efforts were carried out to develop frontal and side
collision warning systems that can deal with the urban driving
environment.

The research efforts on Frontal Collision Warning Systems (FCWS) were
carried out by the San Mateo County Transit District (SamTrans),
University of California PATH Program (PATH), California Department of
Transportation (Caltrans), and Gillig Corporation. Most of the San
Francisco Bay Area transit agencies are participating in the project in
an advisory role and have provided significant inputs to the project.
The team conducted in-depth study of accident data for 35 transit
agencies. The team obtained a better understanding of the causes of
transit frontal collisions and the conditions in which crashes may
potentially occur through field testing and data collection using
instrumented buses. Human factors researchers also closely interacted
with SamTrans drivers to understand their needs and expectations. Based
on the accident data analysis and field data collection, the FCWS team
developed sensing schemes, obstacle detection and collision warning
algorithms and a DVI design. Prototype collision warning systems were
instrumented onto three Samtrans buses that include radar and lidar
sensors, obstacle detection and collision warning algorithms, and a DVI.
These prototype FCWS systems address imminent crashes and warning needs
for smoother maneuvering. As the final product, preliminary requirement
specifications were developed and experimentally verified through field
testing using the three buses equipped with the prototype warning
system.

The research efforts on Side Collision Warning Systems (SCWS) were
carried out by the Port Authority of Allegheny County (PAT), Carnegie
Mellon University Robotics Institute (CMU-RI), the Pennsylvania
Department of Transportation, and Clever Devices. Similar to the
research on FCWS, the side collision warning team has collected field
data to study the hazards on both side of the bus while the bus is in
motion and has developed approaches for tracking the movement of
vehicles and pedestrians using scanning laser rangefinders mounted on
the sides of the bus. While vehicle collision avoidance is an important
goal of SCWS, much of the emphasis of this study is placed upon
pedestrian detection by assessing the movement of pedestrians relative
to the sidewalk. A prototype side collision warning system was first
installed on a test vehicle platform and later on a PAT bus for field
experiments. Based on the test results, preliminary requirement
specifications for an SCWS were developed.

Based on the foundation of the frontal and side collision warning
systems, the FCWS and SCWS teams joined efforts to improve the collision
warning algorithms. The objective of the ICWS Program was to study how
frontal and side collision warning system might interface with each
other, and to develop prototype ICWS systems on two buses, one at
Samtrans and the other at PAT. The prototype ICWS buses have been in
revenue operation in the Bay Area and Pittsburgh to collect field
operational data and drivers' responses. Evaluation and performance
analysis are still being conducted.

This report mainly describes the following:

(1) ICWS introduction and overview.

As a driver assistance system, the primary goal of the ICWS is to
predict imminent potential crashes, or collisions with objects so that
it can warn the transit operator when necessary. To achieve this goal,
first of all, the system needs to be capable of gathering information
from both the subject vehicle and the surrounding environment (Transit
bus and object sensing and detection), it then needs to track the
objects (around both front and side), predicts their trajectories and
assesses the threat based on all knowledge available, finally, it needs
to be able to issue warnings to the operator via the Driver Vehicle
Interface. These functions are implemented by the system hardware,
software and algorithms.

(2) ICWS hardware, software and algorithm.

The ICWS hardware includes power adaptors, host-bus sensors, object
sensors, engineering computers, cameras, video recorders and DVI. Host
bus sensors measure bus speed, accelerations, yaw rate, brake pressure,
throttle position, windshield wiper status, back up light, turn signals,
GPS location, etc. Object sensors include frontal Lidars, two additional
Radar sensors used as alternative sensors in harsh weather, one curb
detector and two side laser scanners. Three PC104 engineering computers
are used for ICWS data acquisition, archiving and warning algorithm
generation. Recorders were developed to save the video streams from
cameras that are installed to provide different views around the bus.
These recorders are part of the research system and are not necessary
for the final system. The FCWS is connected with the SCWS using serial
ports. The system hardware provides the platform for system software and
application algorithms.

The ICWS computers are running QNX (FCWS) and Linux (SCWS). Although the
specific implementations are different, a "single-writer
multiple-reader" model is used as the basic protocol for inter-process
communications in the system software. The FCWS exchanges data with the
SCWS via a custom-built protocol. Built on the system hardware and
software are the ICWS application algorithms.

The ICWS algorithms include system modeling, object tracking and threat
assessment, system fault detection and recovery.

The biggest challenge for the FCWS is that buses usually serve in
urban/suburban environment where too many objects may trigger false
alarms. Hence it is a difficult problem to detect real imminent crashes
and give drivers timely warnings while suppressing excessive false
alarms. The third generation algorithm PATH developed for forward
collision warning has five unique features (1) Modeling moving targets
with non-holonomic constraints. (2) Taking into account the driver's
role in the system. (3) Eliminating Coriolis effect. (4) Suppressing
finite size object effect. (5) Using required deceleration as threat
measure. All these features address reducing the nuisance alarms to a
great extent as shown in the data analysis and field testing.

The SCWS uses the linear feature tracker combined with history-based
track validation and is able to generate reasonably accurate velocity
estimates for cars and pedestrians in a cluttered urban environment,
while giving a low rate of spurious motion indications that can cause
false alarms. The estimation of acceleration and turn rate appears to
improve prediction of future positions.

The ICWS has four categories of fault from the system point of view:
power fault, sensor fault, DVI fault, and engineering computer fault.
The practical fault detection algorithms and detection strategies are
proposed and system fault reporting and system recovery methods are
introduced.

(3) DVI development.

The DVI for the ICWS was an extension of the UC Berkeley PATH experience
over the previous two years on transit bus operation. The design of the
DVI took into consideration the characteristics of the bus design and
special needs for transit drivers. Many discussions with and feedback by
Foster Miller, Inc and members of the transit community (SamTrans, PATH
and a dozen of transit agencies in the Bay Area) were also used for the
current design. The current design of the DVI will be evaluated in
simulation by PATH and SamTrans as part of this program and evaluated by
transit operators. These results will be incorporated into the final
performance specifications for an ICWS.

(4) ICWS field testing, data analysis and system evaluation.

The prototype system has undergone detailed testing and analysis.
Simulation tools and playback tools for ICWS were developed to analyze
the raw data, as well as test and evaluate the system. The simulation
tools regenerate all intermediate variables and trace back each detail
of the processing performed. Playback tools are used to show the video
files together with all engineering data so that we can have a
comprehensive understanding of the scenarios.

Series of tests were conducted at both RFS and Crows Landing to test the
FCWS. A leading vehicle and a bus were the main focuses of the testing.
A fifth wheel, an accelerometer and a string pot were installed on the
leading vehicle and synchronized with the FCWS. A string connected to
the bus was used to measure the distance between the leading vehicle and
the bus. The leading vehicle ran at low/medium speed with the bus
following it at a reasonably safe distance. The estimations (from the
FCWS algorithm) of the essential variables: relative positions, target
speed and acceleration were compared with the raw measurement from the
Lidar, the string (when applicable), the fifth wheel or the
accelerometer on the leading vehicle. The result is a good match as
shown later in this document.

The FCWS warning scenarios were categorized and analyzed using a
three-step quantitative approach. The three scenarios include:
moving/stopped target ahead on straight road; stationary target roadside
on curved road; overhead obstacles on declining/flat road are analyzed.
Improvements were made to the algorithm to include features that turn
the nuisance warning to a friendly reminder. With the road geometry
information (e.g., more precise GPS and digital map system), driver
status information, target properties and crash data analysis, some of
the nuisances induced by curved roads and overhead obstacle problems
could be overcome.

Bench tests were also conducted for the SCWS system to verify the
resolution, the accuracy of the object sensors and the accuracy of
velocities measured by DATMO. Closed Course testing was conducted to
verify the warning algorithms by constructing situations in which
cardboard objects came in contact with the bus to verify the true
positives and look at the relative timing of the incident prediction and
DVI activation.

The majority of the positive alarms SCWS issues are understandable by
the transit operator. Many of the false positives are not very seriously
false (velocity off slightly), and the driver might not even consider
them nuisances. When a large amount of false positives are seen by the
operator, the problem can be traced back to sensor failures (e.g. laser
scanner not level due to the bus tilting or road variation and picking
up ground returns). The number of serious false positives which will be
present even if all the sensors work correctly is small and due
primarily to velocity outliers.

(5) Transit CWS Simulator.

The SamTrans FAAC simulator is being modified to incorporate CWS
functions, which will allow us to create specific scenarios of interest,
including scenarios too dangerous to test on real buses, to which large
numbers of drivers can be exposed, providing us with a much more
extensive data set than we could obtain from in-service operation of two
buses. From the simulator experiments, more extensive data sets will be
obtained, which will be used to analyze driver behavior changes due to
the introduction of ICWS and for further optimization of the warning
algorithms and DVI.

(6) ICWS commercialization and further research recommendations.

As more advanced sensors and more powerful computers are available,
together with further integration of the FCWS and the SCWS, the ICWS
will have fewer sensors needed to maintain the same or even higher
sensing capability and process all functions using only one computer,
resulting in smaller volume and less cost. More research is being
conducted to improve the ICWS tracking algorithm and threat assessment
algorithm. Research on use of GPS/Digital map and sensor data fusion
will also be introduced to help ICWS performance improve.

# Introduction {#introduction .unnumbered}

## Background

The Federal Transit Administration has been funding work over the last
five years to shorten the commercialization and deployment cycle of
collision warning systems for the transit industry. FTA developed
initial cooperative agreements with San Mateo Transit Authority
(Samtrans), California Department of Transportation (Caltrans),
University of California at Berkeley PATH Program (PATH) and Gillig
Cooperation to develop Frontal Collision Warning Systems (FCWS), and
with Port Authority of Allegheny County (PAT), Pennsylvania Department
of Transportation (PennDOT), Carnegie Mellon University Robotics
Institute (CMU RI) to develop Side Collision Warning System (SCWS) and
with Ann Arbor Transit Authority and Veridian Engineering Division to
develop Rear Collision Warning Systems (RCW). The focus of these efforts
was to fund technology development to the point where a commercial
system could be developed. In addition, existing Side Object Detection
systems using ultrasonic automotive sensors were put into operational
field tests to learn how to introduce technology onto a transit platform
in a way that made it acceptable to operators, maintenance personnel and
management. Initial results of this work were an advance in the
technology usable for collision warning systems, specifications for
advanced collision warning systems, and the evaluation of 100
commercially available side object detection systems in revenue
operation.

The next step in this program was to determine what it would take to
field an integrated advanced frontal and side collision warning system
and conduct a more limited field test on ten commercial systems. The
objectives for this work were as follows:

1.  Develop a Functional ICWS

2.  Create System Acceptable to Operators

3.  Prove Technical Feasibility Through Field Test of Prototype System

4.  Demonstrate a Potential for Reduction in the Severity and Frequency
    of Collisions

In 2002, FTA entered into cooperative agreements with a consortium that
included San Mateo Transit Authority (Samtrans), Port Authority of
Allegheny County (PAT), California Department of Transportation
(Caltrans), Pennsylvania Department of Transportation (PennDOT),
University of California PATH Program and the Carnegie Mellon University
Robotics Institute. Prototype hardware designs and algorithm research
were focused early in the project to field an advanced Integrated CWS.
This report documents the results of this research prior to the
evaluation of the prototype advanced ICWS. The final evaluation report
for this Integrated CWS will be produced in June 2005

## Scope

As detailed in the Preliminary ICWS Performance Specifications, the
primary goal of an integrated collision warning system is to predict
imminent potential crashes, or collisions with objects and warn the
transit operator. To achieve this goal the collision warning system has
the sensing capability to gather information from both the subject
vehicle and the surrounding environment (Transit bus and object sensing)
and display it to the operator via the Driver Vehicle Interface. The
ICWS fulfills eight functions as illustrated in Figure 1, including
object sensing, transit bus sensing, the basic signal and data
processing functions shown within the dotted lines and the Driver
Vehicle Interface (DVI). At the beginning of this program, these
functions were examined to see what research needed to be done to
accelerate the deployment of commercial systems.

[]{#_Ref25394492 .anchor}

Figure 1 - Basic ICWS Algorithms

### Transit bus and object sensing

Subject vehicle status sensing refers to the acquisition of information
on operator actions and the current kinematic states of the bus.
Examples of subject vehicle status sensors are: speedometers,
accelerometers, brake pressure sensors, steering angle sensors, and GPS
receivers. Commercial sensors exist in this area and only need to be
specified and incorporated into a commercial system. The goal of this
program was to determine what sensor information is necessary and should
be defined in the ICWS Specifications.

Object sensing refers to the acquisition of information from the
environment (for example, road curvature), the presence of other objects
(for example, vehicles and pedestrians) and the current kinematic states
of the objects. Examples of sensors for object status sensing are
microwave RADARs, laser LIDARs, imaging sensors and ultrasonic sensors.
The sensors used in this early prototype ICWS were the more expensive
and higher performance ones in order to determine where the performance
level should be set for a commercial system. The development of a
cheaper sensor for a commercially viable system is discussed more fully
in the Recommendations Section.

### ICWS Algorithms - Signal and data processing

The main research component of this program involved developing the
algorithms necessary to process the incoming data and generate warnings
to a transit operator. Research was accomplished in each of the five
algorithm areas defined below.

The function of object detection and tracking is to tell if there is an
object within the monitoring coverage of the collision warning system.
The state of the art in object tracking was not sufficient to develop
ICWS systems that could accurately and in a timely fashion present
objects to be tracked. The conversion of sensor data to object data
represented a large challenge to developing these systems and
significant effort was devoted to this cause.

The function of object trajectory estimation is to determine the present
and future kinematic states of an object. The states included such
information as spatial position, velocity and acceleration of an object.
The algorithms for predicting the trajectory are straightforward and did
not need to be researched, but the importance of each of the states for
the warning algorithms were examined and the results incorporated in the
current set of ICWS Specifications.

The function of bus trajectory estimation is to determine the present
and future kinematical states of the transit bus. The states included
such information as spatial position, velocity and acceleration of the
bus. Once again, the algorithms for predicting the trajectory are
straightforward and did not need to be researched, but the importance of
each of the states for the warning algorithms was examined and the
results incorporated in the current set of ICWS Specifications.

The function of threat assessment is to determine the likelihood of
collision between the transit bus and an object by assessing such
factors as the probability of a collision, time to collision and the
likely severity of a collision. These factors form the basic data used
in the warning algorithms. As such, they were a primary part of the
research and used as metrics in the evaluation phase of this program.

The warning algorithms determine the safety level of the transit bus and
its environment based on the threat assessment. One important aspect of
the warning algorithms is to use heuristics based on threat assessment,
object location and timing to minimize the number of nuisance alarms. A
framework for these heuristics was developed which allows future
heuristics to further tune the system based on data obtained during
revenue service during the evaluation part of this program.

### Driver-vehicle interface (DVI)

The DVI is a critical component of the ICWS, which displays the outputs
of the ICWS to the operator for appropriate corrective action. These
signals are presented via displays whose modalities include visual and
the capability for auditory. An effective DVI must be able to bring the
driver's attention to the hazardous situation while he/she performs a
variety of driving and non-driving tasks and does not pose additional
workload or distraction. The DVI for the ICWS was an extension of the UC
Berkeley PATH experience over the previous two years on transit bus
operation. The design of the DVI took into consideration the
characteristics of the bus design and special needs for transit drivers.
Many discussions with and feedback by Foster Miller, Inc and members of
the transit community (SamTrans, PATH and a dozen of transit agencies in
the Bay Area) were also used for the current design. The current design
of the DVI will be evaluated in simulation by PATH and SamTrans as part
of this program and evaluated by transit operators. These results will
be incorporated into the final performance specifications for an ICWS. A
more thorough discussion of the DVI in Section 7 of this report titled
DVI Development.

## Organization of Content

This report documents the research undertaken as part of this program by
two universities, with each one describing their respective parts of the
system. As such each major section is structured to discuss fully either
the side component of that section or the frontal component. Care has
been taken to title the subsections sufficiently to show this
distinction so as not to confuse the reader. This document is divided as
follows:

> 1 Introduction (Background, Scope, and Organization of Content)
>
> 2 Integrated Collision Warning System (System Description and
> Integrated ICWS)
>
> 3 System Overview
>
> 4 Hardware Development
>
> 5 System Software
>
> 6 Algorithm Development
>
> 7 DVI Development
>
> 8 Data Analysis and Evaluation
>
> 9 Calibration and Testing
>
> 10 Transit CWS Simulator
>
> 11 Recommendations
>
> Appendix A: Acronym Definitions
>
> Appendix B: Related Documents
>
> Appendix C: Published Papers
>
> Appendix D: Conversion Tables

# Integrated Collision Warning System  {#integrated-collision-warning-system .unnumbered}

## System Description

The integrated collision warning system is functionally divided into
FCWS and SCWS processors dealing with frontal and side collision
detection and warning signal generation. This modularity makes it easier
to specify and integrate a rear collision warning system in the future
for 360 degree situational awareness. The warning information is
presented to the transit operators through an integrated Driver Vehicle
Interface. Additionally, data collected through each processor is shared
with the other processor and stored for easier data analysis. The
elements of the ICWS include:

-   Vehicle state estimation and bus signals interface -- this includes
    the common infrastructure that each collision warning system needs
    such as vehicle position, speed, heading, door open/close, turn
    signals, etc.

-   Frontal collision processor --Includes sensors for detecting frontal
    obstacles and vehicle status information for determining risk levels
    and for generating warning outputs to integrated DVI. Appropriate
    sensory information is exchanged with the side collision processors.

-   Left and Right Side collision Processors - Includes sensors for
    detecting side obstacles and vehicle status information for
    determining risk levels and for generating warning outputs to the
    integrated DVI. Appropriate sensory information is exchanged with
    the frontal collision detection processor.

-   Integrated DVI -- to display the warning to the operator

-   Data storage -- Stores video and digital data for later analysis and
    evaluation. The data collected by both frontal and side collision
    detection systems are stored with time synchronized data formats for
    post processing analysis.

A top level overview showing the general configuration of the ICWS and a
more detailed hardware / architectural layout are shown in the next two
figures.

[]{#__RefHeading___Toc96687033 .anchor}Figure 2 Configuration of
Integrated Collision Warning System

![](media/image1.wmf)[]{#_Ref18924925 .anchor}

Figure 3 System Architecture

## Integrated ICWS

What do we mean by an "integrated" ICWS? This question would naturally
occur as you look at the functional configuration figure above, since it
appears that each of these systems is operating independently. The
overarching design philosophy for this early prototype combining the
FCWS and SCWS was that the frontal and side collision warning systems
should be closely integrated through information integration. In
implementing the hardware, we wanted to ensure that each system can
operate even if the others go down. With separate computing systems this
dictates a level of independence that does not need to be reflected in
the end commercial product. This integrated prototype is integrated at
the information level primarily through the RS232 interface and the time
synchronization of data streams to allow integrated post processing data
analysis.

A visual integration occurs though the common Driver Vehicle Interface
and the Driver Interface control box. This display to the operator
integrates the warnings by displaying them on a single set of DVI's.
Lastly, a common coordinate system has been defined to allow the
meaningful passage of data between the FCWS and SCWS systems.

[]{#__RefHeading___Toc96687035 .anchor}Figure 4 Three stages towards a
commercial product

As shown in the above figure, this early prototype of an ICWS allows us
to test the concepts and develop an integrated set of performance
specifications for a commercial prototype of an ICWS. Offline data
analysis will show additional levels of integration potential by
revealing the benefit of real time information transfer between systems.
Human factors testing will determine if a transit operator can
assimilate the current DVI information. This familiarity and experience
with the combined systems will show additional areas for further
integrated specifications.

Once these integrated specifications are released, there are still two
stages left to developing the final commercial ICWS. The first is the
initial commercial prototype and the second is the commercial product
itself. The commercial prototype will involve the integration of
hardware subsystems, elimination of redundant components and interfaces,
common software modules and overlapping sensors. This additional step
before the development of a final commercial product is necessary in
order to provide for the integration of the forward and side collision
algorithms using a common algorithm base. This is discussed more fully
in the Recommendations Section of this report.

## Sensing Needs

The farthest detectable range in the same lane is 100m (330ft). The
closest detectable range in the same lane is no greater than 3m (10ft).
The maximum detectable side-looking angle from the front bus corners is
30 degrees. The detectable lateral position for the forward sensors is
over 6m (20ft). The side looking sensors will closely track objects that
are within 3m of the bus however, objects will be detected as far as 50
meters away.

![](media/image2.wmf)

[]{#__RefHeading___Toc96687036 .anchor}Figure 5 Integrated system
spatial coverage illustration

# System Overview {#system-overview .unnumbered}

## FCWS System Overview

### The goals of the FCWS 

The goals of the transit Frontal Collision Warning System (FCWS) under
the context of this project include:

1.  Address imminent crashes.

2.  Provide warnings for smoother maneuvering.

3.  Provide warnings when a bus is too close to a forward vehicle.

### FCWS functions

The operation environment for ICWS is significantly different from the
environment that automobile CWS deals with in the following two ways.
First, most of the transit frontal crashes occurred in urban areas while
previous studies on collision warning and collision avoidance have
mostly focused on highway applications, freight trucks, and light-duty
passenger cars. The urban environment presents considerable challenges
with respect to the diversity of obstacles to be detected and different
traffic patterns. The transit FCWS must be able to deal with the complex
urban environment besides the one that current commercial CWS address.
Second, transit bus drivers are professional and experienced drivers who
may have different needs for a FCWS. Transit drivers have also expressed
concern regarding the presentation of warnings that can be seen by
passengers. Bus passengers might find warnings for advance cues of
potential threats to be annoying and potentially alarming. There is
still a great need for human factors research in FCWS within the transit
environment.

Despite the differences between the collision warning applications, the
FCWS for transit buses requires the same functional elements that are
required by other CWS. The principal functional element of a CWS is
sensing and detection of presence of hazardous objects. Furthermore,
this function must be able to match the complex urban environment. The
second functional element is warning generation. It processes the sensor
information to "detect" the targets that may potentially be dangerous to
the bus, then determines the threat level and generates warnings at a
reasonable good timing if necessary. The third functional element is the
Driver Vehicle Interface (DVI) which issues the warning message to the
driver. The figure below depicts the functional description of the
collision warning system:

![](media/image3.wmf)

[]{#__RefHeading___Toc96687037 .anchor}Figure 6 Functions of frontal
collision warning system

The figure below shows the architecture of the FCWS system PATH
developed:

![](media/image4.wmf)

[]{#__RefHeading___Toc96687038 .anchor}Figure 7 FCWS system architecture

### FCWS system hardware

The FCWS system hardware consists of power and adapters, two PC104
computers (one is an engineering computer and the other is a video
recorder), sensors (including five obstacle sensors, and host-bus
sensors) and cameras (frontal looking camera, driver-side looking
camera, passenger-side looking camera and interior looking camera), and
human machine interface (including a driver control box and two DVI
bars).

The engineering data, which mainly includes the obstacle sensor data and
the host bus sensor data, is recorded and processed by an engineering
computer which is a PC104 computer running QNX operating system. The
obstacle sensors selected by PATH to capture the environment around the
bus include commercially available mono-pulse millimeter-wave RADARs and
scanning infrared lasers. Both the RADAR and scanning laser measure
distance and azimuth angle of multiple targets. The RADAR units are
mounted on the front bumper, one on each end, pointing forward. The
Denso LIDAR unit is mounted near the center of the bumper, pointing
forward. Host bus sensors measure the bus status, including bus speed,
accelerations, yaw rate, brake pressure, throttle position, windshield
wiper status, back up light, turn signals. Other sensors include a GPS
system and a driver control box, which controls the brightness of the
DVI bars and the system sensitivity level.

Video streams from four cameras are combined together with a titler by a
quad combiner and recorded by another PC104 video-recording computer
running QNX 6. It is synchronized with the engineering computer in real
time through RS232 serial ports.

The FCWS and the SCWS communicate with each other through RS232 serial
ports. The two systems exchange information that the other party may
need.

### FCWS system algorithms

The prototype FCWS algorithm was developed based on the data fusion and
decision making model developed by the Joint Directors of Laboratories
(JDL) data fusion sub-panel.

#### The JDL data fusion process model

The JDL data fusion model provides a top-level framework of data fusion
systems, and defines terms commonly used in different areas. The top
level of the JDL data fusion process model is shown in the figure below:

![](media/image5.wmf){width="5.2in" height="2.339583333333333in"}

[]{#__RefHeading___Toc96687039 .anchor}Figure 8 JDL data fusion process
model

The JDL model is a generic model for common understanding and
discussion. It has defined levels of processes to identify functions and
techniques. The model has built a common base for researchers and system
developers working in different areas. With the help of this model, we
can adopt a lot of approaches and techniques developed for other
applications, such as robotics, Computer Integrated Manufacturing
Systems (CIMS), airport surveillance and air traffic control, to develop
a CWS.

+----------------------+-----------------------------------------------+
| **SOURCE**           | The sources provide information at a variety  |
|                      | of levels ranging from sensor data to *a      |
|                      | priori* information from databases to human   |
|                      | input.                                        |
+----------------------+-----------------------------------------------+
| **PROCESS            | Source preprocessing enables the data fusion  |
| ASSIGNMENT**         | process to concentrate on the data most       |
|                      | pertinent to the current situation as well as |
|                      | reducing the data fusion processing load.     |
|                      | This is accomplished via data pre-screening   |
|                      | and allocating data to appropriate processes. |
+----------------------+-----------------------------------------------+
| **OBJECT             | Level 1 processing combines locational,       |
| REFINEMENT**         | parametric, and identity information to       |
|                      | achieve representatives of individual         |
| **(****Level 1)**    | objects. Four key functions are:              |
|                      |                                               |
|                      | -   Transform data to a consistent reference  |
|                      |     frame and units                           |
|                      |                                               |
|                      | -   Estimate or predict object position,      |
|                      |     kinematics, or attributes                 |
|                      |                                               |
|                      | -   Assign data to objects to permit          |
|                      |     statistical estimation                    |
|                      |                                               |
|                      | -   Refine estimates of the objects identity  |
|                      |     or classification                         |
+----------------------+-----------------------------------------------+
| **SITUATION          | Level 2 processing attempts to develop a      |
| REFINEMENT**         | contextual description of the relationship    |
|                      | between objects and observed events. This     |
| **(****Level 2)**    | processing determines the meaning of a        |
|                      | collection of entities and incorporates       |
|                      | environmental information, *a priori*         |
|                      | knowledge, and observations.                  |
+----------------------+-----------------------------------------------+
| **THREAT             | Level 3 processing projects the current       |
| REFINEMENT**         | situation into the future to draw inferences  |
|                      | about the enemy threats, friendly and enemy   |
| **(****Level 3)**    | vulnerabilities, and opportunities for        |
|                      | operations. Threat refinement is especially   |
|                      | difficult because it deals not only with      |
|                      | computing possible engagement outcomes, but   |
|                      | also assessing an enemy's intent based on     |
|                      | knowledge about enemy doctrine, level of      |
|                      | training, political environment, and the      |
|                      | current situation.                            |
+----------------------+-----------------------------------------------+
| **PROCESS            | Level 4 processing is a meta-process, i.e., a |
| REFINEMENT**         | process concerned with other processes. The   |
|                      | three key level 4 functions are:              |
| **(****Level 4)**    |                                               |
|                      | -   Monitor the real-time and long-term data  |
|                      |     fusion performance                        |
|                      |                                               |
|                      | -   Identify information required to improve  |
|                      |     the multi-level data fusion product, and  |
|                      |                                               |
|                      | -   Allocate and direct sensor and sources to |
|                      |     achieve mission goals.                    |
+----------------------+-----------------------------------------------+
| **DATABASE           | Database management is the most extensive     |
| MANAGEMENT**         | ancillary function required to support data   |
|                      | fusion due to the variety and amount of       |
| **SYSTEM**           | managed data, as well as the need for data    |
|                      | retrieval, storage, archiving, compression,   |
|                      | relational queries, and data protection.      |
+----------------------+-----------------------------------------------+
| **HUMAN-COMPUTER**   | In addition to providing a mechanism for      |
|                      | human input and communication of data fusion  |
| **INTERACTION**      | results to operators and users, the           |
|                      | Human-Computer Interaction (HCI) includes     |
|                      | methods of directing human attention as well  |
|                      | as augmenting cognition, e.g., overcoming the |
|                      | human difficulty in processing negative       |
|                      | information.                                  |
+----------------------+-----------------------------------------------+

[]{#__RefHeading___Toc96686994 .anchor}Table 1. JDL data fusion process
model

The JDL model however, is not a universal architecture for practical
applications. It does not specify the level of data fusion. Data fusion
level is an application-specific problem. To define the collision
warning system architecture, analysis of the system function
requirements is needed.

#### Requirements of the transit FCWS

All the functions defined in the JDL model except level four are
requirements of transit FCWS. First of all, the source preprocessing
must be performed to eliminate the unwanted signals and to detect the
objects of interest. The sources here may include object sensors such as
RADARs, LIDARs, CAMs, GPSs, and subject vehicle sensors such as
speedometers, accelerometers, yaw rate and braking pressure sensors.
Sensors are used to convert the measurable elements of the physical
processes of the environment into electric parameters. The process to
convert the physical process elements into electric parameters is
observation. Some unwanted signals, such as pavement clutter, road-side
trees and traffic signs, etc., and interference from the same kind of
sensors mounted on other vehicles or from other sources, as well as
noise from internal components of the sensor, must be suppressed in
order to pickup the real object signals. The preprocessing is the
process to figure out, from one or more observations, whether an object
exists or not, and to measure the status of the existing object.

The process of finding out whether an object exists or not is defined as
detection. It is a probabilistic test of hypotheses. In the simplest
situation, we have two hypotheses, H1 and H0, representing the object's
presence and absence respectively. The probability of being H1 while the
object does exist, viz. probability of correct detection (P~d~), is
always less than 1. The probability of being H1 while the object does
not exist, viz. probability of false alarm (P~fa~), is always greater
than zero.

The process to measure the object status, such as location and velocity,
from the observations, is defined as estimation. The estimated
parameters are random variables, because they are calculated from
observations and the observations are random samples from a
probabilistic set.

The results of detection and estimation are called measurements in this
report. A measurement comes from single or multiple observations.
Measurements, as functions of time, are stochastic processes in reality.
Level 1 processing should then be performed to detect the processes and
to estimate parameters of the processes. It is assumed in most cases
that false alarms are less possible than real objects to form continuous
processes. The detection of the process will eliminate the false alarms
and determine when a process begins and when it ends. The estimation of
the process will refine the measurements. The results of detection and
estimation of processes are called tracks. The process to initiate,
manipulate and end tracks is called tracking.

A track represents a stochastic process converted by a sensor from the
physical process of an object. The parameters of a stochastic process
are correspondent to the parameters (as functions of time) of an
individual object. To develop a description of the current relationship
among multiple objects and events in the context of their environment,
level two processing is needed. Tracks from different sensors may
represent the same object. These tracks must be fused into one track.
This process is called track-to-track fusion, and the fused track is
called the system track. After fusion, a system track becomes a refined
unique representation of an object. The history of the tracks and the
relationship among the tracks as an aggregation represent the traffic
scenario. Once the scenario is described, level three processing is
needed to assess the threats. Threat assessment is the process whereby
the current situation is projected into the future to assess the
severity of a potential traffic accident. Knowledge about vehicle
kinematics, traffic, and the environment is needed for the assessment.
Human behavior may also be used for this assessment. Once a potential
threat is detected and exceeds the threshold, a warning will be sent to
DVI. Level four processing is not needed in an FCWS, because the
developers of the system and the vehicle drivers will perform this
function outside of the system.

#### Architecture of the transit FCWS warning algorithm

Studies on collision warning/avoidance during the past few years have
built a good foundation for the bus FCWS design. Sensors such as RADARs
and LIDARs for automobiles have been developed. Some sensors have been
integrated with built-in Digital Signal Processors (DSP) which can
perform source preprocessing with some also able to perform level one
processing. It is convenient to adopt these intelligent sensors in the
bus FCWS. Threat assessment algorithms have been studied and various
severity measures have been proposed, e.g. TTC, warning distance,
warning boundaries.

To develop a collision warning algorithm architecture from the JDL
model, one of the key issues is to decide where to fuse the data in the
data flow. We prefer the track-to-track fusion that matches the
state-of-the-art technology of the sensors and helps us focus on higher
level processing. The figure below is the block diagram of the transit
FCWS warning algorithm architecture. Details of the warning algorithm
are described in the algorithm chapter.

![](media/image6.wmf){width="6.011805555555555in"
height="2.9743055555555555in"}

[]{#__RefHeading___Toc96687040 .anchor}Figure 9 The architecture of the
transit FCWS warning algorithm

## SCWS System Overview

The computer systems on the bus have four major tasks:

1.  Data collection

2.  Data processing

3.  Data storage

4.  User interface

All items in the above list are critical to a functioning side collision
warning system except for the data storage task, which is a necessary
research tool. The major data processing task is the detection and
tracking of objects from the range data from the SICK Laser ranger.
Early in our design process, it was recognized that this task would
consume the largest share of our processor power. However, the tracking
problem of objects on either side of the bus easily lends itself to a
bilateral partitioning, leading us to an architecture of two
semi-independent computers.

While the bulk of the processing is object tracking, most of the data
collected by the system comes from a set of external cameras that allow
the algorithm designer to better interpret the rather abstract range
finder scans. This video data is not part of the core SCWS. Since it
would be impractical to move so much data over the in-vehicle network,
we added a third computer that serves as a central data repository and
digital video recorder.

One further observation is that the right side of the bus is more
important that the left since it faces the curb and is most often near
pedestrians. For this reason, all the crucial vehicle state sensors are
connected directly to the right processor and shared with other
computers via an Ethernet network. The right computer is also the master
clock in the system, allowing the different computers to properly
interpret the shared data. Finally, the right computer is the central
code repository. Source code, executables, and configurations all reside
on the right computer's hard disk, but are transparently shared via NFS
(Network File System).

### SCWS data acquisition and communication

There are 5 major data sources in the system.

1.  Vehicle State

2.  LIDAR Data

3.  Video Data

4.  Ancillary Data

5.  FCWS/SCWS Interface

Two CPUs collect and process left and right side LIDAR data,
respectively Vehicle state is a critical component of the SCWS, and is
therefore attached to the right processor, the more important of the
two, and then shared with other computers in the system. Vehicle state
includes odometry and IMU data, both of which are instruments that
connect to a serial port.

While both the SCWS and FCWS require a pose estimate, they each compute
their own estimate and do not share this information. This increases the
independence of the system at the expense of redundant hardware.
However, this is more than justified by eliminating the additional
downtime that would come with complete interdependence. The two systems
do share a physical connection to a PATH installed odometer.

Video data sources include a curb detector and a forward looking camera
that serves as a curb predictor. Since the curb detector is more
reliable than the predictor, this instrument is attached to the right
processor, with the predictor camera on the left. Only samples of the
raw video data from either instrument are saved.

Where possible, we have tapped into existing vehicle systems to
supplement our understanding of what the driver and vehicle are doing.
The J1708 data bus broadcasts engine related information, such as
vehicle speed and accelerator pedal position. The DINEX data bus
broadcasts the status of turn signals, warning lights, head lamps, and
tells us which doors are open and whether a passenger has requested a
stop.

The DINEX system is not present on all buses, in which case we rely on
instrumentation installed by PATH. Since this data is not critical, the
FCWS collects this data and shares it via a serial link. This serial
link is the only form of communication between the two systems. It is
also used to hand off objects tracked with one system that are moving
into the field of view of the other.

# Hardware Development {#hardware-development .unnumbered}

## FCWS Obstacle Detection Sensors

### FCWS obstacle sensors

The figure below shows the layout of obstacle sensors and video cameras
(Front view). The positions of each sensor/camera are measured in a FCWS
reference frame. The frame is originated on the ground under the center
point of the frontal bumper with positive directions of x-, y- and z-
axes pointing to driver-side, upward, and forward respectively.

![](media/image7.wmf)

[]{#__RefHeading___Toc96687041 .anchor}Figure 10 Layout of sensors,
cameras and HMI

For convenience, the following abbreviations are used:

  -------------- --------------------------------------------------------
  F-             Frontal-looking or frontal

  D-             Driver-side-looking or driver-side

  P-             Passenger-side-looking or passenger-side

  I-             Interior-looking

  LIDAR          Laser scanning RADAR

  RADAR          Micro-wave RADAR

  CAM            Camera
  -------------- --------------------------------------------------------

For example, [F-CAM]{.underline} represents "frontal-looking camera",
and [D-RADAR]{.underline} stands for "driver-side micro-wave RADAR".

The numbers that are given in the following table are the
obstacle-sensor positions of FCWS on the Samtrans bus.

  ---------------------- -------------------------------------- --------------
  **Sensor/Parameter**                                          **Host
                                                                bus/Value**

  **Description**        **Parameter**                          **Bus 601**

  F-LIDAR                X (lateral, mm)                        768

                         Y(vertical to ground, mm)              445

                         Z(longitudinal to frontal face of the  -25
                         bumper, mm)                            

                         Angle ()                               0

  D-LIDAR                X (lateral, mm)                        1150

                         Y(vertical to ground, mm)              435

                         Z(longitudinal to frontal face of the  -38
                         bumper, mm)                            

                         Angle ( to the left)                   20

  P-LIDAR                X (lateral, mm)                        -1180

                         Y(vertical to ground, mm)              445

                         Z(longitudinal to frontal face of the  -76
                         bumper, mm)                            

                         Angle ( to the right)                  20

  D-RADAR                X (lateral, mm)                        965

                         Y(vertical to ground, mm)              445

                         Z(longitudinal to frontal face of the  -51
                         bumper, mm)                            

                         Angle ()                               0

  P-RADAR                X (lateral, mm)                        -965

                         Y(vertical to ground, mm)              440

                         Z(longitudinal to frontal face of the  -51
                         bumper, mm)                            

                         Angle ()                               0
  ---------------------- -------------------------------------- --------------

[]{#__RefHeading___Toc96686995 .anchor}Table 2. Location and orientation
of obstacle detection sensors

#### Cameras

+-------------------+----------------+----------------+----------------+
| **Camera**        | **X(m)**       | **Y(m)**       | **Z(m)**       |
|                   |                |                |                |
| **(Bus 603)**     |                |                |                |
+-------------------+----------------+----------------+----------------+
| P-CAM             | -0.60          | 2.59           | 0.27           |
+-------------------+----------------+----------------+----------------+
| D-CAM             | 0.95           | 2.69           | -0.26          |
+-------------------+----------------+----------------+----------------+
| F-CAM             | 0.93           | 2.70           | -0.13          |
+-------------------+----------------+----------------+----------------+

[]{#__RefHeading___Toc96686996 .anchor}Table 3. Locations of cameras

#### LIDARs (DENSO Corporation)

The table below shows LIDAR specifications.

+-------------------------------+--------------------------------------+
| Detection range               | 0-120m                               |
+-------------------------------+--------------------------------------+
| Detection angle               | 40deg (lateral, 20deg)               |
+-------------------------------+--------------------------------------+
| Detection angle               | 4.4deg(elevation)                    |
+-------------------------------+--------------------------------------+
| Update rate                   | 100ms                                |
+-------------------------------+--------------------------------------+
| Laser wave length             | 850nm                                |
+-------------------------------+--------------------------------------+
| Laser beam size               | 0.2deg(lateral) 0.9deg(elevation)    |
+-------------------------------+--------------------------------------+
| Number of detection points    | 265(lateral),6 (elevation)           |
|                               |                                      |
|                               | total: 1590points/cycle              |
+-------------------------------+--------------------------------------+

[]{#__RefHeading___Toc96686997 .anchor}Table 4. LIDAR specifications

The power supply of LIDARs is controlled by a speed-controlled relay.
Whenever the bus speed measured is below 3m/s and the creeping detector
detects the bus is not moving, a LIDAR control signal is set inactive to
turn off the power to the LIDARs. When the bus speed measured is greater
than 3m/s or the creeping detector detects the bus is moving, the LIDAR
control signal is active and the LIDAR power is resumed. (This relay may
be removed if the sensor manufacturer improves the design to make the
sensor eye-safe.)

#### RADARs (EVT-300)

The table below shows RADAR specifications.

  ------------------------------ ----------------------------------------
  Detection range                0.3-110m

  Detection angle                12deg (lateral,6deg)

  Update rate                    65ms
  ------------------------------ ----------------------------------------

[]{#__RefHeading___Toc96686998 .anchor}Table 5. RADAR specification

### FCWS Host-bus sensors

Vehicle speed is measured by listening to the vehicle's SAE J1939/1708
data bus and also by tapping off of an analog speed signal directly from
the transmission. This speed signal from the transmission is filtered
and conditioned by an electronic circuit.

Vehicle yaw rate is measured using a fiber optic rate gyro. This unit is
mounted in a waterproof enclosure under the floor near the rear axle.
This transducer has an RS232 interface.

Brake pressure is measured using a pressure transducer mounted on a
spare port of the air brake system under the floor of the driving area.
A proximity sensor, which is used to determine if the bus is moving at
speeds lower than 2-3 miles per hour, is mounted near a universal joint
on the drive shaft. Turn signal activation and backing light status is
recorded by tapping off the existing turn signal circuit and backing
lights. Windshield wiper activation is determined with a proximity
sensor mounted on the windshield wiper mechanism. The host-bus state
signals, including brake pressure, turn signals and back up light status
, windshield wiper signal, creeping detector status and sensitivity
level are filtered before going to A/D converters.

![](media/image8.wmf)

[]{#__RefHeading___Toc96687042 .anchor}Figure 11. Interface between the
engineering computer and host-bus sensors

The GPS antenna is mounted on the rear of roof near the exhaust for the
HVAC, the GPS computer is mounted in a waterproof enclosure near the
HVAC evaporator unit in the rear of the bus. The GPS and CDPD modem
antenna are mounted on the rear of roof near the exhaust for the HVAC,
the GPS and CDPD modem computers are mounted in a waterproof enclosure
near the HVAC evaporator unit in the rear of the bus. 

### FCWS Battery / Ignition monitoring and shutdown circuitry

Two relays control the master power supplies: one is the master relay;
the other is a time-delay relay. After ignition is on/off, the master
relay turns on/off the switch. The switch will trigger the time-delay
relay counter. Once the counter reaches a preset value, the time-delay
relay will turn on/off the 12V and 24V bus Bars. The purpose of the
time-delay is to avoid noise-triggered false on/off of power supplies,
and give some additional time to the computers to save files before exit
the program after the ignition is off. The master relay on/off signal is
sent to the engineering computers to indicate the ignition operation.

## SCWS Side Sensors

### SCWS Laser Scanner

A laser ranging scanner manufactured by SICK, Inc is mounted on each
side of the bus, behind the front wheel wells, below floor level. These
laser scanners are use for object detection. This is a commercially
available LIDAR that has been used extensively in the field of robotics
and engineering for many years. A detailed error analysis of this sensor
is included in the testing section of this document. The specifications
for this LIDAR far exceed what is necessary for this application. It's
usage for the research prototype system allowed the collection of high
quality range data which can be used to show what is possible with
collision warning systems. Any commercial collision warning system will
not need to use as high performance LIDAR as we used. With the data
collected it is easy to down sample and add noise to see how future
algorithms perform.

![](media/image9.png){width="4.799305555555556in" height="3.6375in"}

[]{#__RefHeading___Toc96687043 .anchor}Figure 12. SICK Laser Scanner and
Retraction Assembly

### Laser scanner retraction system

Each laser is mounted in a box approximately 18" H x 12" W x 12" D which
mount in an opening in the sheet metal side panel of the bus. In
operation, each laser extends approximately 4" beyond the side panel of
the bus, but they are retracted below flush with the side panel when not
in use. A small air cylinder on top of each enclosure box actuates the
retract / extend motion of the laser, which swings on an arm pivoted
near the front of the box. If the laser comes in contact with obstacles
in the environment, the compliance of the air cylinder allows the laser
to be pushed back into the enclosure to minimize damage to the laser and
environment.

The actuation system comprises:

\(2\) Bimba air cylinders, 1-1/2" bore x 3" stroke;

\(2\) Automatic 4-way, spring-return, 24VDC, 8W solenoid valves;

\(1\) Watts filter-regulator;

\(1\) 2-way shutoff (ball) valve;

125PSI air supply with storage tank (On bus);

Plastic air tubing, ¼" OD and push-lock fittings.

With system air and/or electrical power off, the laser will be held
retracted into the box by a return spring. To operate the actuation
system, the ball valve (located at the bus air tank) will be opened
allowing compressed air to pass through the filter-regulator (which will
be adjusted to achieve appropriate force from the air cylinders) to the
two solenoid valves located near the cylinders. Spring return on the
solenoid valves will normally hold the cylinders retracted, even with
zero air pressure. When the valves are activated based on a command
signal from the control computer, air will flow through the valves to
the cylinders, causing them to extend and push the laser support arms
against a mechanical stops to precisely position the lasers for
operation. When the valves are deactivated, air plus the return spring
will cause the arms to swing back into the box against a back stop, such
that the lasers will be within the bus envelope. Speed controls on the
cylinders will be adjusted to give appropriate extend and retract
speeds. Cylinder volume is 5.3 cubic inches (each); for the worst case
of 125PSIG (max. available pressure; normal operating pressure is 50
PSIG), each cylinder stroke will consume about 50SCI (standard cubic
inches) of air, or about .029SCF (standard cubic feet). Under normal
operation, we expect operation of the cylinders to occur not more than a
few times per hour, so total air consumption should be small. The bus
air tanks each hold about 1 cubic foot or air, so the volume consumed by
actuation of two cylinders going full-stroke should be less than 1% of
the tank volume. Leakage (through cylinder seals, etc.) is negligible.

## SCWS Curb Detector

For the SCWS we need to determine the location of the sidewalk in order
to better assess the situation. Pedestrians on the sidewalk are
considered safer than if they are not on the sidewalk. We used a laser
line striper (LLS) to detect the position of the curb at the front of
the bus. The technical details of the LLS are presented in a paper.[^1]
Here we give an overview over its working principle and illustrate, how
it was mounted on the bus.

[]{#__RefHeading___Toc96687044 .anchor}Figure 13 Schematic of a laser
line striper.

The LLS projects a pattern of light into the scene that is imaged by a
camera (see Figure 13) and the appearance of the pattern is used to
compute distance to objects in the environment. The LLS is attractive
because of its relative small size and robustness. In addition,
computation of range is very low cost compared to other optical methods
such as stereovision that requires high computation.

[]{#__RefHeading___Toc96687045
.anchor}![](media/image10.jpeg){width="2.4751968503937007in"
height="2.060236220472441in"}![](media/image11.png){width="2.4645669291338583in"
height="0.5165354330708661in"}Figure 14 A LLS mounted on a vehicle
looking to the side at various objects. The return from the sensor is
shown in the lower part of the figure.

We have built and employed such a sensor where the light pattern is a
plane of NIR light and the appearance on the object is a line (see
Figure 14). The novelty of our sensor is that it can work outside in
bright sunlight even though the power output of the laser is limited by
eye safety. The background from ambient sunlight is suppressed by
synchronizing a pulsed laser with a fast shutter, employing a narrowband
filter, and some image analysis. The figure shows our LLS mounted on a
vehicle and looking at various objects on the side of the vehicle. In
the lower part of the figure, one can see the output of the sensor.

The range and resolution are dependent on the sensor configuration. In
the following table they are shown for three different field-of-views:

  ----------------------------------- ----------- ----------- -----------
  field of view \[deg\]               30          55          105

  angular resolution \[deg\]          0.05        0.09        0.16

  max. range (ideal) \[cm\]           700         520         300

  max. range (typical) \[cm\]         300         200         130

  range resolution \[cm\]             1.4         2.6         5.0
  ----------------------------------- ----------- ----------- -----------

[]{#__RefHeading___Toc96686999 .anchor}Table 6. Range and Resolution of
Laser Line Striper

The maximum range is for ideal conditions (high reflectivity objects,
etc.). For typical conditions, it is about half that distance. The range
resolution is for a 2 m distance, resolution varies with the square of
the distance.

The LLS was mounted inside the front bumper of the bus with a
field-of-view perpendicular to the side of the bus (see Figure 15).

[]{#__RefHeading___Toc96687046
.anchor}![](media/image12.jpeg){width="3.791732283464567in"
height="2.8437007874015747in"}![](media/image13.jpeg){width="2.541732283464567in"
height="3.395669291338583in"}Figure 15 LLS mounted in the front bumper
of the bus. On the left side is a frontal view of the SamTrans bus with
the rubber skin of the front bumper removed. The laser can be clearly
seen, the camera is occluded by the holding bracket. On the right is a
side view of the PAT bus. The red semitransparent area indicates the
location of the laser plane.

The LLS returned the cross section profile of the environment besides
the bus. If there is a curb besides the bus, the profile looked like the
one shown in Figure 16.

seen above the road.

Finally, the location of the curb was extracted from the profile by a
histogram method (see paper [^2] for details).

During the operation of the bus with the LLS we had following
experiences with environmental conditions:

**Temperature:** The temperature range for operation in Pittsburgh was
between 0^o^ F and 120^o^ F. The upper range is 20^o^ F above the
ambient temperature. This temperature was added because the laser is
located within the black enclosure of the front bumper and above the
black pavement. We needed to add a heater to the laser in order to
reduce this range of temperatures.

**Water:** The LLS can be exposed to water through rain or the bus wash.
We had to do some extra waterproofing to the camera.

**Mechanical:** The camera and laser needed to be tightly screwed to the
frame of the bumper to keep its alignment. No mechanical damage occurred
during the time of operation.

**Dirt:** Only a minimal amount of dirt accumulated on the lens of the
camera or the exit window of the LLS, it did not affect its operation.

### Sensor fusion of curb detector, bus state and video

In the previous section we described how we detect the curb next to the
front bumper with a laser line striper. In this section we discuss how
we determine the location of the curb alongside the bus and in front of
the bus. Details of this method can be found in a technical paper.[^3]
The section below is a summary.

#### Tracking the curb alongside the bus

The movement of the bus is recorded and therefore it is possible to
transform past curb measurements into the current reference frame. The
collection of these past curb measurements in the current reference
frame gives us the curb alongside the bus. An example can be seen in
Figure 17, where the curb alongside the bus is indicated as a blue line.

![](media/image14.png){width="3.3318897637795275in" height="3.95in"}

![](media/image15.png){width="3.3763779527559055in"
height="2.386220472440945in"}

![](media/image16.png){width="3.9347222222222222in"
height="1.6131944444444444in"}

[]{#__RefHeading___Toc96687047 .anchor}Figure 16 Profile of the road and
curb observed by the laser line striper. Some erroneous readings can be
[]{#__RefHeading___Toc96687048 .anchor}Figure 17 Curb alongside the bus.
On left is a birds-eye-view and on the right is a view from the right
camera. The raw data from the LLS is indicated with red color, the curb
position extracted from that raw data is in green and the tracked curb
is in blue.

#### The curb in front of the bus

To detect the curb in front of the bus we use a vision process which is
initialized by the knowledge of the curb position we already have. The
right image in Figure 17 shows the view of the right, forward looking
camera. We already know where the curb is and in which direction it
proceeds. This is used for the initial start and direction of a search
for an edge in the image. This search is continued till we reach a
preset limit in the image, reach the edge of the bus, or reach an
object. The position of the object is known from the laser scanner data.
An orange line in Figure 17 indicates the curb ahead of the bus in our
example.

#### Calibration of sensors

The above mentioned method of determining the position of the curb in
front of the vehicle requires careful calibration of four sensors: Bus
state, curb detector (LLS), camera, and laser scanner.

The bus is the reference frame and therefore the location and
orientation of the other three sensors need to be determined with
respect to the bus. The locations of the sensors are all measured using
measuring tape. The laser scanner and the LLS are mounted on the bus in
such a way that the orientation of their internal reference frames is
either parallel or perpendicular to the axis of the bus reference frame.
This way the rotation from one to the other coordinate system is easy to
be determined.

Only the orientation of the camera is not trivial. First we want to
define the rotation matrix. We use roll angle φ, pitch angle θ, and yaw
angle ψ.

Equation 1 $R(\phi) = \begin{pmatrix}
\text{cos}(\phi) & - \text{sin}(\phi) & 0 \\
\text{sin}(\phi) & \text{cos}(\phi) & 0 \\
0 & 0 & 1
\end{pmatrix}$

Equation 2 $P(\theta) = \begin{pmatrix}
1 & 0 & 0 \\
0 & \text{cos}(\theta) & - \text{sin}(\theta) \\
0 & \text{sin}(\theta) & \text{cos}(\theta)
\end{pmatrix}$

Equation 3 $Y(\psi) = \begin{pmatrix}
\text{cos}(\psi) & 0 & - \text{sin}(\psi) \\
0 & 1 & 0 \\
\text{sin}(\psi) & 0 & \text{cos}(\psi)
\end{pmatrix}$

Equation 4 $C = \begin{pmatrix}
0 & 0 & 1 \\
1 & 0 & 0 \\
0 & 1 & 0
\end{pmatrix}$

The matrix which rotates the camera coordinates to the vehicle
coordinates is:

Equation 5 $A = C \cdot R \cdot P \cdot Y$

The matrix C takes care of the different conventions for the orientation
of the axis. For the vehicle the x-axis is forward, the y-axis points to
the right, and the z-axis points down. For the camera the z-axis is in
the forward direction, the x-axis points to the right, and the y-axis
points down.

The goal is now to find the three angles which describe the orientation
of the camera. For this one can make use of the image provided by the
camera. The distance between two pixels in the image correspond to about
1/10 of a degree in the real world and therefore one can measure angles
quite accurately with the help of the image. Each point in the image has
one horizontal and one vertical angle. Since we need to determine three
angles (roll, pitch, and yaw), we need at least two points in the image
and their corresponding points in the real world. To simplify the
problem we choose three points with following properties:

1.  The first point has the same y-position (in bus coordinates) as the
    camera.

2.  The second and third points are vertical to each other in the bus
    coordinate frame.

![](media/image17.png){width="3.895669291338583in"
height="2.9062992125984253in"}

[]{#__RefHeading___Toc96687049 .anchor}Figure 18 Example of three points
chosen for the calibration. Point *a* is on the ground at the front
wheel at the same y-position as the camera. Points *b* and *c* are edges
at the lower and upper part of the open front door.

If the three angles are small, the solution is to a good approximation:

Roll = difference in angle between the vertical and the line *b-c*.

Yaw = horizontal angle of *a*.

Pitch = vertical angle of *a* after correction of fact that *a* is not
at the same height as the camera.

We found out that this approximation is not always good enough and we
worked out the exact solution.

The three points are expressed in homogenized coordinates:

Equation 6 $a = \begin{pmatrix}
a_{1} \\
a_{2} \\
1
\end{pmatrix}$, $b = \begin{pmatrix}
b_{1} \\
b_{2} \\
1
\end{pmatrix}$, $c = \begin{pmatrix}
c_{1} \\
c_{2} \\
1
\end{pmatrix}$

All vectors in the following section are homogenized, *i.e.* they are
always divided by their own third component so that the third component
becomes 1.

The approximate yaw angle is:

Equation 7 $\psi_{a} = \text{tan}^{- 1}(a_{1})$

To get the approximate pitch angle, we need to do following calculation:

Equation 8 $a_{y} = Y(\psi_{a}) \cdot a$

Equation 9
$\theta_{a} = \text{tan}^{- 1}(a_{y2}) + \text{tan}^{- 1}(\frac{\Delta z}{\Delta x})$

where Δ*z* and Δ*x* are the distances in the *z* and *x* directions (bus
coordinates) between the camera and point *a*.

Now we can construct following three points by rotating the points *a*,
*b*, and *c*:

Equation 10 $a_{\text{yp}} = P(\theta_{a}) \cdot Y(\psi_{a}) \cdot a$

Equation 11 $b_{\text{yp}} = P(\theta_{a}) \cdot Y(\psi_{a}) \cdot b$

Equation 12 $c_{\text{yp}} = P(\theta_{a}) \cdot Y(\psi_{a}) \cdot c$

Now we need to find a rotation which will make the line *b~yp~-c~yp~*
vertical while leaving *a~yp~* unchanged, *i.e.* rotate around *a~yp~*
Therefore we need to solve following equation for the rotation angle
φ~a~:

Equation 13
$(S(\phi_{a}) \cdot b_{\text{yp}})_{1} - (S(\phi_{a}) \cdot c_{\text{yp}})_{1} = 0$

where the index 1 means the first component of the vector (which of
course has been homogenized). The rotation *S* is defined as

Equation 14
$S(\phi_{a}) = P^{- 1}(\alpha) \cdot R(\phi_{a}) \cdot P(\alpha)$

Equation 15 $\alpha = \text{tan}^{- 1}(a_{\text{yp}2})$

Equation 13 is the condition that the two points become vertical,
Equation 14 is the rotation around the point *a~yp~*.

Putting all the numbers into the equations and doing the multiplications
is tedious but straightforward until one reaches an equation of the
form:

Equation 16
$\text{cos}(\phi_{a}) \cdot x - \text{sin}(\phi_{a}) \cdot y = z_{1}$

The solution to this equation is:

Equation 17
$\phi_{a} = \pm \text{cos}^{- 1}\left( \frac{x \cdot z_{1} + y \cdot z_{2}}{\sqrt{(x^{2} + y^{2})(z_{1^{2}} + z_{2^{2}})}} \right)$

With

Equation 18 $z_{2} = - \sqrt{x^{2} + y^{2} - z_{1^{2}}}$

The angle φ~a~ is negative if

Equation 19
$\text{cos}(\phi_{a}) \cdot x - \text{sin}(\phi_{a}) \cdot y - z_{1} > 0$

Now one can construct the full rotation matrix:

Equation 20
$A = C \cdot P^{- 1}(\alpha) \cdot R(\phi_{a}) \cdot P(\alpha) \cdot P(\theta_{a}) \cdot Y(\psi_{a})$

Notice that this equation does not contain the usual roll angle φ, pitch
angle θ, and yaw angle ψ. If desired, one can determine them in the
following way:

Equation 21 $\phi = \text{tan}_{2^{- 1}}(a_{\text{32}},a_{\text{33}})$

> $$\begin{array}{r}
> \theta = - \text{sin}^{- 1}(a_{\text{31}}) \\
> \psi = \text{tan}_{2^{- 1}}(a_{\text{21}},a_{\text{11}})
> \end{array}$$

where *a~ij~* are the components of the matrix *A* and tan~2~^-1^(*x,y*)
is the inverse tangent which takes appropriate care of the sign with
respect to the four quadrants.

The solution has been implemented in a Matlab^TM^ program. This
Matlab^TM^ program includes an interface which lets you choose the
points by clicking on the image. It does the calibration on full images
as shown in Figure 18 or on quad-images. The calibration is also used to
do image overlays as one can see in Figure 17 on the right.

## PC-104 Platforms

### SCWS PC-104 platforms

All computers contain processor boards from various manufacturers, all
based on the Intel Pentium III with speeds ranging from 700 MHz to 1.26
GHz. This part selection was motivated by the need for the most powerful
processor available at the time in a rugged PC/104 form factor that is
capable of withstanding extreme temperatures.

All computers run Red Hat Linux 7.2 with a 2.4.18 kernel and Ha

patched to reduce kernel latencies. This ensures that a heavily loaded
computer is more responsive and allows us to use Linux for this data
driven very soft real-time task. The benefit of this, as opposed to a
real time system, is in ease of use.

The left and right computers are almost identical, physically. Minor
hardware and software changes are all that is required to interchange
the two.

+-----------------------+-----------------------+-----------------------+
| PC/104 Stack          | Left                  | Right                 |
+-----------------------+-----------------------+-----------------------+
| CPU                   | J1708 Interface       | IMU                   |
|                       |                       |                       |
| 2 Serial Ports        |                       |                       |
|                       |                       |                       |
| 100 Mbps Ethernet     |                       |                       |
|                       |                       |                       |
| 40 GB Notebook Hard   |                       |                       |
| Disk                  |                       |                       |
+-----------------------+-----------------------+-----------------------+
| Frame Grabber         | Forward Looking Curb  | Striper/Curb Detector |
|                       | Detector              |                       |
+-----------------------+-----------------------+-----------------------+
| High Speed Serial     | Left SICK Data, FCWS  | Right SICK Data,      |
| Ports (4)             | Interface, DINEX      | Odometry              |
+-----------------------+-----------------------+-----------------------+
| Sound Card            | (N/A)                 | Driver Vehicle        |
|                       |                       | Interface             |
+-----------------------+-----------------------+-----------------------+
| Digital I/O           | Left SICK Power and   | Right SICK Power and  |
|                       | Retraction, Left DVI  | Retraction, Striper   |
|                       |                       | Power, Right DVI      |
+-----------------------+-----------------------+-----------------------+
| Power Supply          |                       |                       |
+-----------------------+-----------------------+-----------------------+

[]{#__RefHeading___Toc96687000 .anchor}Table 7. Configuration of Left
and Right SCWS Computers

Measurement of CPU loading on each of the three computers indicates that
our 600MHz left computer is 65% loaded, our 1.2GHz right computer is 45%
loaded, and our 700MHz data logging computer (Digital Video Recorder) is
60% loaded.

### FCWS PC-104 platforms

The FCWS system hardware is composed of sensors, an engineering data
recording- processing computer and a video-recording computer, as
illustrated in the figure below. The engineering data that sensors send
out is recorded and processed by a PC104 computer system. Besides
regular ports for a PC104 computer, it has a digital I/O card, an
Analog/Digital I/O card, a CAN card which reads J data bus, a Serial
Port card and a counter/timer card.

A sensor arrangement for FCWS is designed to include sensors to detect
frontal and frontal corner obstacles and to monitor steering angle
movement, brake pressure, throttle position, vehicle velocity and
acceleration. Video data from the cameras is recorded using another
PC104 computer.

![](media/image18.wmf)

[]{#__RefHeading___Toc96687050 .anchor}Figure 19. FCWS System
Architecture

The figure below shows the layout of the computer enclosure. The
computer enclosure contains an engineering computer, a video recorder,
electronics circuits including battery ignition monitoring and shutdown
circuitry, power adapters, bus power bars and cable connectors.

![](media/image19.wmf)

[]{#__RefHeading___Toc96687051 .anchor}Figure 20. FCWS Computer
enclosure on the bus (top view)

## Digital Video Recorder PC-104 Platforms

### SCWS Digital Video Recorder PC-104 platform

The digital video recorder / data repository computer also serves as an
Internet gateway via a cellular telephone modem. This provides remote
system monitoring; something we have found quite useful when managing
such a complex system in the field.

+-----------------------------------+-----------------------------------+
| PC/104 Stack                      |                                   |
+-----------------------------------+-----------------------------------+
| PCMCIA Interface                  | PCMCIA Cellular Modem Adapter     |
+-----------------------------------+-----------------------------------+
| MPEG-1 Hardware Encoder           | External Video Cameras            |
+-----------------------------------+-----------------------------------+
| CPU                               | GPS for Location Tagged Data      |
|                                   |                                   |
| > 2 Serial Ports                  |                                   |
| >                                 |                                   |
| > 100 Mbps Ethernet               |                                   |
| >                                 |                                   |
| > Removable Disk Drive            |                                   |
+-----------------------------------+-----------------------------------+
| Power Supply                      |                                   |
+-----------------------------------+-----------------------------------+

[]{#__RefHeading___Toc96687001 .anchor}Table 8. Configuration of SCWS
Digital Video Recorder

Initially, the removable hard drive was a 250 GB desktop model, the
largest drive available. It was hoped that although the environmental
vibration and shock would well exceed the manufacturer's specifications,
the drive would still function most of the time. This has proven to not
be the case. For this reason, the two 80 GB notebook drives provide
enough data storage for about 2 weeks of use, as opposed to the 3+ weeks
with the larger drive.

### **FCWS Digital Video Recorder PC-104 platforms**

The cameras capture the front road scene, the left and right front
corner road scene, and the passenger compartment of the bus. The video
streams from the four cameras are combined into one video stream by a
quad image combiner to extend the hard drive storage capacity. The
video-in port of video recording computer is connected to the video-out
port of the quad combiner by a75$\Omega$ video cable.

![](media/image20.wmf)

[]{#__RefHeading___Toc96687052 .anchor}Figure 21. FCWS Video
recorder-camera interface

The video recording system is a standalone PC/104 system with a video
board. It reads commands from the engineering computer and records the
MPEG video clips to a removable hard drive. The video is recorded at
1Mbps, which is about 450MBytes per hour. The specifications of the
board are as follows:

  -------------------------------------- --------------------------------
  **General specifications**             

  Capture rate                           30 frames/sec (NTSC, RS-170,
                                         CCIR)\
                                         25 frames/sec (PAL)

  A/D resolution                         8-bits for luminance\
                                         8-bits for chrominance

  Output resolution                      768 x 576 (PAL)\
                                         640 x 480 (NTSC, RS-170)

  Video inputs                           4 multiplexed input channels
                                         total: 2  S-video or 4
                                         composite.

  Video output                           PAL or NTSC from a BNC connector

  Output data                            MPEG2 streaming data at rates of
                                         100 kbits to 10 Mbits/second

  Bus structure                          PC/104

  Board size                             3.80" x 3.55"

  Input power                            5 volts at 280 mA

  Number of cards per system             2

  Supporting operating systems           Windows, Linux, QNX6
  -------------------------------------- --------------------------------

[]{#__RefHeading___Toc96687002 .anchor}Table 9. Video board
specifications

The video board supports variable bit rates (number of bits of the
stored video data per second).

### SCWS timing synchronization

Internal to the SCWS, NTP (Network Time Protocol) is used to synchronize
the clocks on our three computers over the Ethernet network. The right
computer is considered the master clock, independent even of the more
accurate GPS clock, which is however slow to converge and unreliable as
the bus moves. Upon system boot, the left computer and the data storage
computer resynchronize their clocks to the master clock to correct for
temperature induced clock drift, which is especially noticeable when the
computers have been exposed to extreme temperatures. Thereafter, the NTP
daemon on the slave computer uses the network to statistically sample
the master clock so that it can determine the error on the local clock.
It corrects this error by effectively speeding up or slowing down the
local clock to close the difference. The clock is always monotonically
increasing, and without steps in time.

### FCWS timing synchronization

The following serial ports on the engineering computer are used for
synchronization between the FCWS engineering computer and the video
recorder:

Port 1: Sensor-Video Computer Communications (115200 Baud)

Port 8: (RS-232) Video timestamper (9600 baud)

The video files and the sensor file need to be synchronized to describe
the same scenario. The video recorder reads commands from the
engineering computer and records the MPEG video clips to a removable
hard drive. The commands from the engineering computer are "begin
recording (with a time stamp)," and "stop recording". Every time the
video recorder gets a "begin record" command it closes the old video
file, opens up a new file (named by the time stamp) and starts
recording.

![](media/image21.wmf)

[]{#__RefHeading___Toc96687053 .anchor}Figure 22 FCWS Warning signal
definition

### FCWS / SCWS data synchronization

Data between the FCWS and the SCWS is a serial port on the SCWS left
computer and the FCWS at 115 Kbaud. Data that is exchanged by the
computers is sent with no time tag, but is saved by the receiver with
the receiver's time tag

### FCWS / SCWS data protocol

Each message consists of a Header, ID, Length, Data, Checksum values.
The Header is a four character sequence (HEADER0 ... HEADER3). The ID
is a message identification byte.  Messages from the FCWS computer to
the SCWS computer will have odd ID numbers.  Messages from the SCWS
computer to the FCWS computer will have even ID numbers.  This is a two
byte value. The length is the total number of data bytes.  The length
does not include itself, the header, ID, or checksum.  This is a 2-byte
sequence. Data is an array denoting the length of bytes. The checksum is
the last byte per message. The checksum is a two\'s complement of the
sum of all the prior bytes in the message, including the header, ID,
length, and data.  The two\'s complement is used so that if all of the
bytes of the message (including checksum) are summed by the receiver,
the result is zero for a valid message. Specific values and parameters
are shown below:

  ----------------------------------- -----------------------------------
  HEADER0                             0x99

  HEADER1                             0x44

  HEADER2                             0x22

  HEADER3                             0x66

                                      

  PATH_TO_CMU_ID                      1

  CMU_TO_PATH_ID                      2

                                      

  Bytes for status flags              

  FRONT_DOOR_OPEN                     0x01

  REAR_DOOR_OPEN                      0x02

  RIGHT_TURN_SIGNAL_ON                0x04

  LEFT_TURN_SIGNAL_ON                 0x08

  HAZARD_LIGHTS_ON                    0x10

  POWER_DOWN                          0x20

  OVERRIDE_ON                         0x40

  IN_REVERSE                          0x80

                                      

  UNKNOWN_POSITION_CM                 -10000

  UNKNOWN_POSITION_M                  (UNKNOWN_POSITION_CM/10.0)

                                      

  Bit numbers for warning message     
  from SCWS to FCWS                   

  RIGHT_FRONT_LOW_ALERT               0 

  RIGHT_FRONT_LOW_WARN                1 

  RIGHT_FRONT_MEDIUM_ALERT            2 

  RIGHT_FRONT_MEDIUM_WARN             3 

  RIGHT_FRONT_HIGH_ALERT              4 

  RIGHT_FRONT_HIGH_WARN               5 

  RIGHT_REAR_LOW_ALERT                6 

  RIGHT_REAR_LOW_WARN                 7 

  RIGHT_REAR_MEDIUM_ALERT             8 

  RIGHT_REAR_MEDIUM_WARN              9 

  RIGHT_REAR_HIGH_ALERT               10 

  RIGHT_REAR_HIGH_WARN                11 

  LEFT_FRONT_LOW_ALERT                12 

  LEFT_FRONT_LOW_WARN                 13

  LEFT_FRONT_MEDIUM_ALERT             14 

  LEFT_FRONT_MEDIUM_WARN              15

  LEFT_FRONT_HIGH_ALERT               16 

  LEFT_FRONT_HIGH_WARN                17 

  LEFT_REAR_LOW_ALERT                 18 

  LEFT_REAR_LOW_WARN                  19

  LEFT_REAR_MEDIUM_ALERT              20 

  LEFT_REAR_MEDIUM_WARN               21 

  LEFT_REAR_HIGH_ALERT                22 

  LEFT_REAR_HIGH_WARN                 23 

  RIGHT_NOTIFY                        24

  LEFT_NOTIFY                         25

  RIGHT_UNDER_WHEEL                   26

  LEFT_UNDER_WHEEL                    27

  LOW_SETTING                         28

  MEDIUM_SETTING                      29

  HIGH_SETTING                        30
  ----------------------------------- -----------------------------------

[]{#__RefHeading___Toc96687003 .anchor}Table 10. Parameter Values

#### Data sent from the FCWS to the SCWS 

+--------------------+-------------------------------------------------+
| timestamp_secs     | Number of seconds since 1/1/1970                |
+--------------------+-------------------------------------------------+
| timestamp_usecs    | Additional microseconds                         |
+--------------------+-------------------------------------------------+
| Warning_msgs       | Warning field                                   |
+--------------------+-------------------------------------------------+
| Forward object of  |                                                 |
| interest,          |                                                 |
| z=-10000, x=-10000 |                                                 |
| means no object    |                                                 |
+--------------------+-------------------------------------------------+
| > front_obj_x      | Longitudinal position of object (= x in SCWS)   |
+--------------------+-------------------------------------------------+
| > front_obj_y      | Lateral position of object (= -y in SCWS)       |
+--------------------+-------------------------------------------------+
| >                  | Orientation of object velocity vector (=        |
|  front_obj_heading | -heading in SCWS)                               |
+--------------------+-------------------------------------------------+
| > front_obj_speed  | Left object speed along heading direction       |
+--------------------+-------------------------------------------------+
| sound_index        | Index of sound in sound directory, -1 for none  |
+--------------------+-------------------------------------------------+
| sound_bearing      | Left to right bearing of sound, percentage      |
+--------------------+-------------------------------------------------+
| curb_loc_2x        | Longitudinal curb position (= z in FCWS)        |
+--------------------+-------------------------------------------------+
| Status             | (Note: REAR_DOOR_OPEN, HAZARD_LIGHTS_ON, and    |
|                    | OVERRIDE_ON are not produced by the FCWS)       |
+--------------------+-------------------------------------------------+

[]{#__RefHeading___Toc96687004 .anchor}Table 11. Data sent from the FCWS
to the SCWS

#### Data sent from the SCWS to the FCWS 

+--------------------+-------------------------------------------------+
| timestamp_secs     | Number of seconds since 1/1/1970                |
+--------------------+-------------------------------------------------+
| timestamp_usecs    | Additional microseconds                         |
+--------------------+-------------------------------------------------+
| warning_msgs       | Warning field                                   |
+--------------------+-------------------------------------------------+
| brake_pressure     | Brake pressure - currently always 0             |
+--------------------+-------------------------------------------------+
| Latitude           | GPS latitude                                    |
+--------------------+-------------------------------------------------+
| Longitude          | GPS longitude                                   |
+--------------------+-------------------------------------------------+
| Altitude           | GPS altitude                                    |
+--------------------+-------------------------------------------------+
| Speed              | Speed from vehicle state estimation in km/hour  |
+--------------------+-------------------------------------------------+
| Left object of     |                                                 |
| interest,          |                                                 |
| x=-10000, y=-10000 |                                                 |
| means no object    |                                                 |
+--------------------+-------------------------------------------------+
| > left_obj_x       | Longitudinal position of object (= z in FCWS)   |
+--------------------+-------------------------------------------------+
| > left_obj_y       | Lateral position of object (= -x in FCWS)       |
+--------------------+-------------------------------------------------+
| > left_obj_heading | Orientation of object velocity vector (=        |
|                    | -heading in FCWS)                               |
+--------------------+-------------------------------------------------+
| > left_obj_speed   | Left object speed along heading direction       |
+--------------------+-------------------------------------------------+
| Right object of    |                                                 |
| interest,          |                                                 |
| x=-10000, y=-10000 |                                                 |
| means no object    |                                                 |
+--------------------+-------------------------------------------------+
| > right_obj_x      | Longitudinal position of object (= z in FCWS)   |
+--------------------+-------------------------------------------------+
| > right_obj_y      | Lateral position of object (= -x in FCWS)       |
+--------------------+-------------------------------------------------+
| >                  | Orientation of object velocity vector (=        |
|  right_obj_heading | -heading in FCWS)                               |
+--------------------+-------------------------------------------------+
| > right_obj_speed  | Right object speed along heading direction      |
+--------------------+-------------------------------------------------+
| Tracked and        |                                                 |
| predicted curb     |                                                 |
| locations, ordered |                                                 |
| in increasing x    |                                                 |
| (longitudinal)     |                                                 |
+--------------------+-------------------------------------------------+
| > curb_loc_1x      | Longitudinal curb position (= z in FCWS)        |
+--------------------+-------------------------------------------------+
| > curb_loc_1y      | Lateral curb position (= -x in FCWS)            |
+--------------------+-------------------------------------------------+
| > curb_loc_2x      | Longitudinal curb position (= z in FCWS)        |
+--------------------+-------------------------------------------------+
| > curb_loc_2y      | Lateral curb position (= -x in FCWS)            |
+--------------------+-------------------------------------------------+
| > curb_loc_3x      | Longitudinal curb position (= z in FCWS)        |
+--------------------+-------------------------------------------------+
| > curb_loc_3y      | Lateral curb position (= -x in FCWS)            |
+--------------------+-------------------------------------------------+
| > curb_loc_4x      | Longitudinal curb position (= z in FCWS)        |
+--------------------+-------------------------------------------------+
| > curb_loc_4y      | Lateral curb position (= -x in FCWS)            |
+--------------------+-------------------------------------------------+
| > curb_loc_5x      | Longitudinal curb position (= z in FCWS)        |
+--------------------+-------------------------------------------------+
| > curb_loc_5y      | Lateral curb position (= -x in FCWS)            |
+--------------------+-------------------------------------------------+
| curr_curb_loc_x    | Current Longitudinal curb position (= z in      |
|                    | FCWS)                                           |
+--------------------+-------------------------------------------------+
| curr_curb_loc_y    | Current Lateral curb position (= -x in FCWS)    |
+--------------------+-------------------------------------------------+
| sound_index        | Index of sound in sound directory, -1 for none  |
+--------------------+-------------------------------------------------+
| sound_bearing      | Left to right bearing of sound, percentage      |
+--------------------+-------------------------------------------------+
| Status             | (Note: IN_REVERSE is not produced by the SCWS)  |
+--------------------+-------------------------------------------------+

[]{#__RefHeading___Toc96687005 .anchor}Table 12. Data sent from the SCWS
to the FCWS

# System Software {#system-software .unnumbered}

## SCWS Software Architecture Development

The SCWS uses software architectural and communications tools that were
originally developed to support the ongoing robotics research of the
Navlab project.[^4] The architectural tools allow algorithm developers
to view the rest of the system through a set of abstract, reconfigurable
interfaces. In the initial development and ongoing debugging of an
algorithm or in the post development analysis of data, the interfaces
can be configured to read data from time tagged files using a common set
of data access tools. As the algorithm matures, the interfaces can be
reconfigured to use a common set of inter-process communications tools
which integrate the individual algorithm into the larger system running
in the field.[^5]

### Inter-process communications

The vast majority of inter-process communications in the SCWS can be
considered as analogous to signals in electronics. These are repeated
estimations of a consistently changing value, such as the most recent
line scanner data or most recent set of tracked objects in the
environment. It doesn\'t truly matter if the recipient misses a signal
value: all that matters is the most recent value. What does matter is
the minimization of latencies in transporting the signal value from
producers to consumers. An appropriate paradigm for propagation of
signal type information is global shared memory: A producer sets the
memory and a consumer simply reads the most recent value. The Neutral
Messaging Library (NML) from the Real-Time Control System (RCS)[^6]
library produced by NIST demonstrates this control-centric method for
integrating robotic systems. We have chosen a simpler implementation
than NML for global shared memory which uses a \"single-writer,
multiple-reader\" model. When processes are communicating on the same
machine we use actual System V shared memory, whereas when processes are
communicating between machines we transparently propagate changing
memory values from writer to readers via the UDP socket protocol managed
by shared memory managers running on each machine.

One of the reasons we chose to implement a simple shared memory
communications scheme rather than adopting NML was that while signals
make up the bulk of the communications, signals are not the only
paradigm for inter-process communications in a robotic system. Symbols,
i.e., atomic pieces of information, changes in state, or requests for
information, are very difficult to communicate via a signal-based
communications paradigm. For example, unlike signals, if a symbol value
is dropped or missed, then information is lost, state changes don\'t get
noticed, and requests are ignored. The guarantee that a symbol has been
transported from writer to reader is worth significant additional
latency and complexity in the implementation. Symbolic information is
typically communicated in robotic systems via TCP/IP message based
packages

ranging in complexity from the raw use of socket libraries all the way
up to complex, object based systems such as the Common Object Request
Broker Architecture (CORBA).[^7] In order to limit the complexity and
size of our software while still providing some abstraction and
flexibility, we have chosen a simple TCP/IP based messaging package
developed for the Navlab project: The Inter-Process Toolkit (IPT).[^8]

A key abstraction built in the SCWS using the messaging toolkit is the
concept of a central black board. Individual algorithms mainly query the
black board for their configuration parameters, but they can also post
information in the black board and watch for changes in values on the
black board. Thus, the black board becomes a channel for propagating
information through the system that has to be generally available, but
for which a certain degree of latency is acceptable. For example, when a
driver sets the sensitivity switch to different levels, this causes a
change to be posted to the warning levels stored in the central
blackboard. These changes are then propagated to the warning algorithm
automatically and transparently. In addition, since much of the
system\'s high level information is being funneled through the
blackboard, we have chosen to make the black board manager the system
process manager. It initiates, parameterizes, and monitors the system
processes. Interestingly, this paradigm of a central black board was one
of the earliest used in robotics [^9], but it has been often rejected
because if the black board is the only means for propagating information
through the system, it becomes an intolerable bottleneck for the kind of
low-latency, high-bandwidth signal-type information that forms the
backbone of information flow for a real robotic system.

Thus we see the core of our communications philosophy: Instead of having
one tool or one approach, which must be bent and stretched to handle all
possible uses, we select a suite of simple tools, each one narrowly
focused on a particular style of communications necessary for the
efficient and successful operation of the system.

### Vehicle state propagation

A fundamental question for most mobile robots is, \"where am I?\" For
almost every module in the SCWS this question needs to be answered
before going on to \"what am I seeing?\" Thus, a fundamental part of the
SCWS architecture is the ubiquitous availability of vehicle state, i.e.,
the estimate of where the vehicle is, where it is pointing, and where it
is going.

In past Navlab systems, the question of \"where am I?\" was assumed to
mean \"where am I right now?\". Thus, perception algorithms would ask
\"where am I right now?\", get the answer from the pose estimation
system, and then apply that to the latest sensor information. This works
fine when a robot is moving slowly, at a few meters per second, but when
a robot is moving fast, at 10, 20, or even 30m/s, small discrepancies in
time between the latest sensor pose estimation and the latest sensor
information can lead to significant errors in placing that sensor
information in the world, and thus to significant errors in operation.

The goal of the current Navlab pose estimation system used in the SCWS
is to allow perception algorithms on any machine in the system ask
\"where was I at time T?\", where T is the time stamp of some relevant
sensor event.

The pose propagation architecture we use is shown below. On one machine
there is a pose estimation system connected to all the various sensors
which is repeatedly answering the question \"where am I?\" Each pose
estimate is put into a ring buffer in shared memory that any process on
that machine can access. The user pose estimation routines take a time
tag, and attempt to interpolate (or extrapolate a small amount) in this
pose history buffer to come up with the best estimate of the vehicle
pose at the requested time. When a new pose estimate is created, in
addition to being entered in the local pose history table, it is sent
via the shared memory managers to every machine in the system. On each
of these client machines there is a process waiting for incoming pose
estimates and using them to build a pose history buffer which can be
used by other processes running on that machine to precisely match up
pose estimations with sensor time tags.

Of course, a potential weakness of this system is that the clocks on all
the machines must be precisely synchronized. Although we experimented
with using hardware solutions using the IRIG-B protocol to allow us to
have time estimates synchronized to within microseconds across machines,
we found that the freely available package NTP [^10] could synchronize
our clocks to within a millisecond across the machines, even in the face
of the harsh, changing environmental conditions encountered by a system
running over long periods of time on a transit bus. Millisecond
synchrony is more than sufficient for successful integration of vehicle
pose and sensor information even at high vehicle speeds.

### Data flow

Apart from the ubiquitous connections to the blackboard and vehicle
state propagation system, the data flow for the vast majority of
communications within the SCWS is fairly simple. The system has a left
side processor which contains most of the processes for producing
warnings on the left side of the bus, a right side processor for
producing warnings on the right side of the bus, and a central processor
responsible for managing the system, saving data from the left and ride
side processors, and saving a video record of the bus operation.

#### Left side data flow

On the left side of the bus, warnings are generated based only on the
laser range data.

-   The data from the SICK laser range finder is read in by a reflexive
    \"guarding\" module. This module monitors the returns from the laser
    range finder and the velocity of the bus to do a quick determination
    if the sensor will hit anything. If the algorithm sees an imminent
    collision, it sends the signal which retracts the sensor and flags
    the range data as \"bad\". The algorithm continues to monitor the
    environment as best as it can from its retracted position, and when
    it determines there is enough room to extend safely, it does so.
    This algorithm should be considered analogous to a \"flinching\"
    reaction in a human which keeps the sensor (and those the sensor may
    hit) safe. No matter what happens, the guarding module publishes the
    laser data to the rest of the system via shared memory. The guarding
    module also examines the quality of the data coming from the laser.
    If it detects too many permanent blockages, usually due to mud or
    dried road salt, it will retract the sensor until the sensor is
    cleaned.

```{=html}
<!-- -->
```
-   The detection and tracking of moving objects (DATMO) algorithm reads
    in laser data and vehicle state data via shared memory and produces
    a list of moving and stationary objects around the vehicle.

```{=html}
<!-- -->
```
-   The warning algorithm takes the list of moving and stationary
    objects around the vehicle and combines the vehicle state
    (specifically the bus velocity and turning speed) to predict
    collisions. It produces an annotated list of objects with warning
    classifications and overall warning level.

```{=html}
<!-- -->
```
-   The left Driver Vehicle Interface (DVI) control module watches the
    overall warning level and controls the appropriate lights on the
    left side to warn the driver about objects around the vehicle. The
    left DVI control module also monitors the sensitivity and override
    switches on the DICB and changes values in the blackboard in
    response.

#### Right side data flow

On the right side of the bus, the system uses a curb detection and
prediction system to augment the laser range data in generating
warnings.

-   The curb striper algorithm digitizes a laser strip painted on the
    curb and uses its knowledge of the intrinsic and extrinsic camera
    parameters to produce a set of detected 3D points to shared memory

```{=html}
<!-- -->
```
-   The curb processing algorithm combines the output of the curb
    striper with the vehicle state data to produce an estimate of where
    the curb was over the last few seconds. It then digitizes an image
    from the right rear forward looking camera and uses the curb
    estimate to initiate a visual search for the curb ahead of the bus.
    The resulting curb information is published via shared memory.

```{=html}
<!-- -->
```
-   The warning algorithm is configured to read the curb information and
    uses it to modify its warning level production.

```{=html}
<!-- -->
```
-   As with the left DVI control module, the right DVI control module
    monitors the right warning levels and sets the lights appropriately.
    The right DVI control module does not monitor the switches. That is
    done by the left DVI control module alone.

In addition, the right side processor has the vehicle state estimation
module that is connected to the various sensors and data source and
produces the actual vehicle state for the rest of the system.

#### Central processor data flow

The central processor is responsible for many of the data collection and
system management aspects of the system.

-   It runs the central blackboard and process management modules.

```{=html}
<!-- -->
```
-   The four external side bus cameras feed into a quad-combiner which
    then feeds into an MPEG encoder card which the central processor
    reads. The MPEG stream is time tagged and saved to disk

```{=html}
<!-- -->
```
-   It runs modules attached to all the other various shared memory
    outputs on the system, such as vehicle state estimation, laser range
    data, classified objects, etc., and saves them to disk.

### Integration with the FCWS

The connection to the Forward Collision Warning System (FCWS) is through
a serial link. There is a gateway module running on the left processor
which gathers together, packages, and writes the following information
over that serial link:

-   Warning levels

-   Nearest object position and speed for each side

-   Curb position, if any

-   GPS position

-   Vehicle speed

-   Auxiliary bus state information, such as door open/closed status or
    lights status.

The gateway module also monitors the output from the FCWS and saves the
following information:

-   Front warning levels

-   Nearest front object position and speed.

-   The FCWS\'s estimation of the auxiliary bus state information.

The bus state information is duplicated because the actual hardware
sensors may not be connected to the same system on different platforms.
For example, on the Pittsburgh bus the SCWS system has direct access to
the bus state information sensors and has a separate module to read and
publish these values, but on the San Mateo bus, the bus state
information is read from the FCWS and propagated to the rest of the
system by the gateway module.

## FCWS Software Introduction

This chapter focuses on the data acquisition program of the FCWS on
integrated Samtrans bus 601. This includes most of the interfaces that
serve as the bridge between the low-layer hardware/software drivers and
the upper-layer application programs such as warning algorithms. The
communication of FCWS and ICWS is specified in the ICD document.

![](media/image22.wmf)

[]{#__RefHeading___Toc96687054 .anchor}Figure 23 FCWS Data acquisition
program

The purpose of the data acquisition program is to save data from sensors
and synchronize the engineering computer with a video recorder.
Basically, the data acquisition program is comprised of an
initialization process and a loop body; the program has a short period
of time to save all files and then to abort when the power is turned
off. The loop body is composed of the following actions:

1.  Copy the sensor data from the database to the local memory.

2.  Save sensor data from the local memory to a set of disk files.

3.  Check power-off flag (if power-off flag is set, run power-off
    subroutine)

4.  Check time consumed for file collection.

5.  (If exceeds 15 minutes, open a new set of files)

6.  Generate synchronization signals.

7.  Wait for the 75ms flag.

The LIDAR data is saved every 75 ms, which is the lowest update rate.
About every 15 minutes old files will be closed and a new set of files
will be opened. A timestamp is also included with each entry.

### FCWS Software structure

![](media/image23.wmf)

[]{#__RefHeading___Toc96687055 .anchor}Figure 24 FCWS Software flow
chart

### FCWS Initialization

#### Define variables

##### File pointers (Global variables)

  ----------------------------------------- -----------------------------
  **File pointer**                          **Sensor**

  \*f_RADARA                                P-RADAR

  \*f_RADARB                                D-RADAR

  \*f_LIDARO                                F-LIDAR

  \*f_LIDARM                                P-LIDAR

  \*f_LIDARN                                D-LIDAR
  ----------------------------------------- -----------------------------

[]{#__RefHeading___Toc96687006 .anchor}Table 13. FCWS File pointers --
sensors

##### System signals

  ----------------------------------- -----------------------------------
  **Signal**                          **Description**

  SIGINT                              Interruption

  SIGQUIT                             Quit

  SIGTERM                             Terminate

  ERROR                               System error
  ----------------------------------- -----------------------------------

[]{#__RefHeading___Toc96687007 .anchor}Table 14. FCWS System signals

After initialization (signals added), whenever the program receives
these signals, it will close files, log out of the database and exit.
For example, Ctrl+C from the keyboard will generate a SIGTERM signal and
this program will receive the signal then close files, log out of the
database and exit.

##### Database variables

The following database variables are used for database read operation;
each variable (a structure) contains some variables and an unsigned
char. This char will be the pointer of the sensor data in the local
memory after the execution of clt_read () function (database read
operation).

  ------------------------------ ----------------------------------------
  **Variable**                   **Sensor**

  Db_data_radarA                 P-Radar

  Db_data_radarB                 D-Radar

  Db_data_LidarOA                F-Lidar(section A)

  Db_data_LidarOB                F-Lidar(section B)

  Db_data_lidarMA                P-Lidar(section A)

  Db_data_lidarMB                P-Lidar(section B)

  Db_data_lidarNA                D-Lidar(section A)

  Db_data_lidarNB                D-Lidar(section B)

  Db_data_long_input             Host-bus sensors

  Db_data_gps_gga                GPS

  Db_data_gps_vtg                GPS

  Db_data_jeec2                  J-bus

  Db_data_dduA                   DDU-display of P-Radar

  Db_data_dduB                   DDU-display of D-Radar
  ------------------------------ ----------------------------------------

[]{#__RefHeading___Toc96687008 .anchor}Table 15. FCWS Database variables
-- sensors

##### Sensor data pointers

The pointers listed below point to the sensor data in the local memory.
For instance, pRADAR gets its value from the database variable:
db_data_radarA or db_data_radarB that contains the pointer pointing to
the RADAR data. The data structures of RADAR, LIDAR, host-bus sensors
are shown in FCWS hardware documentation. These pointers are then used
to save sensor data from local memory to a hard disk.

  ---------------------------------- ------------------------------------
  **Pointers**                       **Sensor**

  \*pradar                           RADAR

  \*pddu_display                     DDU-display

  \*plidarA                          Lidar(section A)

  \*plidarB                          Lidar(section B)

  \*plong_input                      Host-bus sensors

  \*pgps_gga                         GPS (position)

  \*pgps_vtg                         GPS(speed)

  \*plong_jeec                       For J-bus
  ---------------------------------- ------------------------------------

[]{#__RefHeading___Toc96687009 .anchor}Table 16. FCWS Sensor data
pointers

##### Time variables

###### Start_time, Curr_time

These two variables are used to check the time consumed for file
collection, if Curr_time-Start_time\>15 minutes, the old files will be
closed and a new set opened.

###### Hour, minute, second, millisec

These four variables are used to generate the time of day an entry is
recorded.

#### Process user switches

A user should specify the time for file collection. In this program, 15
minutes are allotted for file collection. Command format: wrfiles3 --m
15

#### Open a serial port for the titler

This port is used to send current time (hour, minutes, second) to the
titler for adding a timestamp.

#### Log in to the database

In order to read data from the database, we need to get a node ID and
then log in to the database.

#### Get the current time

The current time is used to calculate time consumed for file collection.

#### Open files

  ---------------------------- ------------------------------------------
  **File name**                **Sensor**

  AMMDDSSS.dat                 P-Radar

  BMMDDSSS.dat                 D-Radar

  OMMDDSSS.dat                 F-Lidar

  MMMDDSSS.dat                 P-Lidar

  NMMDDSSS.dat                 D-Lidar

  EMMDDSSS.dat                 Host-bus sensors and others
  ---------------------------- ------------------------------------------

[]{#__RefHeading___Toc96687010 .anchor}Table 17. FCWS File name format

In the above names, MM is replaced by a 2-digit month code, DD is
replaced by a 2-digit day code, and SSS is replaced by a 3-digit serial
code. Serial codes for a given day start at 000 and proceed to 999.
Detailed information of the file format is in the program comments.

### FCWS Loop body

#### Database operations

The program copies the specified sensor data from the database to local
memory consecutively before performing any disk operations. (Please
note, we do not read data of a specified sensor, save it to a disk file,
and then read data of another sensor.) As a result, the timestamp of all
disk files can be consistent. The reason is that memory operation are
much faster than disk operations, given the same quantity of data
transmissions.

#### Disk file operations

Disk file functions save: the data of RADAR sensor and DDU display, two
sections of LIDAR sensor data (section A and section B combined) and
saves data of host-bus sensors, GPS, and J-bus data. All these save
functions perform memory read and disk write operations and add the same
timestamp to their files.

##### RADAR file format (P-RADAR, D-RADAR)

![](media/image24.wmf)

[]{#__RefHeading___Toc96687056 .anchor}Figure 25 FCWS RADAR file format

##### LIDAR file format (F-LIDAR: First generation) 

![](media/image25.wmf)

[]{#__RefHeading___Toc96687057 .anchor}Figure 26 FCWS lidar file format

##### Host-bus sensor file format

![](media/image26.wmf)

[]{#__RefHeading___Toc96687058 .anchor}Figure 27 FCWS Host-bus sensor
file format

#### Check power off flag

If the power off flag is set, there is less than 1 minute to close files
and clear video alarm signals. If power resumes within 1 minute, a new
set of files will be opened, a new timestamp will be sent to the video
recorder and all synchronization signals will be cleared before the
program check time consumed for file collection.

#### Check time to open a new set of files

If the current time - the start time \>15 minutes, the old files will be
closed and a new set of files will be opened. Whenever a new set is
opened the current timestamp will be sent to the video recorder through
the serial port already opened.

![](media/image27.wmf)

[]{#__RefHeading___Toc96687059 .anchor}Figure 28 Check time to open a
new set of files

### FCWS Synchronization

The video files recorded and the sensor file recorded need to be
synchronized to describe the same scenario. The engineering computer
will send the master time to the video recorder after ignition to
synchronize the two systems in real time. The video recorder will adjust
its clock accordingly. The engineering computer will send instructions
to the video recorder to open or close a file for video recording when
it opens or closes a new set of engineering files. The video recorder
also records the start time and end time of each video file for
synchronization verification.

### FCWS Program exit

The program will abort when the power is turned off for four minutes.
The program will exit when there are:

\(1\) Signals (added in initialization) received

\(2\) Invalid user switch or bad number of minutes for file collection.

\(3\) Failure to initialize the timer (75ms).

\(4\) Error in opening serial port for video timestamp.

\(5\) Database initialization error, database reading error, database
update error.

# Algorithm Development {#algorithm-development .unnumbered}

## Object Tracking Using Scanning Laser Rangefinders

CMU has developed software for the tracking of vehicles and pedestrians
using scanning laser rangefinders mounted on a moving vehicle. Although
the system combines various algorithms and empirical decision rules to
achieve acceptable performance, the basic mechanism is tracking of line
features, so we call this approach *linear feature tracking.*

There are three major parts to this presentation:

-   Introduction of the sensor characteristics, comparison with other
    tracking problems, and discussion of some specific problematic
    situations.

-   Presentation of the structure and algorithms used by the tracker.

-   Discussion of the performance and limitations of the current system.

### Input / Output example

To get some idea of what the tracker does, consider the tracker input
and output. Figure 29 is a portion of an input frame from the laser
rangefinder:

[]{#__RefHeading___Toc96687060
.anchor}![](media/image28.png){width="4.363888888888889in"
height="2.2493055555555554in"}Figure 29: Tracker input (one frame)

Figure 30 is a visualization of the tracker output. The numbers are
track identifiers, with additional information displayed for moving
tracks. Track 38 (brown) is a car moving at 5.7 meters/sec and turning
at 21 degrees/sec. The light blue arc drawn from track 38 is the
projected path over the next two seconds. The other tracks are
non-moving clutter objects such as trash cans and light poles. The
actual scanner data points are single pixel dots. The straight lines
have been fitted to these points. An X is displayed at the end of the
line if we are confident that the line end represents a corner.

[]{#__RefHeading___Toc96687061
.anchor}![](media/image29.png){width="6.020138888888889in"
height="1.976388888888889in"}Figure 30: Tracker output example

### Sensor characteristics

A laser rangefinder (or LIDAR) is an active optical position measurement
sensor. Using the popular time-of-flight measurement principle, a laser
pulse is sent out by the sensor, reflects off of an object in the
environment, then the elapsed time before arrival of the return pulse is
converted into distance. In a scanning laser rangefinder, mechanical
motion of a scanning mirror directs sequential measurement pulses in
different directions, permitting the building of an approximation of a
2D model of the environment (3D with two scan axes.) We will use the
term *scanner* as a shorter form of scanning laser rangefinder.

![](media/image30.png){width="4.553472222222222in"
height="3.420138888888889in"}

[]{#__RefHeading___Toc96687062 .anchor}Figure 31: Scanner angular
resolution

The scanning mirror moves continuously, but measurements are made at
discrete angle increments (see Figure 31.) Though this is not actually
how the scanner operates, the effects of angular quantization are easier
to understand if you visualize the scanner as sending out a fixed
pattern of light beams which sweep across the environment as the scanner
moves (sort of like cat whiskers.)

When viewed in the natural polar coordinates, the rotational (azimuth)
and radial (range) measurement errors are due to completely different
processes, and have different range dependence:

-   The azimuth error is primarily due to the angular quantization,
    though this is related to the underlying physical consideration of
    laser spot size. For a given beam incidence angle on the target, the
    Cartesian position uncertainty is proportional to the range.

-   The range measurement error comes from the per-pulse range
    measurement process, and in a time-of-flight system is largely due
    to the timer resolution. This results in a range accuracy that is
    independent of distance.

Linear feature tracking was developed for a collision warning system for
transit buses. This system uses the SICK LMS 200, which is a scanning
laser rangefinder with a single scan axis. The scanner is oriented to
scan in a horizontal plane, and all processing is done using 2D geometry
in this scan plane. Performance specifications are 1cm range resolution,
50 meter range, 1 degree basic azimuth resolution and 75 scans/second
update rate. The output of the scanner is simply a vector of 181 range
values. If the measurement process fails due to no detectable return,
this is flagged by a distinct large range value.

Note that with these range and angle resolutions, the position
uncertainty is dominated by azimuth quantization throughout the entire
useful operating range. At a practical extreme range of 20 meters, a one
degree arc is 34cm, whereas the range resolution is still 1cm.

### The tracking problem

Given this sensor, we would like to identify moving objects, determine
the position and velocity, and also estimate higher order dynamics such
as the acceleration and turn rate. The tracker must also be
computationally efficient enough so that it can process 75 scans a
second in an embedded system with other competing processes.

A *track* is an object identity annotated with estimates of dynamics
derived by observing the time-change. The function of the tracker is to
generate these tracks from a time-series of measurements. The purpose of
maintaining the object identity is twofold:

-   We need to establish object correspondences from one measurement to
    the next so that we can estimate dynamics.

-   The object identity is in itself useful as it allows us to detect
    when objects appear and disappear.

In general, tracking can be described as a three-step process which is
repeated each time a new measurement is made:

1.  *Predict* the new position of each existing track based on the last
    estimate of position and motion.

2.  *Associate measurement data* with existing tracks. If there is no
    good match, consider making a new track.

3.  *Estimate* new position and motion based on the difference between
    the predicted position and the measured one.

#### Comparison of tracking with laser scanner vs. other sensors

The problem of tracking moving objects using a scanning laser
rangefinder is in some ways intermediate in characteristics between long
range RADAR tracking (e.g. of aircraft) and computer vision tracking.

What advantages for object tracking does a laser scanner have over
computer vision? Two difficult problems in vision based tracking are:

-   *Position:* determination of the position of objects using vision
    can only be done using unreliable techniques such as stereo vision
    or assuming a particular object size. Position determination is
    trivial using ranging sensors like RADAR and laser scanners, as long
    as there is adequate angular resolution,

-   *Segmentation*: when two objects appear superimposed by our
    perspective, how do we tell where one ends and the next begins?
    Range measurement makes segmentation much easier because foreground
    objects are clearly separated from the background.

An important problem that laser scanners have in common with computer
vision is *point correspondence:* given two measurements of the same
object, which specific measurements correspond to the same point on the
object.

In long range RADAR, the point correspondence problem typically doesn\'t
exist \-- the object size is at or below the angular resolution, so the
object resembles a single point. In contrast, when a laser scanner is
used in an urban driving situation, we need to be able to track objects
whose size is 10 to 100 times our angular resolution. Not only do the
tracked vehicles not resemble points, after taking into consideration
the effect of azimuth resolution, they often effectively extend all the
way to the horizon in one direction.

When the size of objects can\'t be neglected, this creates ambiguity in
determining the position of the object (what point to use). Since a
tracker estimates dynamics such as the velocity by observing the change
in position over time, this position uncertainty can create serious
errors in the track dynamics.

As in computer vision, the extended nature of objects does also have
some benefits. Because we have multiple points on each object, we can
make use of this additional information to classify objects (bush, car,
etc.)

#### Shape change

It is a crucial aspect of the tracking problem considered here that the
laser rangefinder is itself in motion. If the scanner is not moving, the
problem of detecting moving objects is trivial: just look for any change
in the sensor reading. Once the scanner is moving, we expect fixed
objects to appear to move in the coordinates of the scanner, and can
correct for this with a coordinate transformation.

It is assumed that the motion of the scanner is directly measured, in
our case by a combination of odometry and an inertial turn rate sensor.
Since tracking is done over relatively short ranges and short periods of
time, the required accuracy of the estimate of scanner motion is not
great, and relatively inexpensive sensors can be used.

The more intractable difficulty related to scanning from a moving
vehicle is that, even after object positions are corrected by a
coordinate transform, the appearance still changes when we move due to
the changing scanner perspective. The scanner only sees the part of the
object surface currently facing the scanner. As the scanner moves around
a fixed object, we see different contours of the object surface.

The shape change doesn\'t cause any serious difficulty for determining
that scan data corresponds to the same object from one scan to the next
because the change is small. What is difficult is determining that these
small changes are due to changes in perspective, and not actual motion
of the tracked object.

![](media/image31.png){width="4.6409722222222225in"
height="2.8069444444444445in"}

[]{#__RefHeading___Toc96687063 .anchor}Figure 32: Shape change

To get a sense of the shape change problem, consider a naive algorithm
which considers the object position to be the mean position of the
object\'s measured points (see Figure 32.) Suppose that we are driving
past a parked car. At time 1, we see only the end of the car. At time 2,
we see both the side and end. By time 3, we only see the side. During
this process, the center of mass of the point distribution shifts to the
left, giving the parked car a velocity moving into our path, causing a
false collision prediction. The point distribution also moves in our
direction of motion creating false velocity in that direction.

#### Occlusion

Another problem happens when a small object moves in front of a larger
background object (see Figure 33.) In this case, what is in effect the
shadow of the foreground object creates a false moving boundary on the
background object (as well as splitting the background object in two.)
Due to the changing perspective, moving shadows also appear when both
objects are fixed but the scanner is moving.

![](media/image32.png){width="5.667361111111111in"
height="2.0701388888888888in"}

[]{#__RefHeading___Toc96687064 .anchor}Figure 33: Occlusion

#### 2D scan of a 3D world

Two major problems come from using a single axis scanner:

-   When the scanner pitches or rolls, we see a different contour of
    each object, and if the surface is not nearly vertical, we may see a
    large amount of motion.

-   When the ground is not flat, the scanner beam may hit the ground,
    resulting in seeing the ground itself as an obstacle. Due to pitch
    and roll, these ground-strike returns may also appear to be rapidly
    moving.

Use of a scanner with several beams that scan in parallel can greatly
help with this problem because we can detect when the beam is striking
an object that is significantly sloped, and either disregard it or
attempt to compensate in some way.

#### Vegetation

With some objects, the outline seen by the scanner appears to fluctuate
in a random way as the scanner moves. Vegetation has this problem.
Figure 34 shows the superimposed points from 20 scans combined with
markers for the points from one single scan. Clearly there is a great
deal of noisy fluctuation of the range measurements. Also, the
underlying outline which we can see in the superimposed scans is complex
enough to defy simple geometric models.

![](media/image33.png){width="5.999305555555556in"
height="4.499305555555556in"}

[]{#__RefHeading___Toc96687065 .anchor}Figure 34: Vegetation

#### Weak returns

Some objects have very poor reflectivity at the infrared frequency where
the SICK scanner operates. Figure 35 shows an example of a car that is
almost invisible to the scanner. During the 10 seconds that we drive by,
we are able to build up a reasonably complete idea of the car (small
dots), apparently largely from specular glints. However, on any given
scan, very little of the car is visible. In this particular single scan,
we are mainly seeing inside the wheel wells (oblong areas area inside
outline box.) Evidently the dirt inside the wheel well is a better
reflector than the paint.

![](media/image34.png){width="5.422916666666667in"
height="4.190972222222222in"}

[]{#__RefHeading___Toc96687066 .anchor}Figure 35: Weak returns

#### Clutter

Another cause of unclear object outlines is clutter: when objects are
close together. In this case, it isn\'t clear whether to segment the
data as one or two objects. If the segmentation flips between one and
two objects, this causes apparent shape change. Clutter can also cause
spurious disappearance of tracks, for example when a pedestrian moves
close to a wall, and appears to merge with the wall.

### Tracker structure and algorithms

These are the major parts of the tracker:

-   Segmentation: group scanner points according to which object they
    are part of.

-   Feature extraction: fit line and corner features.

-   Prior noise model: assign feature error covariances using a
    measurement error model.

-   Data association: find the existing track corresponding to each new
    segment, creating a new track if there is none.

-   Dynamic model and Kalman filter: determine velocity, acceleration
    and turn rate from the raw position measurements.

-   Track evaluation: assess the validity of the dynamic estimate and
    see if the track appears to be moving. Check how well the estimate
    "predicts" the measured past positions when time is reversed.

There are 60 numeric parameters used by the tracker. For concreteness
and conciseness, we will refer to the specific numeric values that have
been empirically tuned for our particular scanner and application,
rather than to parameter names. Generally the parameter values are not
all that sensitive, but for best performance with a different scanner or
application, different values would be used.

Also, since the source code is available and well commented, we will
avoid in-depth discussion of implementation details better read from the
source. In particular, although efficiency is one of the important
characteristics of the tracker, we won\'t do much in-depth discussion of
performance-related issues.

One performance consideration is worth discussing because it affects the
structure of the algorithm, especially in the segmentation and feature
extraction steps. We have exploited two major geometric constraints that
come from the use of a single scanner:

-   Given an assumption that all corners are 90 degrees, at any time it
    is possible to see at most two sides and three corners of an object.
    Data structures are designed for this fixed number of linear
    features, rather than an arbitrary number. This also simplifies the
    feature correspondence problem in data association.

-   In various places we exploit the assumption that the inherent
    azimuth ordering in the scanner output is also an ordering of
    consecutive points on the object surface.

Both of these assumptions break down if there is more than one scanner.
We have demonstrated one way to use multiple scanners: convert all the
scan points into a point cloud in Cartesian coordinates, and then
convert each point back to polar coordinates of a single "virtual
scanner." Although not a great solution, it does show that the
limitation to a single scanner can be relaxed.

#### Segmentation

Segmentation takes the list of 181 range and azimuth points returned by
the scanner and partitions it into sublists of contiguous points. Two
neighboring points are contiguous if the separation is less than 0.8
meters. After segmentation, the points are converted into a non-moving
Cartesian coordinate system by transforming out the effects of the known
motion of the scanner.

During segmentation we also classify each point as *occluded* or normal.
A point is occluded if an adjacent point in the scan is in a different
segment and in front of this point, or if it is the first or last point
in the scan. This flag has two uses:

-   When an occluded point appears at the boundary of an object, we
    consider this to be a false boundary (the feature is *vague.*)

-   We only count non-occluded points when determining if there enough
    points to create a new track or if the point density is high enough
    for a segment to be compact.

In segmentation, missing range returns are treated as points at maximum
distance, and are not assigned to any segment. If there is a large
enough dropout in the middle of an object, this splits the object into
two segments.

####  Linear feature extraction

For each segment, we do a least-squares fit to a line and to a
right-angle corner. Since the stability of feature locations is crucial
for accurate velocity measurement, there are two refinements to the
basic least-squares fit:

-   Points are weighted proportional to their separation along the line.
    Since some parts of the object may be much closer than others, the
    point density along the object contour can vary a great deal. This
    weighting reduces problems with rounded corners that have high point
    density causing the line fit to rotate away from more distant points
    that actually contain more information about the overall rectangular
    shape approximation.

-   The 20% of points with worst unweighted fit are discarded, and then
    we refit. Although this reduces sensitivity to outliers from any
    source, the scanner has little intrinsic noise, so the effect is
    mainly on real object features that violate the rectangular model,
    notably rounded corners and wheel wells.

Because conceptually both the point spacing (distance along the line)
and fit error (distance normal to the line) depend on the line (which is
what we are trying to find in the first place) we use an iterative
approximation. Each line fit requires three least-squares fits:

-   An equal-weight least-squares fit of all the points in the segment,
    used to determine the point spacing for weighting.

-   A trial weighted fit, used to determine the outlier points.

-   The final weighted fit.

The position of each line end-point is determined by taking the ending
point in the input points and finding the closest point lying on the
fitted line.

##### Corner fitting

Corner fitting is done after fitting as a line. This is a somewhat
degenerate case of a polyline simplification algorithm. We split the
point list in two at the knuckle point: the point farthest from the line
fit. The geometrically longer side is then fit as a line. Since we
constrain the corner to a right angle, the long side fit determines the
direction of the short side. All we need to do is determine the location
of the short side, which is done by taking the mean position along the
long side of the short-side points. The location of the corner itself is
the intersection of the two sides.

When doing the corner fit, we test for the corner being well-defined
(approximately right angle) by doing an unconstrained linear fit on the
short side, and testing the angle between the two sides. The angle must
be at least 50 degrees away from parallel to be considered a good fit.

The corner must also be convex, meaning that the point of the corner
aims toward the scanner (hence away from the interior of the opaque
object.) We impose this restriction because in practice concave corners
only appear on large fixed objects like walls, not on moving objects.

![](media/image35.png){width="4.149305555555555in"
height="3.765972222222222in"}

[]{#__RefHeading___Toc96687067 .anchor}Figure 36: Corner fitting

Figure 36 is an example of corner fitting in the presence of corner
rounding, outliers and variable point spacing. The fit matches the
overall outline accurately fairly accurately.

##### Shape classification

After fitting and line and corner, each segment is given a shape
classification:

  ------------- ------------------------------------------------------------
  **corner**    Corner fit mean-squared error less than line fit and less
                than 10 cm.

  **line**      Line fit mean-squared error less than 10cm.

  **complex**   Fall-back shape class for objects with poor linear fit.
  ------------- ------------------------------------------------------------

There are two Boolean shape attributes which are semi-independent from
the shape-class:

+----------+-----------------------------------------------------------+
| **c      | Diagonal of bounding box \< 0.7 meters and point density  |
| ompact** | \> 5 points/meter. The compact criterion is chosen so     |
|          | that pedestrians will be compact (as will lamp-posts,     |
|          | fire hydrants, etc.) Because compact objects are small,   |
|          | we can estimate their velocity reasonably accurately      |
|          | without having a good linear fit.                         |
+----------+-----------------------------------------------------------+
| **disor  | Rotation angle not well defined. True if the line or long |
| iented** | side of corner has less than 6 points, the RMS fit is     |
|          | worse than 4 cm, or the line and corner fit disagree by   |
|          | more than 7 degrees and the chosen classification's RMS   |
|          | error is less than 4 times better than the alternative.   |
|          | Segments that are complex or \< 0.7 meters diagonal are   |
|          | always disoriented.                                       |
|          |                                                           |
|          | The disoriented attribute is used to determine whether to |
|          | use the change in orientation for turn rate estimation.   |
|          | Also, if a segment is disoriented and not compact, then   |
|          | it has a poor linear fit, and we are more skeptical of    |
|          | the motion estimate.                                      |
+----------+-----------------------------------------------------------+

Due to the restrictions of single-scanner perspective and 90 degree
convex corners, there is a small fixed set of possible features that a
segment can have:

  ------------ ------------------------------------------------------------
  **min, max** The two diagonal corners of the bounding box (in world
               coordinates.) Defined for all shape classes.

  **first,     End points in a line segment, ends of the two sides in a
  last**       corner segment.

  **corner**   Corner point in a corner segment.

  **center**   Rough estimate of object center derived from other features.
               Defined for all shape classes.
  ------------ ------------------------------------------------------------

**Figure 37** illustrates a segment with all features present.

![](media/image36.png){width="5.420138888888889in"
height="3.1256944444444446in"}

[]{#__RefHeading___Toc96687068 .anchor}Figure 37: Features

#### Feature noise model

We observed earlier that due to the distinct angular and range
resolution of the scanner, the shape of the position uncertainty in
Cartesian coordinates is strongly asymmetrical.

The Kalman filter used in the tracker uses a statistical model of error
in the measurement. If the actual measurement and modeling errors are
white and zero-mean, then the Kalman filter is optimal. In practice, our
errors differ greatly from this ideal, so there is no theoretical
optimality. However inaccurate, in order to use the Kalman filter, we
must attempt to capture the measurement error characteristics as a
two-dimensional position covariance.

There are two parts of the noise model: the *prior* noise and the
*adaptive* noise. The prior noise is determined from the segment points
for one single scan, and is based mainly on the geometric properties of
the scanner. The adaptive noise is estimated as part of the filter
update, and will be discussed later.

The prior noise for linear features is computed from separate
longitudinal and lateral variances that are then rotated into the world
coordinates. The lateral variance is 1 cm or the mean-square line fit
error, whichever is greater.

![](media/image37.png){width="6.020138888888889in"
height="2.047222222222222in"}

[]{#__RefHeading___Toc96687069 .anchor}Figure 38: Incidence angle and
point spacing

Geometrically, the longitudinal measurement uncertainty goes to infinity
as the scanner ray approaches parallel to the line (see Figure 38.) Due
to the segmentation algorithm, the observable inter-point spacing is
limited to the segment threshold (0.8 meters.) The prior longitudinal
variance is (0.3 \* *spacing*) 2, where *spacing* is the maximum
inter-point spacing of the last 7 points at the end of the line. Using
the actual inter-point spacing incorporates the geometric angular
resolution effect, and also additionally discriminates against objects
with missing returns (due to poor reflectivity.)

If a segment is disoriented, we are unsure of the orientation, so also
unsure of the orientation of the measurement error uncertainty. In this
case we bound the ratio of the longitudinal and lateral variance to 5 by
increasing the lateral variance.

##### Vague line ends

A line-end feature can be marked as *vague,* which means that its
longitudinal position is so poorly determined that the endpoint position
should be regarded as arbitrary by the tracker. A vague line end is
still usefully localized laterally, and one end of a line can be vague
when the other end isn\'t.

Due to shape change and occlusion, it turns out to be crucial to
identify situations where the position of line ends is unreliable. This
need results in a rather complex decision rule. A line end is vague if:

-   The scanner point defining the endpoint position was occluded,

-   The prior longitudinal error estimate is exceeds 15 cm, or

-   The next adjacent point not assigned to this segment is at least 1.2
    meters behind the line. Any intervening no-return points are
    skipped.

Rule 3 has these functions:

-   It is a more precise occluded test in the case of a line with known
    orientation.

-   It also marks ends as vague when the adjacent point in the next
    segment falls near the same line, hence may well be part of the same
    object that happened to be segmented separately due to missing
    returns.

#### Data association

Data association is the process of determining which current
measurements correspond to existing tracks. In the context of the
long-range tracking literature, our approach is basically
nearest-neighbor. We pick one single segment in the measurement data
which most closely resembles the prediction and use that for the new
estimate. However, since each segment has many 2D points, the concept of
"distance" is rather complex.

Our first approach to data association used the combined log-likelihood
of the feature positions as a distance measure, then chose the most
likely segment. However, we found this approach performed poorly, in
that it both created bad tracks by associating separate objects and also
caused good tracks to break up if the object shape changed too much. To
the human eyeball the errors seemed rather silly.

##### Overlap based association

We now use an approach based primarily on spatial overlap between the
track and the measurement data. The concept of overlap-based association
is simple: given rectangles representing the outlines of the track and
the measurement, the two are associated if the outlines overlap.

The actual overlap algorithm is somewhat different, and handles
non-rectangular objects much better. For two objects to overlap, we
require that at least one actual measurement point fall inside the track
outline, and symmetrically, that when the track\'s last measured points
are updated for predicted motion, that at least one of these predicted
points falls inside the segment\'s outline. Since the points are on the
edge of the object, we expand the object outline by 0.8 meters when
checking the point overlap. If there is no linear fit (**complex** shape
class), then we use the bounding box in fixed coordinates as the object
outline.

This algorithm works well even when a moving track passes though a
concave part of a large fixed object (wall, etc.) In this case, none of
the wall\'s points fall inside the track outline, even though the
bounding box for the wall may entirely enclose the track.

##### Track splitting and merging

In addition to its robustness, another advantage of overlap-based
association is that it leads to a straightforward way of detecting when
tracks split or merge. In these cases, overlap does not establish a
one-to-one correspondence between tracks and segments. If a track
splits, then there will be two segments overlapping the track. If two
tracks merge, then there will be two tracks overlapping one segment.

Actual splitting or merging can happen when pedestrians get into or out
of vehicles. In practice, segmentation errors are the most common cause
of splitting and merging. In the presence of clutter, an object may
appear to split from or merge with another nearby object. Missing
returns can also create holes in the middle of objects, causing them to
split in two.

Whatever the cause, it is important to detect when tracks merge because
it is a common reason for track disappearance. See section *Track
Creation and Deletion* where we discuss the "died without offspring"
test.

When two tracks join, the ID of the merged track is that of the older
track.

##### Maximum closeness:

In addition to split/merge situations, our fairly permissive overlap
test can also generate complex overlap relationships when tracks are
simply close together. Since in general the overlap test may create a
many-to-many association between tracks and segments, we need a
procedure for deciding which particular association to make.

This discrimination is done based on *maximum closeness,* where
closeness of a track and segment is defined as the sum of the reciprocal
of the individual feature distances. This differs from the RMS distance
in two important ways:

-   The result is dominated by the best agreeing features, not the
    worst, so it is not confused by the gross shape change which happens
    when tracks split or merge, and

-   The result is not normalized by the number of features in
    correspondence. The more features, the higher the closeness. In a
    split/merge situation, we want to discard the track or segment with
    less information.

Tracks are associated from oldest to newest, which gives older tracks
preference. A track may associate with a segment that is not mutually
closest (there may be another newer track that is closer.)

##### Feature correspondence

Once we have decided which track and segment correspond, we are faced
with the problem of *feature correspondence*: which features in the
track and segment represent the same point? Due to our extremely
restricted feature model, this is much simpler than it might be.

The only case where features may not correspond directly is when
associating lines and corners. This happens when we move around an
object, sometimes being able to see two sides, sometimes only one.
Correct handling of this case is important because often visibility of
one side of an object will be marginal due to shallow gaze angle, and
the segmentation will keep flipping between line and corner.

Often the corner will correspond to one of the endpoints of the line,
but sometimes the line ends match better onto the ends of the sides of
the corner. We associate the line with whichever side of the corner is a
better directional match, as long as this has less mismatch than the
direct ends-to-ends match.

##### Track creation and death

If we fail to associate a measurement segment with any existing track,
then we consider creating a new track. A new track is created if the
segment has at least 3 non-occluded points and is not a line with both
ends vague.

If we fail to associate an existing track with any measurement segment,
then we may delete the track. Tracks are deleted if they have not been
associated for 10 cycles or the total number of previous associations,
whichever is less.

When a track dies, we report this event, whether the track merged with
another object, and also whether the track "died without offspring".
This is determined by examining the split-from chain of all currently
live tracks, seeing if the dead track was a parent. This test is done to
support detection of objects that may still be present but have passed
out of the scanner field of view (pedestrians that have fallen down.) If
some track that split off is still alive, then the split was probably
spurious (due to a segmentation problem), so there is no real
disappearance.

#### Track state and dynamic model

The basic dynamic model is constant acceleration and constant turn rate.
There are nine state variables: XY incremental motion, XY velocity, XY
acceleration, position theta, heading theta and angular velocity. The
first six states estimate translational motion and the last three
estimate heading and turn rate. All of the translational states are
expressed in the fixed world coordinate system. The track incremental
motion is initially zero, and is an estimate of the net XY motion since
track creation.

Using a constrained vehicle dynamic model such as the bicycle model, the
velocity and acceleration are single-dimensional when expressed in the
track\'s moving coordinates. We implement a similar effect by rotating
the velocity and acceleration by the predicted incremental rotation on
each update cycle. However, we maintain a 2D velocity and acceleration,
so our model allows for velocity and acceleration normal to the vehicle
heading (in addition to the apparent acceleration due to turning.) While
this can happen in a skidding vehicle, this is not really relevant in
our application. The interpretation of the 2D acceleration is somewhat
non-obvious, as it is the residual acceleration after the effect of
turning has been removed.

There are actually separate linear Kalman filters for linear estimation
(6 state) and rotational estimation (3 state.) In the future we plan to
investigate a combined 7 state nonlinear filter which would
simultaneously estimate the linear and rotational motion without
unnecessary degrees of freedom.

The dynamic model for compact objects (such as pedestrians) is modified
by forcing acceleration to zero. Pedestrians don\'t spend much time in a
constant acceleration regime, so predicting parabolic trajectories
doesn\'t make sense. We do however predict pedestrian "turn rate" as in
the bicycle model. Though pedestrians don\'t really turn on constant
curvature paths, they don\'t normally turn abruptly either, so this
seems to have some value.

##### Rotational estimation

Additional complexity for rotational estimation comes from the fact that
we can sometimes (see *Shape Classification*) directly measure vehicle
heading (orientation theta), and thus fairly directly measure the turn
rate. For other tracks, the only way to discern the turn rate is to
observe the change in the direction of velocity (heading theta) over
time.

Due to the kinematics of the bicycle model, the instantaneous velocity
of a turning vehicle is not normal to the front or back surface of the
vehicle. Fortunately, we are really mainly interested in estimating the
angular velocity, not the heading, and the time rate of change of the
velocity vector is the same as for the orientation of any part of the
vehicle outline.

However, we run into problems when we are forced to switch between these
two orientation estimates (when the **disoriented** flag changes.) This
can create spurious jumps in position that would be interpreted as
angular velocity. By maintaining separate estimates for these two
different headings we avoid this confusion.

##### Tracking features

Because we have up to three features for each for each track, we have a
data fusion problem. How do we combine the motion estimates from the
separate features into one track motion estimate? Fortunately, the
Kalman filter provides a natural framework for data fusion. Each feature
can be considered an independent measurement of the position.

Because each feature has its own error model, the Kalman filter weights
the contribution of each feature appropriately.

One complication is that the features do not have the same position
because they are different points on the object. To allow motion
assessment of individual features, we need to know the previous position
of each feature. This is done by a modified Kalman filter structure.
Each feature has an independent position, but shares its motion with the
track. The position innovation is determined by subtracting the old
feature position and the new measurement. The Kalman gain is the
computed using the feature\'s position noise and the track state
covariance. The position part of the track state change is applied to
both the feature position and the track incremental motion.

Vague line ends are tracked specially. Although vague line ends are
given a large longitudinal position covariance, this does not cause a
sufficiently profound suppression of spurious longitudinal motion. There
is large rapid motion of vague line ends in common situations such as a
side of an object becoming visible when we round the corner. To
completely suppress this motion, we zero the longitudinal component of
velocity and acceleration state change for vague line ends. We allow the
incremental motion to be affected so that huge jumps in longitudinal
position can still cause the association to fail (due to excess state
change Mahalanobis distance.)

Which features contribute to the motion estimate depends on the shape
classification. Of course, features not present can\'t contribute. Less
obviously, the motion of the bounding box corners only contributes to
the motion estimate when the shape class is **complex**. We track the
position of the bounds regardless, so that we have a valid feature
position and noise estimate available if the shape switches to
**complex**.

##### Track center

We maintain a crude estimate of the track center. This is done via the
center pseudo-feature. During shape classification, we compute the
center as the midpoint of the object. For center finding on linear
objects, we extend vague sides to be at least 2 meters long. This can be
regarded as a prior object model that everything is a 2 meter square.

Because this is a very low quality estimate, and is in any case contains
no additional motion information not present in the other features, the
motion of the center feature is measured using a separate Kalman filter.
This gives some smoothing of the center position, while avoiding any
interaction with the actual motion estimate.

##### Noise adaptation

We make use of measurement noise adaptation to estimate the position
covariance for each feature. The primary benefit of measurement noise
adaptation is in its effect on the data-fusion aspect of the Kalman
filter: noisy features contribute less to the motion estimate. This is
valuable because some features are much noisier than others.

The noise is estimated from the statistics of the measurement residue,
which is the disagreement between the predicted feature position and the
measured one. (With our dynamic model, the residue is the same as the
feature position innovation.)

The covariance of the residue is an estimate of the position measurement
covariance.

In theory, for computing an adaptive measurement noise, we should reduce
the noise residue by the estimate covariance to represent the
contribution to the measurement residue from the estimate uncertainty.
However, this risks creating small or even negative measurement noise
when the estimate covariance is too high. Since we are knowingly
inflating process and measurement noise to deal with time-varying
behavior and modeling error, subtracting out the state covariance would
to cause problems.

If the mean of the residue is not nearly zero, this indicates a
non-zero-mean error. In some applications it would be appropriate to
subtract out this mean from the measurement; in our case there is no
independent measurement of the feature position, so a significant mean
error means that we are not tracking properly. If the residue mean
exceeds 10 cm, we reset the feature position from the current
measurement and clear the residue mean to zero.

The need for this resetting comes from a sort of instability that the
noise-adaptive tracker exhibits in the presence of time-varying
non-zero-mean disturbances. In short, if a feature position drifts
around, and no compensating track velocity is inferred (perhaps due to
data fusion correctly rejecting spurious motion), then the measurement
covariance for that feature becomes inflated, and this further degrades
the tracking performance for that feature. Our response in this
situation is to allow the feature to be basically ignored by fusion, but
to keep the feature position approximately correct by resetting the
position when the residue mean becomes too large.

Residue update has two phases: during track startup, we compute
recursive mean and covariance with equal sample weighting. After 0.3
seconds, we switch to a first order response (exponential decay) with
0.3 second time constant. Initially, the sample is small, so the
estimate is unreliable. For the first 15 cycles, we use only the prior
noise estimate. After 15 cycles, we use the sum of the residue
covariance and (*prior\_ noise* \* 0.57). The prior noise then serves as
a lower bound on the adaptive noise. This lower bound is particularly
valuable in cases where the prior noise is high (like vague line ends.)

##### Improving the Kalman filter for non-Gaussian errors

The Kalman filter is optimal when the measurement error and process
disturbance are Gaussian. One of the characteristics of the Gaussian is
its light tails: it is very unlikely that a result will be very far from
the mean. Unfortunately, when feature tracking breaks down, it can
produce outlier measurements which are effectively impossible in the
Gaussian model.

Since a Kalman filter doesn\'t suppress these glitches it is common to
extend the Kalman filter by limiting the magnitude of outlier
measurements or discarding them. We make use of this approach in several
places:

-   If the change in track state due to a measurement is too incredible,
    then we discard the measurement. This is done when the Mahalanobis
    distance of the state change with respect to the estimate covariance
    exceeds 6. Before discarding the measurement, we see if we can get a
    reasonable association by resetting a subset of the features. We
    reset any features that contribute a state change with Mahalanobis
    \> 4 by setting their position to the measurement, zeroing their
    contribution to the innovation.

-   The time rate of change (*d/dt)* of velocity, acceleration and
    angular velocity are limited to physically plausible values: 9.8
    meters/sec^2^, 5 meters/sec^3^, 60 degrees/sec^2^. Note that these
    limits are applied to the incremental state change, not just to the
    output estimate. For example, the acceleration limit is applied not
    only to the acceleration estimate, but (more importantly) also to
    the change in velocity estimate on any given update cycle. This
    prevents impossible jumps in position from causing big velocity
    jumps.

-   The measurement of heading from feature orientation (orientation
    theta) is prone to jump when there is a track merge/split or the
    shape classification changes. If the innovation exceeds 7 degrees in
    this situation, then we reset the position to the new measurement.

##### Track startup

When a track is newly created, the dynamics are unknown, so we assume
that the velocity and acceleration are zero. If a track splits off of an
existing track, we initialize the velocity and acceleration to the one
for the existing track, but still leave the estimate covariance at the
default (large) value.

Commonly tracks will come into range already rapidly moving, so this
prior velocity can be significantly in error. It takes some time for the
measurement to settle to the correct value, and *dv/dt* limiting
prolongs this. During *dv/dt* limiting we hold acceleration fixed, as
otherwise the acceleration slews wildly because the Kalman filter
feedback loop is open. We also modify the Kalman filter update so that
the velocity covariance is effectively fixed during *dv/dt* limiting*,*
preventing spurious covariance convergence.

The physical *dv/dt* limit does not apply during track startup because
this velocity change is not a physical acceleration. As a heuristic, we
increase the *dv/dt* limit when the velocity covariance is high. We
allow the velocity to change by 12 sigmas per second if this is higher
than the physical limit.

##### Information increment test

When attempting data association of a segment and track, we find the
*information increment*, which is a measure of how much the track was
influenced by this measurement. If the information increment is low,
then the track is not responding to the measurement because the
measurement is considered too noisy relative to how certain we think we
are of the current state. In this case, the tracker is not actually
tracking anything, just speculating based on past data.

A common problem situation is that a track may change to a line with
both ends vague. In this case, the track is localized in one direction
only, and the longitudinal velocity is unmeasured, which can lead to
unacceptable spurious velocities. Low information increment can also
happen if we reset all of the features in a track or when a track is
very noisy (via noise adaptation.)

To keep the track from coasting indefinitely, we fail the association
when the information increment is below 0.04 (response time constant of
25 cycles.) Tracks that are not associated for 10 cycles are deleted. If
there is a one-to-one overlap relationship, then we pretend not to
associate for purposes of the deletion test, but actually do associate.
This helps to keep good tracks tracking properly in situations where the
association is clear but the information is momentarily bad.

Though we call this \"information\" increment, the computation is really
based on the covariance. This mimics the computation of the Kalman gain,
which is what actually determines the response speed of the filter. If
*w*~+~ is an eigenvalue of the covariance after the measurement, and
*w*~-~ is the eigenvalue before update, then the info increment is the
mean of *w*~-~ /*w*~+~ - 1 for the two eigenvalues. The eigenvalues are
sorted so that we compare eigenvalues of similar magnitude.

The assumption is that the eigenvectors are little changed by any one
measurement cycle, so the change in the sorted eigenvalues represents
the change in uncertainty on each axis in the rotated (uncorrelated)
coordinates. This insures that the track is well localized in two
dimensions.

#### Track evaluation

The tracker outputs two flags for each track to aid in the
interpretation of the result:

  ------------ ------------------------------------------------------------
  **Valid**    true when the motion estimate is believed to be accurate

  **moving**   true if there is compelling evidence that the track is
               moving
  ------------ ------------------------------------------------------------

Spurious velocities on fixed objects can easily cause false collision
warnings. Since true imminent collisions are very rare, and fixed
objects are very common, we must drive down the reporting of false
velocities on fixed objects to a very low level in order to get an
acceptable rate of false alarms.

To achieve this we have developed an additional evaluation procedure
that operates independently of the Kalman filter. We collect the last 35
segments (raw measurements) associated with each track, then check how
well the track path matches up with the measurements if we project it
backward in time.

If our dynamic model adequately describes the motion and the estimated
parameters are close, then the paths should agree well. If the match is
poor, then there is either unmodeled disturbance (rapid change in
acceleration or turn rate), or there is a tracking failure due to
problems in feature extraction, etc.

We accumulate two different streams of information about the difference
between the back-predicted location and measured location of
corresponding features:

1.  The mean-square Mahalanobis distance according to prior position
    error. This is used to evaluate moving and valid.

2.  The Euclidean distance normalized by the elapsed time from the
    measurement to now. The mean is a velocity correction and the
    covariance is an estimate of the velocity covariance. The velocity
    correction is added to the output velocity. If the covariance is
    bigger than Kalman filter covariance, then it is output instead.

We also find the sum of the position information for all features in the
history that contributed to the distance estimate. This is used to
determine whether the history has adequate quality to localize the track
in two dimensions. The smaller eigenvalue of the information matrix must
be greater than 35 meters^-1^.

This information eigenvalue is also used to normalize the Mahalanobis
distance back into a nominal length, which is then compared to a
threshold for the moving/valid test. For a track to be marked valid, the
distance must be less than 5 cm, and after that must stay below 15cm to
remain valid. Though the units are meters, the physical interpretation
is obscure. The empirically chosen values seem reasonable if regarded as
an RMS fit error in meters. The advantage of this distance measure over
a simple RMS distance is that it takes into consideration the asymmetric
prior error distributions generated by the position measurement model.

We also compare the past measurements to the null hypothesis that we are
not moving at all. This is done using the same comparison procedure, but
with no projected motion. We then compare the matching error of the two
hypotheses. For the track to be moving and valid, the matching error of
the moving hypothesis must be 4 times less than that of the fixed
hypothesis. This rejects situations where there is equal evidence for
moving and non-moving because one feature moves and another doesn\'t.

In order to minimize the effect of noisy variation, the results of the
history analysis are filtered using an order 21 median filter before
being tested against the above limits.

Because the history-based validation is fairly computationally expensive
(about 250 microseconds per track on 1.2 GHz PC), we have used several
optimizations:

1.  Only do history test on apparently moving tracks. A track is
    apparently moving if it has a feature that has been tracked for at
    least 15 cycles, the speed is greater than 0.75 meters/sec,
    Mahalanobis distance of the velocity from zero exceeds 6. There is
    hysteresis in this test so that tracks tend to stay moving once they
    are initially moving. Also, if a track is a line with two vague ends
    that was not previously moving, then only the lateral component of
    the velocity is considered.

2.  Only use the oldest 1/3 of the history data, as this contains most
    of the information about velocity error.

3.  Limit the number of tracks validated on any tracker cycle to 4.
    There are seldom this many moving tracks, so this limit is rarely
    exceeded. The purpose is to bound the runtime of a single tracker
    iteration.

### Evaluation

Linear feature tracking has been tested primarily as part of the larger
collision avoidance system, with the main tuning criterion being
minimizing the number of false alarms due to spurious motion. However,
there are a number of ways that we can characterize the performance of
the tracker. First, we can visually examine the output to get sense of
the noise and response time of the output. Figure 39 shows the output
for a car that comes into view turning a corner, then drives straight.

![](media/image38.png){width="6.3in" height="4.729861111111111in"}

[]{#__RefHeading___Toc96687070 .anchor}Figure 39: Tracker dynamics

The track is flagged as moving and valid at 4.3 seconds. After that
time, the noise fluctuations in the velocity seem to be less than 0.5
meters/sec peak-to-peak. The acceleration and turn rate (v_theta) are
fairly smooth, but clearly have a lot of delay. The vehicle is probably
near peak turn rate at the time that the track is detected as moving,
but the peak in output turn rate happens almost a second later.

The acceleration estimate is also responding to the low frequency change
in velocity, but slowly. At around 5.7 seconds it appears that the
acceleration is near zero. The output estimate is dropping fairly
rapidly, but doesn\'t make it to zero during the apparent low
acceleration period.

![](media/image39.png){width="6.0in" height="3.24375in"}

[]{#__RefHeading___Toc96687071 .anchor}Figure 40: Track history matching

The history-based track evaluation provides another way to investigate
tracker performance. By using the current motion estimate to predict the
past position, then comparing with the actual measurements, we can get a
sense of how well the tracker can predict motion over short periods of
time (0.3 seconds.)

Though the idea of comparing the prediction to actual measurements is a
good way to verify the tracker performance, this particular data is not
very good evidence because of the short time scale (where the effect of
acceleration and turn rate error is slight) and because the velocity
correction is calculated from this very data so that it minimizes the
error. It would be much more convincing to show that we can predict the
future.

We can see that there is very good agreement of position, showing the
velocity estimate is fairly accurate. Also, we can see that the turning
approximately matches up as well. Slight acceleration error is visible
in the middle of the sequence (the first order velocity correction
forces both ends to line up.)

### Summary

We have found that the linear feature tracker combined with
history-based track validation is able to generate reasonably accurate
velocity estimates for cars and pedestrians in a cluttered urban
environment, while giving a low rate of spurious motion indications that
can cause false alarms. These false alarms are discussed in detail in
the Sections 6.3 titled SCWS Warning Algorithm and 6.4 False Alarms. The
estimation of acceleration and turn rate appears to improve prediction
of future positions, but the prediction would be significantly better if
the response time could be improved.

We also have enough experience in working on this particular problem to
be able to state with some confidence that any significantly simpler
approach will not be able to achieve comparably low levels of false
motion estimates. When the scanner is in motion, the apparent shape of
objects changes, and fairly sophisticated measures are required to
determine that this is not actual motion.

The computational efficiency is significantly better than some of our
previous attempts at solving this problem. No large data structures such
as maps are used. The average time to process one scan on a 1.2 GHz PC
is 4 milliseconds. The code size is about 7000 lines of C++, and is
written at a fairly high level of abstraction, so considerable further
performance gains could likely be achieved if necessary.

## FCWS Warning Algorithm

From 2001 to August 2003, three generations of warning algorithms were
developed, with each later version being an improvement on the previous
version. The current version is the third generation algorithm, which
has undergone further improvements based on data analysis and driver
feedback since September 2003. The features and improvements of the
third generation of algorithm are summarized in the following table and
the five points below. The improvements to the third generation
algorithm and its modification in order to process radar data are
introduced later in the chapter.

+--------+-------+---------+---------+----------+---------+---------+
|        | O     | Bus     | Driver  | Coriolis | Fini    | Threat  |
|        | bject | model   |         | effect   | te-size | measure |
|        | model |         |         |          | -object |         |
|        |       |         |         |          | effect  |         |
+--------+-------+---------+---------+----------+---------+---------+
| 1^st^  | F     | No      | No      | No       | No      | TTC     |
| alg    | ree-m | consid  | consid  | consi    | consid  |         |
| orithm | oving | eration | eration | deration | eration | (Time   |
| (2001) |       |         |         |          |         | -to-Col |
|        |       |         |         |          |         | lision) |
+--------+-------+---------+---------+----------+---------+---------+
| 2^nd^  | Non   | Non-ho  | Em      | De       | No      | S       |
| alg    | -holo | lonomic | pirical | coupling | consid  | peed-de |
| orithm | nomic |         | TTC     | bus      | eration | pendent |
| (2002) |       |         | th      | motion   |         | TTC     |
|        |       |         | reshold | from     |         |         |
|        |       |         |         | sensor   |         |         |
|        |       |         |         | data     |         |         |
+--------+-------+---------+---------+----------+---------+---------+
| 3^rd^  | F     | Non-ho  | Em      | De       | Delayed | R       |
| alg    | ree-m | lonomic | pirical | coupling | -filter | equired |
| orithm | oving |         | r       | bus      | which   | decel   |
| (2003) | (st   |         | equired | motion   | can     | eration |
|        | opped |         | decel   | from     | well    |         |
|        | or    |         | eration | sensor   | e       |         |
|        | cre   |         | th      | data     | stimate |         |
|        | eping |         | reshold |          | accel   |         |
|        | targe |         |         |          | eration |         |
|        | ts) + |         |         |          | from    |         |
|        | non   |         |         |          | range   |         |
|        | -holo |         |         |          | data    |         |
|        | nomic |         |         |          |         |         |
|        | (m    |         |         |          |         |         |
|        | oving |         |         |          |         |         |
|        | tar   |         |         |          |         |         |
|        | gets) |         |         |          |         |         |
+--------+-------+---------+---------+----------+---------+---------+

[]{#__RefHeading___Toc96687011 .anchor}Table 18. Features and
improvements of three generations of FCWS algorithms

The main features of the third generation algorithm are described in the
following five points:

1.  Model: Moving targets are modeled with non-holonomic constraints, so
    that heading and yaw-rate can be more precisely estimated. For
    stopped and creeping targets a free-moving model is used because
    moving direction can not be detected from short-time displacement. A
    free-moving model is a 2D kinematic model based on Newton's laws of
    motion. A non-holonomic constraint means that lateral slide is
    prohibited.

2.  Driver's role: It is taken into account that the bus driver is
    working in parallel with FCWS, and responsible for fusing warning
    information with his/her own perception and decision making.
    Empirical data were analyzed to derive thresholds from the driver's
    behavioral data so that the FCWS can better match the driver's
    normal operation.

3.  Coriolis effect: The algorithm decouples the bus's motion from
    sensor observations so that the Coriolis effect can be eliminated.
    The Coriolis effect introduces an imaginary component of motion due
    to the rotating of the sensor's coordinate frame of reference.

4.  Finite-size-object effect: The finite-size effect introduces ranging
    error due to the size of vehicle bodies. The "delayed filter" can
    better estimate target velocity and acceleration by delaying the
    update of the model to improve the displacement-to-error
    (signal-to-noise) ratio that is usually impaired by finite-size
    effect.

5.  Threat measure: Required deceleration is used as threat measure.
    Required deceleration is the minimum deceleration that should be
    applied to the bus to avoid an imminent collision with a preceding
    object. TTC (Time-to-Collision), speed-dependent TTC (which is a
    look-up-table of empirical TTC derived from real data indexed by
    object speed and bus speed) were also tried as threat measures, but
    required deceleration is more natural in terms of matching with the
    driver's operation. It is the delayed filter which makes it possible
    to utilize the required deceleration as the threat measure.

### FCWS Algorithm structure

The structure of the warning algorithm is shown below. The main data
structure-track file, the tracking and warning detection algorithm are
described in detail at the end of this section.

![](media/image40.wmf)

[]{#__RefHeading___Toc96687072 .anchor}Figure 41 FCWS Algorithm
structure

### FCWS Data structure

#### Track file structure

A track file is a list of tracks being processed. Each track is a
correlation and refinement of the time sequence of observations of an
object (a target). An ID indexes each track using the name (usually an
integer) for an object under tracking. An object under tracking is
described by the object state in the track file. Object state is a
combination of kinematic states and track properties of an object.

![](media/image41.wmf)

[]{#__RefHeading___Toc96687073 .anchor}Figure 42 FCWS Track file
structure

The designed track file consists of two major memory buffers: track head
buffer and data buffer. Both are declared as linear arrays but are
organized in linked-lists. Every track head cell belongs to one of the
following five levels:

> LEVEL_DISUSE: currently not in use;
>
> LEVEL_INITIAL: initial tracks;
>
> LEVEL_TENTATIVE: tentative tracks;
>
> LEVEL_PREMTURE: premature tracks;
>
> LEVEL_FIRM: firm tracks.

Each level of track heads is organized as a double-linked list. Each
track head is then linked to a double-linked list of historical data of
the track. The whole data buffer is organized as a circular queue (or
equivalently a First-In-First-Out (FIFO)). If the head of the queue
reaches the tail, the oldest cells are released from double-links of
tracks to provide memory for new data. New data collected in the latest
snapshot are saved in a double-linked list. The structure of track file
is illustrated in the above picture.

##### Track head data structure

One key element of a track file is the data structure for storing track
information. This is defined in the TRACK_HEAD structure;

![](media/image42.wmf)

> where ID is the index of a track, Pred is the predicted state.

##### Object state data structure

Another key element of a track file is the data structure for storing
data of an object in one snapshot. This is defined in the OBJECT_STATES
structure:

![](media/image43.wmf)

> where t\[\] are time members (e.g. time of availability of data and
> estimated time for filtering output considering the delays); pntr\[\]
> are pointer members for data structure manipulation (e.g. building up
> linked lists); iobsv\[\] and dobsv\[\] are observation members for raw
> data storage; par\[\] are estimated motion states members for refined
> parameters storage; the identifiers in brackets are constants.

This structure is easily extendable. The usage of members is defined in
the program and is subject to change. Currently the constants are:

![](media/image44.wmf)

##### Linked lists of tracks

Tracks are categorized into four levels: initial, tentative, premature
and firm (see section Error: Reference source not found and Error:
Reference source not found for details of these levels). Each level of
tracks is organized in a double-linked list. The sub-routine
"ChangeTrackLevel()" can move a track from one level to another. Upon
initialization, all track heads are put in "disuse" category.
Sub-routine "FreeTrack()" can move a track from any level to disuse.

##### Linked lists of track histories

The historical data of tracks are built in double-linked lists. Each
node is an OBJECT_STATES structure. The following figure shows a typical
linked list.

![](media/image45.wmf)

[]{#__RefHeading___Toc96687074 .anchor}Figure 43 Linked list of tracks
and historical data

> where ID of the track is saved in DATA set of each node.

[^1]: Mertz, Kozar, Miller, Thorpe. "Eye-safe Laser Line Striper for
    Outside Use." [IV 2002, Proceedings of the IEEE Intelligent Vehicle
    Symposium (IV2002).]{.underline} June 2002.
    <http://www.ri.cmu.edu/pubs/pub_3890.html>

[^2]: Aufrère, Mertz, and Thorpe. "Multiple Sensor Fusion for Detecting
    Location of Curbs, Walls, and Barriers," [Proceedings of the IEEE
    Intelligent Vehicles Symposium (IV2003).]{.underline} June 2003.

[^3]: op. cit.

[^4]: Thorpe, Charles E. [Vision and Navigation: The Carnegie Mellon
    Navlab.]{.underline} Kluwer Academic Publishers, 1990.

[^5]: Gowdy, Jay. [Emergent Architectures: A Case Study for Outdoor
    Mobile Robots]{.underline}. Thesis for PhD at the Robotics
    Institute, Carnegie Mellon, CMU-RI-TR-00-27. November 2000.

[^6]: Gazi, Moore, Passino, Shackleford, Proctor and Albus. [The RCS
    Handbook:  Tools for Real-Time Control Systems Software
    Development.]{.underline} New York: John Wiley & Sons, 2001.

[^7]: The Object Management Group. [The Common Object Request Broker:
    Architecture and Specification.]{.underline} Massachusetts: 1996.

[^8]: Gowdy, Jay. [{IPT}: An Object Oriented Toolkit for Interprocess
    Communication.]{.underline} Technical Report for the Robotics
    Institute, Carnegie Mellon University, CMU-RI-TR-96-07. March 1996.

[^9]: Hayes-Roth, B. [A blackboard architecture for
    control.]{.underline} Artificial Intelligence, Volume 26. 1985:
    251-321.

[^10]: Mills, David L. [Internet time synchronization: The Network Time
    Protocol.]{.underline} Published as: [The Network Working Group
    Request for Comments: 1129.]{.underline} October 1989: 1-29.