structs.estimator_data

Data shared by all estimators.

EstimatorData

State, covariance, sigma points, input, and measurement of an estimator.

as_state_array(data) classmethod

Returns the state as an array.

Parameters:

Name	Type	Description	Default
data	EstimatorData	The state of the estimator.	required

Returns:

Type	Description
Array	The concatenated state of shape (..., dim_x).

Source code in drone_estimators/structs/estimator_data.py

@classmethod
def as_state_array(cls, data: EstimatorData) -> Array:
    """Returns the state as an array.

    Args:
        data: The state of the estimator.

    Returns:
        The concatenated state of shape (..., dim_x).
    """
    xp = data.pos.__array_namespace__()
    x = xp.concat((data.pos, data.quat, data.vel, data.ang_vel), axis=-1)
    if data.rotor_vel is not None:
        x = xp.concat((x, data.rotor_vel), axis=-1)
    if data.dist_f is not None:
        x = xp.concat((x, data.dist_f), axis=-1)
    if data.dist_t is not None:
        x = xp.concat((x, data.dist_t), axis=-1)
    return x

create(pos, quat, vel, ang_vel, rotor_vel=None, dist_f=None, dist_t=None) classmethod

Create the state from the given values with identity covariance.

Parameters:

Name	Type	Description	Default
pos	Array	The position.	required
quat	Array	The quaternion.	required
vel	Array	The velocity.	required
ang_vel	Array	The angular velocity.	required
rotor_vel	Array | None	Optional, the rotor velocities.	None
dist_f	Array | None	Optional, the disturbance forces.	None
dist_t	Array | None	Optional, the disturbance torques.	None

Returns:

Type	Description
EstimatorData	The state.

Source code in drone_estimators/structs/estimator_data.py

@classmethod
def create(
    cls,
    pos: Array,
    quat: Array,
    vel: Array,
    ang_vel: Array,
    rotor_vel: Array | None = None,
    dist_f: Array | None = None,
    dist_t: Array | None = None,
) -> EstimatorData:
    """Create the state from the given values with identity covariance.

    Args:
        pos: The position.
        quat: The quaternion.
        vel: The velocity.
        ang_vel: The angular velocity.
        rotor_vel: Optional, the rotor velocities.
        dist_f: Optional, the disturbance forces.
        dist_t: Optional, the disturbance torques.

    Returns:
        The state.
    """
    batch_shape = pos.shape[:-1]
    dim_x = 13
    if rotor_vel is not None:
        dim_x = dim_x + 4
    if dist_f is not None:
        dim_x = dim_x + 3
    if dist_t is not None:
        dim_x = dim_x + 3

    covariance = np.tile(np.eye(dim_x), (*batch_shape, 1, 1))

    sigmas_f = np.zeros((*batch_shape, 2 * dim_x + 1, dim_x))
    sigmas_h = np.zeros((*batch_shape, 2 * dim_x + 1, 7))

    u = np.zeros((*batch_shape, 4))  # input
    z = np.zeros((*batch_shape, 7))  # measurement
    dt = np.ones(batch_shape)

    return cls(
        pos,
        quat,
        vel,
        ang_vel,
        rotor_vel,
        dist_f,
        dist_t,
        covariance,
        sigmas_f,
        sigmas_h,
        u,
        z,
        dt,
    )

create_empty(rotor_vel=False, dist_f=False, dist_t=False, dim_u=4, dim_z=7, batch_shape=()) classmethod

Create the initial state with zeros and identity covariance.

Parameters:

Name	Type	Description	Default
rotor_vel	bool	If the rotor velocities are part of the state.	False
dist_f	bool	If the disturbance forces are part of the state.	False
dist_t	bool	If the disturbance torques are part of the state.	False
dim_u	int	The dimension of the input.	4
dim_z	int	The dimension of the measurement.	7
batch_shape	tuple[int, ...]	The batch dimensions, e.g., (n_drones,).	()

Returns:

Type	Description
EstimatorData	The initial state.

Source code in drone_estimators/structs/estimator_data.py

