navlie.lib.imu.IMUKinematics¶
- class navlie.lib.imu.IMUKinematics(Q: ndarray, gravity=None)¶
Bases:
ProcessModelThe IMU Kinematics refer to the following continuous time model:
\[ \begin{align}\begin{aligned}\dot{\mathbf{r}} &= \mathbf{v}\\\dot{\mathbf{v}} &= \mathbf{C}\mathbf{a} + \mathbf{g}\\\dot{\mathbf{C}} &= \mathbf{C}\mathbf{\omega}^\wedge\end{aligned}\end{align} \]Using \(SE_2(3)\) extended poses, it can be shown that the discrete-time IMU kinematics are given by:
\[\mathbf{T}_{k} = \mathbf{G}_{k-1} \mathbf{T}_{k-1} \mathbf{U}_{k-1}\]where \(\mathbf{T}_{k}\) is the pose at time \(k\), \(\mathbf{G}_{k-1}\) is a matrix that depends on the gravity vector, and \(\mathbf{U}_{k-1}\) is a matrix that depends on the IMU measurements.
The \(\mathbf{G}_{k-1}\) and \(\mathbf{U}_{k-1}\) matrices are not quite elements of \(SE_2(3)\), but instead belong to a new group named here the “Incremental Euclidean Group” \(IE(3)\).
- Parameters:
Q (np.ndarray) – Discrete-time noise matrix.
g_a (np.ndarray) – Gravity vector resolved in the inertial frame. If None, default value is set to [0; 0; -9.80665].
- evaluate_with_jacobian(x: ~navlie.types.State, u: ~navlie.types.Input, dt: float) -> (<class 'navlie.types.State'>, <class 'numpy.ndarray'>)¶
Evaluates the process model and simultaneously returns the Jacobian as its second output argument. This is useful to override for performance reasons when the model evaluation and Jacobian have a lot of common calculations, and it is more efficient to calculate them in the same function call.
- input_covariance(x: State, u: Input, dt: float) → ndarray¶
Covariance matrix of additive noise on the input.
- input_jacobian_fd(x: State, u: Input, dt: float, step_size=1e-06, *args, **kwargs) → ndarray¶
Calculates the input jacobian with finite difference.
- jacobian_fd(x: State, u: Input, dt: float, step_size=1e-06, *args, **kwargs) → ndarray¶
Calculates the model jacobian with finite difference.
- evaluate(x: IMUState, u: IMU, dt: float) → IMUState¶
Propagates an IMU state forward one timestep from an IMU measurement.
The continuous-time IMU equations are discretized using the assumption that the IMU measurements are constant between two timesteps.
- jacobian(x: IMUState, u: IMU, dt: float) → ndarray¶
Returns the Jacobian of the IMU kinematics model with respect to the full state
- covariance(x: IMUState, u: IMU, dt: float) → ndarray¶
Covariance matrix \(\mathbf{Q}_k\) of the additive Gaussian noise \(\mathbf{w}_{k} \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_k)\). If this method is not overridden, the covariance of the process model error is approximated from the input covariance using a linearization procedure, with the input Jacobian evaluated using finite difference.