Usage

How to start using this library!

Before we get started, just a quick note: This library only contains the math behind localizing with distance sensors! However if you have any questions about the hardware side of distance sensor localization, we would love to help, contact us!

Setup

This library is created around standard measurements of position and heading. By this we mean that all position(X & Y) are positioned at Quadrant 1. And all angles are based off of the Unit Circle angles. If you're using RoadRunner, don't worry we have included extension functions to reformat!

Lets get started, first you want to create an instance of the DistanceLocalization class, as follows:

Java

ThreeSensorLocalization tsl = new ThreeSensorLocalization(
    new Pose(x, y, rad), new Pose(x, y, rad), new Pose(x, y, rad),
    sensorDistanceSafety
);

Kotlin

val tsl = DistanceLocalization(
    Pose(x, y, rad), Pose(x, y, rad), Pose(x, y, rad), 
    sensorDistanceSafety
)

You'll notice there the DistanceLocalization class takes a few parameters, lets walk through these:

1. leftSensorPosition: This is the position of the left distance sensor relative to the robot!

2. frontSensorPosition: This is the position of the front distance sensor relative to the robot!

3. rightSensorPosition: This is the position of the right distance sensor relative to the robot!

4. sensorDistanceSafety: This is a safety condition put in place, similar to a 'max bound', this should be set to some value around 30" - 50", but will vary based on your sensor reliability.

For the three sensor positions, create it in such a way that:

1. If the sensor is on the relative left of the robot, make the X negative. If the sensor is on the relative right of the robot, make the X positive.

2. If the sensor is on the relative front of the robot, make the Y positive. If the sensor is on the relative back of the robot, make the Y negative.

3. Follow the unit circle for the direction in radians which the sensor faces!

Example of positioning system, with the AlphaGo 2021-22 robot:

In the example, the left sensor is positioned at (-8.25, 7.5) facing 180° (PI radians). The front sensor is positioned at (0.0, 8.0) facing 90° (PI/2 radians). And finally the right sensor is positioned at (8.25, 7.5) facing 0° (0 radians).

To update the class, call the update() function and input the distances from the three sensors, and the robot's heading(from a reliable source such as IMU) in radians, as follows:

tsl.update(leftDistance, frontDistance, rightDistance, robotTheta)

To access the poseEstimate, call dl.poseEstimate or assign a variable to the update method!

If there was a problem calculating either the X or the Y, it will be returned as Double.NaN. This is most likely because the sensors calculating for that corresponding position have been marked out of range, or if both sensors are detecting the same wall at certain angles! SO MAKE SURE TO ACCOUNT FOR THIS!!! (Either do a Double.NaN check or move your robot at a different angle / position!) An example of this is if the robot is near the top right corner, lets say at (120, 120) or in roadrunner (48, -48), and the front of robot is facing 90° left. If the distance reported by the front and left sensors exceed your sensorDistanceSafety, the X position will report Double.NaN. Here is a similar picture of this situation for clarification: