A localization system for a mobile robot with Lag-based feedback loop. More...

Detailed Description

A localization system for a mobile robot with Lag-based feedback loop.

Introduction

Back in the 2000s all [citation needed] robots were differential wheeled robots [1] and localization with sensor fusion was a popular problem. This example shows a simple solution to that problem using DetectorGraph. It uses an Extended Kalman Filter [2] to continuously predict and correct the robot's Pose (and associated uncertainty) from two information sources: the input to the wheels and a GPS-like system.

FuturePublisher vs. Lag<T>

This localization algorithm depends on a feedback-loop in that the output of one graph evaluation (i.e. LocalizationBelief) is used as an input for the next one (i.e. Lagged<LocalizationBelief>):

 class KalmanPoseCorrector : public Detector,
     public SubscriberInterface< Lagged<LocalizationBelief> >,
     public SubscriberInterface<GPSPosition>,
     public Publisher<LocalizationBelief>
 {
 public:
     KalmanPoseCorrector(Graph* graph) : Detector(graph), mCurrentBelief()
     {
         Subscribe< Lagged<LocalizationBelief> >(this);
         Subscribe<GPSPosition>(this);
         SetupPublishing<LocalizationBelief>(this);
     }

Note that in this graph the TopicState where the feedback loop closes is a legitimate output in itself and it's very likely that new Detectors in the graph would subscribe to it. In cases like this the use of DetectorGraph::Lag is preferred as it is more extensible and it preserves the normal TopicState guarantee for the TopicState in question as well as it makes the Lagged version of the TopicState clearly documented and also available. Lag allows even for detectors that subscribes to both the immediate and the Lagged version of a TopicState - this could be useful for things like differentiation etc.

Configuration TopicStates

It also shows how TopicStates can be used for static/configuration data (e.g. RobotConfig). This allows for easy dependency tracking, visualization and testing at pretty much no runtime cost.

Localization Algorithm

Regarding the Localization algorithm itself, it's mostly taken from [3] except for the correction model where this example uses a different input (i.e. GPSPosition) with much simpler transfer function to the Pose vector.

Architecture

The graph below shows the relationships between the topics (rectangles) and detectors (ellipses). Note that this graph can be automatically generated for any instance of DetectorGraph::Graph using DetectorGraph::GraphAnalyzer.

RobotBrain

Other Notes

This example also uses the Eigen library [4] for matrix/vector arithmetic and linear-algebraic operations.

Note that this entire algorithm is contained in a single file for the sake of unity as an example. In real-world scenarios the suggested pattern is to split the code into:

   detectorgraph/
        include/
            robotbrain.hpp (RobotBrain header)
        src/
            robotbrain.hpp (RobotBrain implementation)
        detectors/
            include/
                ExtendedKalmanPosePredictor.hpp
                KalmanPoseCorrector.hpp
            src/
                ExtendedKalmanPosePredictor.cpp
                KalmanPoseCorrector.cpp
        topicstates/
            include/
                RobotConfig.hpp
                InitialPose.hpp
                WheelSpeeds.hpp
                LocalizationBelief.hpp
                GPSPosition.hpp

References

[1] Differential Wheeled Robots - https://en.wikipedia.org/wiki/Differential_wheeled_robot
[2] Extended Kalman Filter - https://en.wikipedia.org/wiki/Extended_Kalman_filter
[3] EKF applied to Mobile Robot’s Localization (Section 2.4.4, page 17) - http://cpscotti.com/pdf/pfc_scotti.pdf
[4] Eigen - http://eigen.tuxfamily.org/dox/index.html

Definition in file robotlocalization.cpp.