interpolate

Interpolate between states

collapse all in page

Syntax

interpStates = interpolate(space,state1,state2,ratio)

Description

example

interpStates = interpolate(space,state1,state2,ratio) interpolates states between the specified start state state1 and end state state2 based on the specified interpolation ratio ratio.

Examples

collapse all

Open Live Script

Create an SE(2) state space with default properties.

space = stateSpaceSE2
space = 
  stateSpaceSE2 with properties:

                 Name: 'SE2'
          StateBounds: [3×2 double]
    NumStateVariables: 3
             WeightXY: 1
          WeightTheta: 0.1000

Create a pair of states in 2-D space.

state1 = [2 10 -pi];
state2 = [0 -2.5 -pi/4];

Interpolate halfway between two states.

state = interpolate(space,state1,state2,0.5)
state = 1×3

    1.0000    3.7500   -1.9635

Interpolate multiple points with a fixed interval.

states = interpolate(space,state1,state2,0:0.02:1)
states = 51×3

    2.0000   10.0000   -3.1416
    1.9600    9.7500   -3.0945
    1.9200    9.5000   -3.0473
    1.8800    9.2500   -3.0002
    1.8400    9.0000   -2.9531
    1.8000    8.7500   -2.9060
    1.7600    8.5000   -2.8588
    1.7200    8.2500   -2.8117
    1.6800    8.0000   -2.7646
    1.6400    7.7500   -2.7175
      ⋮
Open Live Script

Create an SE(3) state space with default properties.

space = stateSpaceSE3
space = 
  stateSpaceSE3 with properties:

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

Create a pair of states in 3-D space.

state1 = [2 10 3 0.2 0 0 0.8];
state2 = [0 -2.5 4 0.7 0.3 0 0];

Interpolate halfway between two states.

state = interpolate(space,state1,state2,0.5)
state = 1×7

            1         3.75          3.5       0.7428      0.25188            0      0.62033

Interpolate multiple points with a fixed interval.

states = interpolate(space,state1,state2,0:0.02:1)
states = 51×7

            2           10            3      0.24254            0            0      0.97014
         1.96         9.75         3.02      0.26633     0.010877            0      0.96382
         1.92          9.5         3.04      0.28994     0.021745            0       0.9568
         1.88         9.25         3.06      0.31333     0.032598            0      0.94908
         1.84            9         3.08       0.3365     0.043428            0      0.94068
          1.8         8.75          3.1      0.35943     0.054225            0       0.9316
         1.76          8.5         3.12      0.38209     0.064984            0      0.92184
         1.72         8.25         3.14      0.40448     0.075695            0      0.91141
         1.68            8         3.16      0.42657     0.086352            0      0.90032
         1.64         7.75         3.18      0.44835     0.096946            0      0.88858
      ⋮

Input Arguments

collapse all

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

Start state for interpolation, specified as a three-element or seven-element vector of real values.

For the 2-D state space objects stateSpaceSE2, stateSpaceDubins, and stateSpaceReedsShepp, the state is a vector 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, the state is a vector 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.

Data Types: single | double

End state for interpolation, specified as a three-element or seven-element vector of real values.

For the 2-D state space objects stateSpaceSE2, stateSpaceDubins, and stateSpaceReedsShepp, the state is a vector 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, the state is a vector 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.

Data Types: single | double

Interpolation ratio, specified as a scalar in the range of [0, 1], or an n-element column vector of values in the range [0, 1] . n is the number of desired interpolation points.

Data Types: single | double

Output Arguments

collapse all

Interpolated states, returned as an n-by-3 or n-by-7 matrix of real values. n is the number of interpolation points specified by the ratio input argument.

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 the interpolated states.

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 the interpolated states.

Data Types: single | double

See Also

| | |

Introduced in R2019b

Navigation Toolbox Documentation

Support