Skip to content

Use new EKF and motion model

Igor Bogoslavskyi requested to merge 865-refactor-filter-architecture into master

Description

Why

Let's start with "why". The code that was in master before !918 (merged), while doing what it needs to (kinda) has a couple of downsides:

  • it is written in general way and it is hard to understand what it does without the knowledge of the details
  • some design decisions made are suboptimal and are hard to fix. For example, the motion model both owns a state and is able to update one that is provided to it. This is done for efficiency (I believe?) but the code was never measured.
  • if we need a new motion model we would need to completely new implementation for it. And we will need new motion models. The ones I can think of are: Ackermann drive, Differential drive, Constant acceleration in 3D, Constant velocity in 3D. All of these are hard to add with the current code.
  • The Kalman filter currently used in our codebase has a stable but hard to read implementation using the Square Root Covariance filter. Over the process of developing state estimator I've found a bunch of bugs there, each of them being hard to find. Which also speaks to the quality of the tests.

The aim of the rewrite in !918 (merged) was to start with general but readable code first and add possible implementations that would allow for more complex behaviors. The idea started as a thought that it would be cool to have something as readable as:

using State = FloatState<
  X, X_VELOCITY, X_ACCELERATION, Y, Y_VELOCITY, Y_ACCELERATION, YAW, YAW_VELOCITY>;

using MeasurementState = FloatState<X, Y, YAW>;

State state{};
state.at<X_VELOCITY>() = 1.0F;
state.at<Y_VELOCITY>() = 1.0F;

KalmanFilter<LinearMotionModel<State>, WienerNoiseModel<State>> filter;
filter.predict(dt);
filter.correct(LinearMeasurement<MeasurementState>{{1, 1, 1}, {0.1, 0.1, 0.1}});
cout << filter.state() << endl;

The main benefits here are that all the variable names are concrete types, (see common_variables.hpp in this MR), which enables us to write clear and concise code that is able to perform some actions at compile time.

Another reason was that the noise models used for the kalman filter had mostly an ad-hoc implementation. This MR also tackles that and the noise models are now generated with their respective classes.

What is implemented here

This MR removes the old implementations for the Kalman filter and motion models and uses new ones in all the places where the old ones were used.

Notes for reviewer

This MR has a history. It started as the one huge MR for everything but eventually all the changes got outsourced into the other MRs. Now it just uses the code from those MRs and uses that code instead of the old one.

Pre-review checklist for the author before submitting for review

Every developer is encouraged to be familiar with our contributor guidelines.

  1. MR formalities
    1. "WIP" or "Draft" removed from the MR title
    2. MR title and description help a friendly human understand the problem solved
    3. MR has a link to the original issue in the description, if it exists
    4. If the source branch is on a fork, MR is configured to allow commits from developers with access to push to the target branch
    5. Sensible notes for the reviewer added to the section above to facilitate review
    6. Target branch set correctly. Default: master
    7. MR assigned to a capable reviewer. Default: @JWhitleyWork
    8. Splitting the MR into smaller, easier-to-review merge requests was considered
  2. Code and tests
    1. Code is properly formatted
    2. Tests affected by new code pass locally
    3. Reasonable coverage with unit tests of 90+%; else create a follow-up ticket
    4. Review any // TODO item added in the MR that can be addressed without the reviewer's help
  3. Documentation
    1. Any new and modified code has accurate doxygen documentation
    2. Any diagrams are committed

Checklist for the reviewer

Only the reviewer is allowed to make changes in this section!

Items not applicable to this MR are crossed out by the reviewer.

  • For new nodes, the checklist is expanded and reviewed
  • For a new package, the checklist is expanded and reviewed
  • Reviewer crossed out non-applicable items

Checklist

If the MR provides an improvement, don't hesitate to add a 👍 emoji for a neat line of code or a "Thanks for implementing this" comment. This will reward the MR author and prevent the review from being only about what still needs to be improved.

Mark all the items that are done.

Checklist for development
  1. Basic checks
    1. The MR title describes what is being done on the ticket
    2. All functional code written in C++14, tooling code may be written in Python 3.5+ or Bash
    3. The first commit has a proper commit message to be used as a basis for the squashed commit created at the very end
  2. Code correctness
    1. The problem/feature is solved (reproducibly)
    2. The solution is performant enough for the use case in mind
    3. Any disabled lints inside the code or at the package level are justified
  3. Open work
    1. Any added source-code comment about future work refers to a follow-up GitLab issue explicitly; e.g., // TODO #551 refactor code below
  4. Documentation
    1. New classes, methods, functions in headers are documented with doxygen-style comments
    2. If implementation (of a function...) is modified, the doxygen documentation is updated accordingly
    3. The design article is updated with the implementation
    4. Drawings are created when needed and committed to git
    5. Modified files have a license that is compatible with AutowareAuto
  5. Testing
    1. Code coverage with unit tests does not decrease. Aim for coverage with unit tests of 90+%; else create a follow-up ticket
    2. Unit tests make sense and cover the business logic and error cases
    3. Integration tests are sensible and not flaky
Checklist for new ROS2 nodes
  1. Every node is registered as a component
  2. The naming conventions are followed
  3. At least the basic launch integration test is included
Checklist for new package
  1. Structure
    1. The package name and organization into files is sensible
    2. Core functionality is separated from the ROS2-specific part where reasonable
    3. There is a design document that explains the package at a high level
    4. All dependencies are explicitly included in package.xml with the proper <*depend> declaration

When starting from scratch, new packages should be created with the autoware_auto_create_pkg macro and they will automatically satisfy the following criteria.

  1. If an existing package is added to AutowareAuto, it should match the output of autoware_auto_create_pkg regarding
    1. same set of linters
    2. visibility control
    3. finding build dependencies in cmake with ament_auto_find_build_dependencies()
    4. installing with ament_auto_package()

Post-review checklist for the author

After receiving approval:

  1. Rendered documentation looks as expected
  2. All checkboxes in the MR checklist are checked or crossed out. Syntax example: 1. [ ] ~~this item crossed out~~
  3. Developers were informed about breaking changes on slack in the committers-autoware-auto channel
  4. Assign MR to maintainer with sufficient rights to merge. Default: @JWhitleyWork
Edited by Yunus Emre Çalışkan

Merge request reports