@classmethod
def create_empty(
    cls,
    rotor_vel: bool = False,
    dist_f: bool = False,
    dist_t: bool = False,
    dim_u: int = 4,
    dim_z: int = 7,
    batch_shape: tuple[int, ...] = (),
) -> EstimatorData:
    """Create the initial state with zeros and identity covariance.

    Args:
        rotor_vel: If the rotor velocities are part of the state.
        dist_f: If the disturbance forces are part of the state.
        dist_t: If the disturbance torques are part of the state.
        dim_u: The dimension of the input.
        dim_z: The dimension of the measurement.
        batch_shape: The batch dimensions, e.g., (n_drones,).

    Returns:
        The initial state.
    """
    pos = np.zeros((*batch_shape, 3))
    quat = np.tile([0.0, 0.0, 0.0, 1.0], (*batch_shape, 1))
    vel = np.zeros((*batch_shape, 3))
    ang_vel = np.zeros((*batch_shape, 3))
    dim_x = 13
    if rotor_vel:
        rotor_vel = np.zeros((*batch_shape, 4))
        dim_x = dim_x + 4
    else:
        rotor_vel = None
    if dist_f:
        dist_f = np.zeros((*batch_shape, 3))
        dim_x = dim_x + 3
    else:
        dist_f = None
    if dist_t:
        dist_t = np.zeros((*batch_shape, 3))
        dim_x = dim_x + 3
    else:
        dist_t = None

    covariance = np.tile(np.eye(dim_x), (*batch_shape, 1, 1))

    sigmas_f = np.zeros((*batch_shape, 2 * dim_x + 1, dim_x))
    sigmas_h = np.zeros((*batch_shape, 2 * dim_x + 1, dim_z))

    u = np.zeros((*batch_shape, dim_u))  # input
    z = np.zeros((*batch_shape, dim_z))  # measurement
    dt = np.ones(batch_shape)

    return cls(
        pos,
        quat,
        vel,
        ang_vel,
        rotor_vel,
        dist_f,
        dist_t,
        covariance,
        sigmas_f,
        sigmas_h,
        u,
        z,
        dt,
    )

from_state_array(data, array) classmethod

Updates data in the given structure based on a given array.

Parameters:

Name	Type	Description	Default
data	EstimatorData	The state of the estimator to update.	required
array	Array	The state vector of shape (..., dim_x).	required

Returns:

Type	Description
EstimatorData	The updated state.

Source code in drone_estimators/structs/estimator_data.py

@classmethod
def from_state_array(cls, data: EstimatorData, array: Array) -> EstimatorData:
    """Updates data in the given structure based on a given array.

    Args:
        data: The state of the estimator to update.
        array: The state vector of shape (..., dim_x).

    Returns:
        The updated state.
    """
    pos = array[..., 0:3]
    quat = array[..., 3:7]
    vel = array[..., 7:10]
    ang_vel = array[..., 10:13]
    idx = 13
    if data.rotor_vel is not None:
        rotor_vel = array[..., idx : idx + 4]
        idx = idx + 4
    else:
        rotor_vel = None
    if data.dist_f is not None:
        dist_f = array[..., idx : idx + 3]
        idx = idx + 3
    else:
        dist_f = None
    if data.dist_t is not None:
        dist_t = array[..., idx : idx + 3]
        idx = idx + 3
    else:
        dist_t = None

    return data.replace(
        pos=pos,
        quat=quat,
        vel=vel,
        ang_vel=ang_vel,
        rotor_vel=rotor_vel,
        dist_f=dist_f,
        dist_t=dist_t,
    )

get_state_dim(data) classmethod

Returns the dimension of the state.

Parameters:

Name	Type	Description	Default
data	EstimatorData	The state of the estimator.	required

Returns:

Type	Description
int	The dimension of the state, depending on the optional states.

Source code in drone_estimators/structs/estimator_data.py

@classmethod
def get_state_dim(cls, data: EstimatorData) -> int:
    """Returns the dimension of the state.

    Args:
        data: The state of the estimator.

    Returns:
        The dimension of the state, depending on the optional states.
    """
    dim_x = 13
    if data.rotor_vel is not None:
        dim_x = dim_x + 4
    if data.dist_f is not None:
        dim_x = dim_x + 3
    if data.dist_t is not None:
        dim_x = dim_x + 3
    return dim_x