Finalized IMU documentation

src/lib.rs

@@ -100,7 +100,7 @@ pub mod variables;
 
 /// Untagged symbols if `unchecked` API is desired.
 ///
-/// We strongly recommend using [assign_symbols](crate::assign_symbols) to
+/// We strongly recommend using [assign_symbols] to
 /// create and tag symbols with the appropriate types. However, we provide a
 /// number of pre-defined symbols if desired. Note these objects can't be tagged
 /// due to the orphan rules.

src/residuals/imu_preint/README.md

@@ -0,0 +1,209 @@
+Preintegration of IMU measurements.
+
+Inertial Measurement Unit (IMU) preintegration is a common technique used to summarize multiple IMU measurements into a single constraint used in preintegration. Our implementation is based on "local coordination integration", similar to the one [used in gtsam by default](https://github.com/borglab/gtsam/blob/4.2.0/doc/ImuFactor.pdf).
+# Example
+```
+use factrs::residuals::{ImuPreintegrator, Accel, Gravity, Gyro, ImuCovariance};
+use factrs::variables::{SE3, VectorVar3, ImuBias};
+use factrs::assign_symbols;
+
+assign_symbols!(X: SE3; V: VectorVar3; B: ImuBias);
+
+let mut preint =
+    ImuPreintegrator::new(ImuCovariance::default(), ImuBias::zeros(), Gravity::up());
+
+let accel = Accel::new(0.1, 0.2, 9.81);
+let gyro = Gyro::new(0.1, 0.2, 0.3);
+let dt = 0.01;
+// Integrate measurements for 100 steps
+for _ in 0..100 {
+   preint.integrate(&gyro, &accel, dt);
+}
+
+// Build the factor
+let factor = preint.build(X(0), V(0), B(0), X(1), V(1), B(1));
+```
+
+# Theory
+<div class="warning">
+Note, our convention throughout the library is to always put rotations first
+in any list, vector, matrix ordering, etc.
+</div>
+
+Our state will consist of five components, specifically the rotation from body to the world frame $R$, body velocity in the world frame $v$, body position in the world frame $p$,
+angular velocity bias $b^\omega$, and linear acceleration bias $b^a$. We'll
+denote the vector $X$ as these components together in the order listed
+previously.
+
+An IMU measures body angular velocity $\omega$ and body linear acceleration
+$a$. This results in the continuous time differential equation
+
+$$\begin{aligned}
+\dot{R} &= R (\omega - b^\omega) ^\wedge \\\\
+\dot{v} &= g + R (a - b^a) \\\\
+\dot{p} &= v \\\\
+\end{aligned}$$
+
+with initial conditions given by $R_0, v_0, p_0, b^{\omega}_0, b^a_0$, or
+$X_0$.
+
+The goal here is to find an integration that is *independent* of
+these initial conditions. In other words, we want a solution $\Delta X$ that
+allows us to perform $X_1 = X_0 \cdot \Delta X$ for some custom operation
+$\cdot$. This results in an integration we can do beforehand, aka a
+"preintegration".
+
+## Handling Gravity
+
+The first thing to note is there is a number of constants in the system that
+can be removed to simplify things. For example, gravity in the velocity
+differential equation,
+$$
+\dot{v} \triangleq \dot{v}^g + \dot{v}^a = g + R(a - b^a)
+$$
+$\dot{v}^g$ is a constant and can be integrated afterwards. Similarly, for
+the position component, we have
+$$
+\dot{p} \triangleq \dot{p}^0 + \dot{p}^g + \dot{p}^a = v_0 + v^g + v^a
+$$
+where $\dot{p}_0$ and $\dot{p}^g$ are constants and can be integrated
+afterwards. This brings our differential equations to
+$$ \begin{aligned}
+\dot{R} &= R (\omega - b^\omega) ^\wedge \\\\
+\dot{v}^a &= R(a - b^a) \\\\
+\dot{p}^a &= v^a \\\\
+\end{aligned} $$
+
+## Local Coordinates
+
+Additionally, rather than integrate in the global frame, we can integrate in
+a local frame that is aligned with the initial rotation $R_0$. We recommend
+reviewing (CITE) for more information on this. This results in
+the equations 
+$$ \begin{aligned}
+\dot{\theta} &= H(\theta)^{-1} (\omega - b^\omega) \\\\
+\dot{v}^a &= \exp(\theta^\wedge) (a - b^a) \\\\
+\dot{p}^a &= v^a \\\\
+\end{aligned} $$
+where $\theta$ is the local rotation in the Lie algebra and $H(\theta)$ is
+the Jacobian of the exponential map at $\theta$.
+
+## Discretization
+
+Moving this into discrete time using a simple Euler scheme results in,
+$$ \begin{aligned}
+\theta_{k+1} &= \theta_k + H(\theta_k)^{-1} (\omega_k - b^\omega_0) \Delta t \\\\
+v^a_{k+1} &= v^a_k + \exp(\theta_k^\wedge) (a_k - b^a_0) \Delta t \\\\
+p^a_{k+1} &= p^a_k + v^a_k \Delta t + \exp(\theta_k^\wedge) (a_k - b^a_0) \Delta t^2 / 2 \\\\ 
+\end{aligned} $$
+where we have assumed a constant bias over our timestep. We are now
+independent of the initial conditions (except for bias which we'll cover
+shortly). Additionally, the bias will have the simple discretized form,
+$$ \begin{aligned}
+b^\omega_{k+1} &= b^\omega_k \\\\
+b^a_{k+1} &= b^a_k \\\\
+\end{aligned} $$
+
+## Covariance Propagation
+
+TODO: Include some reference to Kalibur github on noise model units
+
+We also propagate the covariance of the entire system. There is six noise
+terms that we introduce to account for the various errors of the system,
+each of which is a 3-vector with a 3x3 covariance matrix. These are
+- $\epsilon_{\omega}$: the noise of the angular velocity measurement
+- $\epsilon_{a}$: the noise of the linear acceleration measurement
+- $\epsilon_{\omega^b}$: the noise of the angular velocity bias
+- $\epsilon_{a^b}$: the noise of the linear acceleration bias
+- $\epsilon_{int}$: the noise of the integration error
+- $\epsilon_{init}$: the noise of the bias initialization
+These will be stacked into a 18-vector with a block-diagonal 18x18 covariance
+matrix $Q$. We introduce the noise as follows,
+$$ \begin{aligned}
+\theta_{k+1} &= \theta_k + H(\theta_k)^{-1} (\omega_k + \epsilon_\omega - b^\omega_0 - \epsilon_{init}) \Delta t \\\\ 
+v^a_{k+1} &= v^a_k + \exp(\theta_k^\wedge) (a_k + \epsilon_a - b^a_0 - \epsilon_{init}) \Delta t \\\\
+p^a_{k+1} &= p^a_k + v^a_k \Delta t + \exp(\theta_k^\wedge) (a_k + \epsilon_a - b^a_0 - \epsilon_{init}) \Delta t^2 / 2 + \epsilon_{int} \\\\
+b^\omega_{k+1} &= b^\omega_k + \epsilon_{\omega^b} \\\\
+b^a_{k+1} &= b^a_k + \epsilon_{a^b} \\\\
+\end{aligned} $$
+
+We can then propagate a covariance matrix for our preintegrated values using the Jacobian of the system with respect to the noise terms. 
+$$ \begin{aligned}
+A_k &\triangleq \frac{\partial f}{\partial X} = \begin{bmatrix} 
+I - \frac{\Delta t}{2} \tilde{\omega}_k & 0 & 0 & -H(\theta_k)^{-1}\Delta t & 0 \\\\
+-R_k \tilde{a}^\wedge H(\theta) \Delta t & I & 0 & 0 & -R_k \Delta t \\\\ 
+-R_k \tilde{a}^\wedge H(\theta) \Delta t^2 / 2 & I \Delta t & I & 0 & -R_k \Delta t^2 /2 \\\\ 
+0 & 0 & 0 & I & 0 \\\\
+0 & 0 & 0 & 0 & I \\\\
+\end{bmatrix} \\\\
+B_k &\triangleq \frac{\partial f}{\partial \epsilon} = \begin{bmatrix}
+H(\theta_k)^{-1} \Delta t & 0 & 0 & 0 & 0 & -H(\theta)^{-1}\Delta t \\\\
+0 & R_k \Delta t & 0 & 0 & 0 & -R_k \Delta t \\\\
+0 & R_k \Delta t^2 / 2 & 0 & 0 & I & -R_k \Delta t^2 / 2 \\\\
+0 & 0 & I & 0 & 0 & 0 \\\\
+0 & 0 & 0 & I & 0 & 0 \\\\
+\end{bmatrix} \\\\
+\end{aligned} $$
+
+where we have used 
+$$ \begin{aligned}
+\tilde{\omega} &\triangleq \omega - b^\omega_0 \\\\
+\tilde{a} &\triangleq a - b^a_0 \\\\
+R_k &\triangleq \exp(\theta_k^\wedge) \\\\
+\frac{\partial H(\theta_k)^{-1} \tilde{\omega}}{\partial \theta} &\approx \frac{-1}{2}\tilde{\omega}^\wedge \\\\
+\frac{\partial R_k \tilde{a}}{\partial \theta} &\approx R_k \tilde{a}^\wedge H(\theta_k) \\\\
+\end{aligned} $$
+
+Put all together, we have the covariance propagation equation
+$$
+\Sigma_{k+1} = A_k \Sigma_k A_k^T + B_k Q B_k^T
+$$
+
+## First-order Bias Updates
+
+As mentioned previously, there still exists a dependence on the initial bias parameters. We approximate the impact of bias changes using a first order approximation. This is done using the Jacobian of the system with respect to the bias terms. We can construct these Jacobians iteratively as follows, letting $x = [\theta, v, p]^\top$, and linearizing about an initial bias $\tilde{b}^\omega_0$
+
+$$ \begin{aligned}
+H_{\omega}^{x_{k+1}} &\triangleq \frac{\partial \theta_{k+1}}{\partial b^\omega_0} = \frac{\partial \theta_{k+1}}{\partial \theta_k} \frac{\partial \theta_k}{\partial b^\omega_0} + \frac{\partial \theta_{k+1}}{\partial \omega_k} = \frac{\partial \theta_{k+1}}{\partial \theta_k} H_{\omega}^{x_{k}} + \frac{\partial \theta_{k+1}}{\partial \omega_k} 
+\end{aligned} $$
+
+Where the acceleration bias update is similar. Fortunately, the Jacobians required here are already computed in the $A_k$ matrix defined previously and we can simply extract them as needed.
+
+## Custom Operator
+
+Returning to our custom operator that we mentioned earlier to accomplish $X_1 = X_0 \cdot \Delta X$, we can define what it will be, taking into account the first-order updates, local coordinates, and the gravity terms we removed earlier.
+
+First, we update our preintegration equations to include any changes due to bias updates,
+$$ \begin{aligned}
+\tilde{\theta} &= \theta + H_{\omega}^{\theta_{k+1}} (b^\omega_0 - \tilde{b}^\omega_0) + H_{a}^{\theta_{k+1}} (b^a_0 - \tilde{b}^a_0) \\\\
+\tilde{v}^a &= v^a + H_{\omega}^{v_{k+1}} (b^\omega_0 - \tilde{b}^\omega_0) + H_{a}^{v_{k+1}} (b^a_0 - \tilde{b}^a_0) \\\\
+\tilde{p}^a &= p^a + H_{\omega}^{p_{k+1}} (b^\omega_0 - \tilde{b}^\omega_0) + H_{a}^{p_{k+1}} (b^a_0 - \tilde{b}^a_0) \\\\
+\end{aligned} $$
+
+Then, we can define our custom operator as follows,
+
+$$ \begin{aligned}
+R_1 &= R_0 \exp(\tilde{\theta}^\wedge) \\\\
+v_1 &= R_0 \tilde{v}^a + v_0 + g \Delta t \\\\
+p_1 &= R_0 \tilde{p}^a + p_0 + v_0 \Delta t + g \Delta t^2 / 2 \\\\
+b_1^\omega &= b_0^\omega \\\\
+b_1^a &= b_0^a \\\\
+\end{aligned} $$
+
+The covariance we propagated earlier will exist in the local frame (the right hand side) of these measurements. We thus can define our residual using a "right ominus" operator, 
+$$ \begin{aligned}
+X_1 = X_0 \cdot (\Delta X \cdot \epsilon) \implies r = \epsilon = \log( X_1^{-1}  (X_0 \cdot \Delta X) )
+\end{aligned} $$
+
+
+## Implementation Details
+
+[ImuPreintegrator] handles the integration of $\Delta X$ and the covariance propagation. It'll be the main entrypoint for doing Imu preintegration. It can produce a factor that contains the proper residual and covariance for use in a factor graph.
+
+[ImuCovariance] holds the six different covariances utilized in the covariance propagation. It also contains a handful of helper functions for simplify setting covariance values.
+
+Internally (if you care to peruse the codebase), there is a number of structures that follow naturally from the theory. For example,
+- `ImuDelta` represents $\Delta X$ and contains $g, \Delta t, \theta, v^a, p^a, \tilde{b}^\omega, \tilde{b}^a$.
+- `ImuState` represents $X$ and contains $R, v, p, b^\omega, b^a$.
+- `ImuBias` represents the bias terms and contains $b^\omega, b^a$.
+- `ImuPreintegrationResidual` is used to calculate the final residual.

src/residuals/imu_preint/mod.rs

@@ -1,7 +1,10 @@
+#![doc = include_str!("README.md")]
+
 mod newtypes;
-pub use newtypes::{Accel, AccelUnbiased, Gravity, Gyro, GyroUnbiased, ImuState};
+pub(crate) use newtypes::ImuState;
+pub use newtypes::{Accel, AccelUnbiased, Gravity, Gyro, GyroUnbiased};
 
 mod delta;
 
 mod residual;
-pub use residual::{ImuCovariance, ImuPreintegrationResidual, ImuPreintegrator};
+pub use residual::{ImuCovariance, ImuPreintegrator};

src/residuals/imu_preint/newtypes.rs

@@ -13,6 +13,7 @@ use crate::{
 pub struct Gyro<D: Numeric = dtype>(pub Vector3<D>);
 
 impl<D: Numeric> Gyro<D> {
+    /// Remove the bias from the gyro measurement
     pub fn remove_bias(&self, bias: &ImuBias<D>) -> GyroUnbiased<D> {
         GyroUnbiased(self.0 - bias.gyro())
     }
@@ -38,6 +39,7 @@ pub struct GyroUnbiased<D: Numeric = dtype>(pub Vector3<D>);
 pub struct Accel<D: Numeric = dtype>(pub Vector3<D>);
 
 impl<D: Numeric> Accel<D> {
+    /// Remove the bias from the accel measurement
     pub fn remove_bias(&self, bias: &ImuBias<D>) -> AccelUnbiased<D> {
         AccelUnbiased(self.0 - bias.accel())
     }
@@ -63,10 +65,12 @@ pub struct AccelUnbiased<D: Numeric = dtype>(pub Vector3<D>);
 pub struct Gravity<D: Numeric = dtype>(pub Vector3<D>);
 
 impl<D: Numeric> Gravity<D> {
+    /// Helper to get the gravity vector pointing up, i.e. [0, 0, 9.81]
     pub fn up() -> Self {
         Gravity(Vector3::new(D::from(0.0), D::from(0.0), D::from(9.81)))
     }
 
+    /// Helper to get the gravity vector pointing down, i.e. [0, 0, -9.81]
     pub fn down() -> Self {
         Gravity(Vector3::new(D::from(0.0), D::from(0.0), D::from(-9.81)))
     }