nodeState

Get or set node state in factor graph

Since R2022a

Syntax

state = nodeState(graph,nodeIDs)

nodeState(graph,nodeIDs,newStates)

Description

state = nodeState(graph,nodeIDs) gets the state of the nodes with the specified IDs nodeIDs in the specified factorGraph object graph.

nodeState(graph,nodeIDs,newStates) sets the state of the nodes with the specified IDs nodeIDs in the specified factorGraph object graph to the specified states newStates.

Input Arguments

graph — Factor graph

factorGraph object

Factor graph, specified as a factorGraph object.

nodeIDs — IDs of nodes to get or set

N-element row vector of nonnegative integers

IDs of the nodes to get or set, specified as an N-element row vector of nonnegative integers. N is the total number of nodes to get or set.

All specified node IDs must specify nodes of the same type.

newStates — New node states

M-by-N matrix

New node states, specified as an M-by-N matrix. M is the number of specified IDs and N is the number of state elements for the specified nodes. Each row of the matrix specifies the state element values for the corresponding element of nodeIDs.

These are the supported node types and the form of their corresponding states:

• POSE_SE2 — Pose in SE(2) state space in the form [x y theta]. x and y are the x- and y-positions, respectively, and theta is the orientation.

• POSE_SE3 — Pose in SE(3) state space in the form [x y z qw qx qy qz]. x, y, and z are the x-, y-, and z-positions, respectively, and qw, qx, qy, and qz represent the orientation as a quaternion.

• VEL3 — 3-D velocity in the form [vx vy vz]. vx, vy, and vz are the x-, y-, and z-velocities, respectively.

• POINT_XY — 2-D point in the form [x y]. x and y are the x- and y-positions, respectively.

• POINT_XYZ — 3-D point in the form [x y z]. x, y, and z are the x-, y-, and z-positions, respectively.

• IMU_BIAS — IMU gyroscope and accelerometer bias in the form [bias_gyro_x bias_gyro_y bias_gyro_z bias_accel_x bias_accel_y bias_accel_z], where:

  • bias_gyro_x, bias_gyro_y, and bias_gyro_z are the x, y, and z IMU gyroscope biases, respectively.

  • bias_accel_x, bias_accel_y, and bias_accel_z are the x, y, and z accelerometer biases, respectively.

Output Arguments

state — State of nodes

M-by-N matrix

State of the nodes, returned as an M-by-N matrix. M is the number of IDs and N is the number of state elements for the specified nodes.

These are the supported node types and the form of their corresponding states:

• POSE_SE2 — Pose in SE(2) state space in the form [x y theta]. x and y are the x- and y-positions, respectively, and theta is the orientation.

• POSE_SE3 — Pose in SE(3) state space in the form [x y z qw qx qy qz]. x, y, and z are the x-, y-, and z-positions, respectively, and qw, qx, qy, and qz represent the orientation as a quaternion.

• VEL3 — 3-D velocity in the form [vx vy vz]. vx, vy, and vz are the x-, y-, and z-velocities, respectively.

• POINT_XY — 2-D point in the form [x y]. x and y are the x- and y-positions, respectively.

• POINT_XYZ — 3-D point in the form [x y z]. x, y, and z are the x-, y-, and z-positions, respectively.

• IMU_BIAS — IMU gyroscope and accelerometer bias in the form [bias_gyro_x bias_gyro_y bias_gyro_z bias_accel_x bias_accel_y bias_accel_z], where:

  • bias_gyro_x, bias_gyro_y, and bias_gyro_z are the x, y, and z IMU gyroscope biases, respectively.

  • bias_accel_x, bias_accel_y, and bias_accel_z are the x, y, and z accelerometer biases, respectively.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2022a

R2023a: Get or set multiple nodes at one time

nodeState now supports getting and setting more than one node at a time by specifying nodeIDs as a vector of node IDs. newStates also supports additional rows to specify states for multiple nodes at once.