navlie.lib.imu.IMUState¶

class navlie.lib.imu.IMUState(nav_state: ndarray, bias_gyro: ndarray, bias_accel: ndarray, stamp: float | None = None, state_id: Any | None = None, direction='right')¶

Bases: CompositeState

The IMU state is a composite state that contains the navigation state (attitude, velocity, position), and the gyro and accelerometer biases. The navigation state is stored as an element of \(SE_2(3)\), and the biases are stored as vectors.

Parameters:

nav_state (np.ndarray with shape (5, 5)) – The navigation state stored as an element of SE_2(3). Contains orientation, velocity, and position.
bias_gyro (np.ndarray with size 3) – Gyroscope bias
bias_accel (np.ndarray with size 3) – Accelerometer bias
stamp (float, optional) – Timestamp, by default None
state_id (Any, optional) – Unique identifier, by default None
direction (str, optional) – Direction of the perturbation for the nav state, by default “right”

value: List[SE23State, VectorState, VectorState]¶

The substates are the CompositeState’s value.

Type:: List[State]

property attitude: ndarray¶

property velocity: ndarray¶

property position: ndarray¶

property bias: ndarray¶: Bias vector with in order [gyro_bias, accel_bias]

property bias_gyro: ndarray¶

property bias_accel: ndarray¶

property nav_state: ndarray¶

property pose: ndarray¶

property direction: str¶

copy()¶: Returns a new composite state object where the state values have also been copied.

jacobian_from_blocks(attitude: ndarray | None = None, position: ndarray | None = None, velocity: ndarray | None = None, bias_gyro: ndarray | None = None, bias_accel: ndarray | None = None)¶: Returns the jacobian of the IMU state with respect to the given blocks. In other words, this function takes care of the state vector ordering for you.

dof¶

Degree of freedom of the state

Type:: int

stamp¶

Timestamp

Type:: float

state_id¶

add_state(state: State, stamp: float | None = None, state_id=None)¶: Adds a state and it’s corresponding slice to the composite state.

get_dof_by_id(state_id) → int¶: Get degrees of freedom of sub-state by id.

get_index_by_id(state_id)¶: Get index of a particular state_id in the list of states.

get_matrix_block_by_ids(mat: ndarray, state_id_1: Any, state_id_2: Any | None = None) → ndarray¶

Gets the portion of a matrix corresponding to two states.

This function is useful when extract specific blocks of a covariance matrix, for example.

Parameters:

mat (np.ndarray) – N x N matrix
state_id_1 (Any) – State ID of state 1.
state_id_2 (Any, optional) – State ID of state 2. If None, state_id_2 is set to state_id_1.

Returns:

Subblock of mat corrsponding to slices of state_id_1 and state_id_2.

Return type:

np.ndarray

get_slice_by_id(state_id, slices=None)¶: Get slice of a particular state_id in the list of states.

get_slices() → List[slice]¶: Get slices for each state in the list of states.

get_stamp_by_id(state_id) → float¶: Get timestamp of sub-state by id.

get_state_by_id(state_id) → State¶: Get state object by id.

get_value_by_id(state_id) → Any¶: Get state value by id.

minus(x: CompositeState) → ndarray¶: A generic “subtraction” operation given another State object of the same type, always returning a numpy array.

minus_jacobian(x: CompositeState) → ndarray¶

Jacobian of the minus operator with respect to self.

\[\mathbf{J} = \frac{D (\mathcal{Y} \ominus \mathcal{X})}{D \mathcal{Y}}\]

That is, if dx = y.minus(x) then this is the Jacobian of dx with respect to y. For Lie groups, this is the inverse of the group Jacobian evaluated at dx = x1.minus(x2).

minus_jacobian_fd(x: State, step_size=1e-08) → ndarray¶: Calculates the minus jacobian with finite difference.

plus(dx, new_stamp: float | None = None) → CompositeState¶: Updates the value of each sub-state given a dx. Interally parses the dx vector.

plus_by_id(dx, state_id: int, new_stamp: float | None = None) → CompositeState¶: Updates a specific sub-state.

plus_jacobian(dx: ndarray) → ndarray¶

Jacobian of the plus operator. That is, using Lie derivative notation,

\[\mathbf{J} = \frac{D (\mathcal{X} \oplus \delta \mathbf{x})}{D \delta \mathbf{x}}\]

For Lie groups, this is known as the group Jacobian.

plus_jacobian_fd(dx, step_size=1e-08) → ndarray¶: Calculates the plus jacobian with finite difference.

remove_state_by_id(state_id)¶: Removes a given state by ID.

set_matrix_block_by_ids(new_mat_block: ndarray, mat: ndarray, state_id_1: Any, state_id_2: Any | None = None) → ndarray¶

Sets the portion of the covariance block corresponding to two states.

Parameters:

new_mat_block (np.ndarray) – A subblock to be entered into mat.
mat (np.ndarray) – Full matrix.
state_id_1 (Any) – State ID of state 1.
state_id_2 (Any, optional) – State ID of state 2. If None, state_id_2 is set to state_id_1.

Returns:

mat with updated subblock.

Return type:

np.ndarray

set_stamp_by_id(stamp: float, state_id)¶: Set the timestamp of a sub-state by id.

set_stamp_for_all(stamp: float)¶: Set the timestamp of all substates.

set_state_by_id(state: State, state_id)¶: Set the whole sub-state by id.

set_value_by_id(value: Any, state_id: Any)¶: Set the value of a sub-state by id.

to_list()¶: Converts the CompositeState object back into a list of states.

navlie.lib.imu.IMUKinematics

navlie.lib.models