distance

Distance between two states

Syntax

dist = distance(space,states1,states2)

Description

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

Examples

Calculate Distance Between Two States in SE3

Create an SE(3) state space.

space = stateSpaceSE3

space = 
  stateSpaceSE3 with properties:

                 Name: 'SE3'
          StateBounds: [7x2 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

space — State space object

stateSpaceSE2 object | stateSpaceSE3 object | stateSpaceDubins object | stateSpaceReedsShepp object

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

states1 — Initial states for distance calculation

n-by-3 matrix of real values | n-by-7 matrix of real values

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

states2 — Final states for distance calculation

n-by-3 matrix of real values | n-by-7 matrix of real values

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

dist — Distance between states

n-element column vector

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

Version History

Introduced in R2019b