distance

Distance between two states

collapse all in page

Syntax

dist = distance(space,states1,states2)

Description

example

dist = distance(space,states1,states2) returns the distance between states1 and states2 in the specified state space space.

Examples

collapse all

Open Live Script

Create an SE(3) state space.

space = stateSpaceSE3
space = 
  stateSpaceSE3 with properties:

                 Name: 'SE3'
          StateBounds: [7×2 double]
    NumStateVariables: 7
            WeightXYZ: 1
     WeightQuaternion: 0.1000

Calculate distance between two states.

dist = distance(space,[2 10 3 0.2 0 0 0.8],[0 -2.5 4 0.7 0.3 0 0])
dist = 12.7269

Calculate Euclidean distance between two states.

space.WeightQuaternion = 0;
distEuc = distance(space,[2 10 3 0.2 0 0 0.8; 4 5 2 1 2 4 2],[62 5 33 0.2 0 0 0.8; 9 9 3 3 1 3.1 7])
distEuc = 2×1

   67.2681
    6.4807

Input Arguments

collapse all

State space object, specified as a stateSpaceSE2, stateSpaceSE3, stateSpaceDubins, or stateSpaceReedsShepp object.

Initial states for distance calculation, specified as an n-by-3 or n-by-7 matrix of real values. n is the number of specified states.

For the 2-D state space objects stateSpaceSE2, stateSpaceDubins, and stateSpaceReedsShepp, each row is of form [x y theta], which defines the xy-position and orientation angle theta of a state in the state space.

For the 3-D state space object stateSpaceSE3, each row is of form [x y z qw qx qy qz], which defines the xyz-position and quaternion orientation [qw qx qy qz] of a state in the state space.

The function supports following combinations for distance calculation:

  • n-to-n — n number of states in states1 and n number of states in states2.

    For example, distance(space,rand(10,7),rand(10,7))

  • 1-to-n — 1 state in states1 and n number of states in states2.

    For example, distance(space,rand(1,7),rand(10,7))

  • n-to-1 — n number of states in states1 and 1 state in states2.

    For example, distance(space,rand(10,7),rand(1,7))

Data Types: single | double

Final states for distance calculation, specified as an n-by-3 or n-by-7 matrix of real values. n is the number of specified states.

For the 2-D state space objects stateSpaceSE2, stateSpaceDubins, and stateSpaceReedsShepp, each row is of form [x y theta], which defines the xy-position and orientation angle theta of a state in the state space.

For the 3-D state space object stateSpaceSE3, each row is of form [x y z qw qx qy qz], which defines the xyz-position and quaternion orientation [qw qx qy qz] of a state in the state space.

The function supports following combinations for distance calculation:

  • n-to-n — n number of states in states1 and n number of states in states2.

    For example, distance(space,rand(10,7),rand(10,7))

  • 1-to-n — 1 state in states1 and n number of states in states2.

    For example, distance(space,rand(1,7),rand(10,7))

  • n-to-1 — n number of states in states1 and 1 state in states2.

    For example, distance(space,rand(10,7),rand(1,7))

Data Types: single | double

Output Arguments

collapse all

Distance between states, returned as an n-element column vector. n is the number of specified states.

The function supports following combinations for distance calculation:

  • n-to-n — n number of states in states1 and n number of states in states2.

  • 1-to-n — 1 state in states1 and n number of states in states2.

  • n-to-1 — n number of states in states1 and 1 state in states2.

Data Types: single | double

See Also

| | |

Introduced in R2019b

Navigation Toolbox Documentation

Support