diff --git a/doc/ImuFactor.lyx b/doc/ImuFactor.lyx index c79a5f37a..4b71a29ed 100644 --- a/doc/ImuFactor.lyx +++ b/doc/ImuFactor.lyx @@ -108,6 +108,222 @@ filename "macros.lyx" \end_layout +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}} +{\mathfrak{\mathbb{R}^{9}}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rninethree}{\mathfrak{\mathbb{R}^{9\times3}}} +{\mathfrak{\mathbb{R}^{9\times3}}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rninesix}{\mathfrak{\mathbb{R}^{9\times6}}} +{\mathfrak{\mathbb{R}^{9\times6}}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rninenine}{\mathfrak{\mathbb{R}^{9\times9}}} +{\mathfrak{\mathbb{R}^{9\times9}}} +\end_inset + + +\end_layout + +\begin_layout Subsubsection* +IMU Factor +\end_layout + +\begin_layout Standard +The IMU factor has 2 variants: +\end_layout + +\begin_layout Enumerate +ImuFactor is a 5-way factor between the previous pose and velocity, the + current pose and velocity, and the current IMU bias. +\end_layout + +\begin_layout Enumerate +ImuFactor2 is a 3-way factor between the previous NavState, the current + NavState and the current IMU bias. +\end_layout + +\begin_layout Standard +Both variants take a PreintegratedMeasurements object which encodes all + the IMU measurements between the previous timestep and the current timestep. +\end_layout + +\begin_layout Standard +There are also 2 variants of this class: +\end_layout + +\begin_layout Enumerate +Manifold Preintegration: This version keeps track of the incremental NavState + +\begin_inset Formula $\Delta X_{ij}$ +\end_inset + + with respect to the previous NavState, on the NavState manifold itself. + It also keeps track of the +\begin_inset Formula $\Rninesix$ +\end_inset + + Jacobian of +\begin_inset Formula $\Delta X_{ij}$ +\end_inset + + w.r.t. + the bias. + This corresponds to Forster et. + al. +\begin_inset CommandInset citation +LatexCommand cite +key "Forster15rss" +literal "false" + +\end_inset + + +\end_layout + +\begin_layout Enumerate +Tangent Preintegration: This version keeps track of the incremental NavState + in the NavState tangent space instead. + This is a +\begin_inset Formula $\Rnine$ +\end_inset + + vector +\emph on +preintegrated_ +\emph default +. + It also keeps track of the +\begin_inset Formula $\Rninesix$ +\end_inset + + jacobian of the +\emph on +preintegrated_ +\emph default + w.r.t. + the bias. + +\end_layout + +\begin_layout Standard +The main function of a factor is to calculate an error. + The easiest case to look at is the NavState variant in ImuFactor2, which + is given as: +\begin_inset Formula +\begin{equation} +\Delta X_{ij}=X_{j}-\hat{X_{ij}}\label{eq:imu-factor-error} +\end{equation} + +\end_inset + + +\end_layout + +\begin_layout Subsubsection* +Combined IMU Factor +\end_layout + +\begin_layout Standard +The IMU factor above requires that bias drift over time be modeled as a + separate stochastic process (using a BetweenFactor for example), a crucial + aspect given that the preintegrated measurements depend on these bias values + and are thus correlated. + For this reason, we provide another type of IMU factor which we term the + Combined IMU Factor. + This factor similarly has 2 variants: +\end_layout + +\begin_layout Enumerate +CombinedImuFactor is a 6-way factor between the previous pose, velocity + and IMU bias and the current pose, velocity and IMU bias. +\end_layout + +\begin_layout Enumerate +CombinedImuFactor2 is a 4-way factor between the previous NavState and IMU + bias and the current NavState and IMU bias. +\end_layout + +\begin_layout Subsubsection* +Covariance Matrices +\end_layout + +\begin_layout Standard +For IMU preintegration, it is important to propagate the uncertainty accurately + as well. + As such, we detail the various covariance matrices used in the preintegration + step. +\end_layout + +\begin_layout Itemize +Gyroscope Covariance +\begin_inset Formula $Q_{\omega}$ +\end_inset + +: Measurement uncertainty of the gyroscope. +\end_layout + +\begin_layout Itemize +Accelerometer Covariance +\begin_inset Formula $Q_{acc}$ +\end_inset + + : Measurement uncertainty of the accelerometer. +\end_layout + +\begin_layout Itemize +Accelerometer Bias Covariance +\begin_inset Formula $Q_{\Delta b^{acc}}$ +\end_inset + + : The covariance associated with the accelerometer bias random walk. +\end_layout + +\begin_layout Itemize +Gyroscope Bias Covariance +\begin_inset Formula $Q_{\Delta b^{\omega}}$ +\end_inset + + : The covariance associated with the gyroscope bias random walk. +\end_layout + +\begin_layout Itemize +Integration Covariance +\begin_inset Formula $Q_{int}$ +\end_inset + + : This is the uncertainty due to modeling errors in the integration from + acceleration to velocity and position. +\end_layout + +\begin_layout Itemize +Initial Bias Estimate Covariance +\begin_inset Formula $Q_{init}$ +\end_inset + + : This is the uncertainty associated with the estimation of the bias (since + we jointly estimate the bias as well). +\end_layout + \begin_layout Subsubsection* Navigation States \end_layout @@ -725,15 +941,6 @@ In other words, the vector field Retractions \end_layout -\begin_layout Standard -\begin_inset FormulaMacro -\newcommand{\Rnine}{\mathfrak{\mathbb{R}^{9}}} -{\mathfrak{\mathbb{R}^{9}}} -\end_inset - - -\end_layout - \begin_layout Standard Note that the use of the exponential map in local coordinate mappings is not obligatory, even in the context of Lie groups. @@ -1018,7 +1225,15 @@ In the IMU factor, we need to predict the NavState needs to be known in order to compensate properly for the initial velocity and rotated gravity vector. - Hence, the idea of Lupton was to split up + Hence, the idea of Lupton +\begin_inset CommandInset citation +LatexCommand cite +key "Lupton12tro" +literal "false" + +\end_inset + + was to split up \begin_inset Formula $v(t)$ \end_inset @@ -1075,8 +1290,11 @@ p_{g}(t) & = & R_{i}^{T}\frac{gt^{2}}{2} \end_inset -The recipe for the IMU factor is then, in summary. - Solve the ordinary differential equations +The recipe for the IMU factor is then, in summary: +\end_layout + +\begin_layout Enumerate +Solve the ordinary differential equations \begin_inset Formula \begin{eqnarray*} \dot{\theta}(t) & = & H(\theta(t))^{-1}\,\omega^{b}(t)\\ @@ -1095,7 +1313,10 @@ starting from zero, up to time \end_inset at all times. - Form the local coordinate vector as +\end_layout + +\begin_layout Enumerate +Form the local coordinate vector as \begin_inset Formula \[ \zeta(t_{ij})=\left[\theta(t_{ij}),p(t_{ij}),v(t_{ij})\right]=\left[\theta(t_{ij}),R_{i}^{T}V_{i}t_{ij}+R_{i}^{T}\frac{gt_{ij}^{2}}{2}+p_{v}(t_{ij}),R_{i}^{T}gt_{ij}+v_{a}(t_{ij})\right] @@ -1103,6 +1324,10 @@ starting from zero, up to time \end_inset + +\end_layout + +\begin_layout Enumerate Predict the NavState \begin_inset Formula $X_{j}$ \end_inset @@ -1197,7 +1422,50 @@ where we defined the rotation matrix \end_layout \begin_layout Subsubsection* -Noise Propagation +Noise Modeling +\end_layout + +\begin_layout Standard +Given the above solutions to the differential equations, we add noise modeling + to account for the various sources of error in the system +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{eqnarray} +\theta_{k+1} & = & \theta_{k}+H(\theta_{k})^{-1}\,(\omega_{k}^{b}+\epsilon_{k}^{\omega} -b_{k}^{\omega}-\epsilon_{init}^{\omega})\Delta_{t}\nonumber \\ +p_{k+1} & = & p_{k}+v_{k}\Delta_{t}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\frac{\Delta_{t}^{2}}{2}+\epsilon_{k}^{int}\label{eq:preintegration}\\ +v_{k+1} & = & v_{k}+R_{k}(a_{k}^{b}+\epsilon_{k}^{a}-b_{k}^{a}-\epsilon_{init}^{a})\Delta_{t}\nonumber \\ +b_{k+1}^{a} & = & b_{k}^{a}+\epsilon_{k}^{b^{a}}\nonumber \\ +b_{k+1}^{\omega} & = & b_{k}^{\omega}+\epsilon_{k}^{b^{\omega}}\nonumber +\end{eqnarray} + +\end_inset + + +\end_layout + +\begin_layout Standard +which we can write compactly as, +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{eqnarray} +\theta_{k+1} & = & f_{\theta}(\theta_{k},b_{k}^{w},\epsilon_{k}^{\omega},\epsilon_{init}^{b^{\omega}})\label{eq:compact-preintegration}\\ +p_{k+1} & = & f_{p}(p_{k},v_{k},\theta_{k},b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a},\epsilon_{k}^{int})\nonumber \\ +v_{k+1} & = & f_{v}(v_{k,}\theta_{k,}b_{k}^{a},\epsilon_{k}^{a},\epsilon_{init}^{a})\nonumber \\ +b_{k+1}^{a} & = & f_{b^{a}}(b_{k}^{a},\epsilon_{k}^{b^{a}})\nonumber \\ +b_{k+1}^{\omega} & = & f_{b^{\omega}}(b_{k}^{\omega},\epsilon_{k}^{b^{\omega}})\nonumber +\end{eqnarray} + +\end_inset + + +\end_layout + +\begin_layout Subsubsection* +Noise Propagation in IMU Factor \end_layout \begin_layout Standard @@ -1257,7 +1525,7 @@ Then the noise on propagates as \begin_inset Formula \begin{equation} -\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}+C_{k}\Sigma_{\eta}^{gd}C_{k}\label{eq:prop} +\Sigma_{k+1}=A_{k}\Sigma_{k}A_{k}^{T}+B_{k}\Sigma_{\eta}^{ad}B_{k}^{T}+C_{k}\Sigma_{\eta}^{gd}C_{k}^{T}\label{eq:prop} \end{equation} \end_inset @@ -1313,7 +1581,7 @@ We start with the noise propagation on \begin_layout Standard \begin_inset Formula \[ -\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t} +\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}+\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\Delta_{t} \] \end_inset @@ -1325,7 +1593,7 @@ It can be shown that for small we have \begin_inset Formula \[ -\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3x3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}} +\deriv{H(\theta_{k})^{-1}\omega_{k}^{b}}{\theta_{k}}\approx-\frac{1}{2}\Skew{\omega_{k}^{b}}\mbox{ and hence }\deriv{\theta_{k+1}}{\theta_{k}}=I_{3\times3}-\frac{\Delta t}{2}\Skew{\omega_{k}^{b}} \] \end_inset @@ -1375,9 +1643,9 @@ Putting all this together, we finally obtain \begin_inset Formula \[ A_{k}\approx\left[\begin{array}{ccc} -I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}\\ +I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3}\\ R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\ -R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3} +R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3} \end{array}\right] \] @@ -1403,26 +1671,12 @@ H(\theta_{k})^{-1}\Delta_{t}\\ \end_layout \begin_layout Subsubsection* -Combined IMU Factor +Noise Propagation in Combined IMU Factor \end_layout \begin_layout Standard We can similarly account for bias drift over time, as is commonly seen in commercial grade IMUs. - This is accomplished via the -\emph on -CombinedImuFactor -\emph default - which is a 6-way factor between the previous -\emph on -pose/velocity/bias -\emph default - and the -\emph on -pose/velocity/bias -\emph default - at the next timestep. - \end_layout \begin_layout Standard @@ -1430,14 +1684,18 @@ We expand the state vector as \begin_inset Formula $\zeta_{k}=[\theta_{k},p_{k},v_{k},b_{k}^{a},b_{k}^{\omega}]$ \end_inset - to include the bias terms. + to include the bias terms and define the augmented noise vector +\begin_inset Formula $\epsilon=[\epsilon_{k}^{\omega},\epsilon_{k}^{a},\epsilon_{k}^{b^{a}},\epsilon_{k}^{b^{\omega}},\epsilon_{k}^{int},\epsilon_{init}^{b^{a}},\epsilon_{init}^{b^{\omega}}]$ +\end_inset + +. This gives the noise propagation equation as \end_layout \begin_layout Standard \begin_inset Formula \begin{equation} -\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}\Sigma_{k}G_{k}\label{eq:prop-combined} +\Sigma_{k+1}=F_{k}\Sigma_{k}F_{k}^{T}+G_{k}Q_{k}G_{k}^{T}\label{eq:prop-combined} \end{equation} \end_inset @@ -1467,10 +1725,19 @@ where \end_inset is the -\begin_inset Formula $15\times15$ +\begin_inset Formula $15\times21$ \end_inset matrix for first order uncertainty propagation. + +\begin_inset Formula $Q_{k}$ +\end_inset + + defines the uncertainty of +\begin_inset Formula $\eta$ +\end_inset + +. The top-left \begin_inset Formula $9\times9$ \end_inset @@ -1509,22 +1776,213 @@ derivation as matrices \begin_inset Formula \[ F_{k}\approx\left[\begin{array}{ccccc} -I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & & & & H(\theta_{k})^{-1}\Delta_{t}\\ -R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2}\\ -R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3} & R_{k}\Delta_{t}\\ - & & & I_{3\times3}\\ - & & & & I_{3\times3} +I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & H(\theta_{k})^{-1}\Delta_{t}\\ +R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t} & R_{k}\frac{\Delta_{t}}{2}^{2} & 0_{3\times3}\\ +R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & 0_{3\times3} & I_{3\times3} & R_{k}\Delta_{t} & 0_{3\times3}\\ +0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} & 0_{3\times3}\\ +0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_{3\times3} \end{array}\right] \] \end_inset +\end_layout + +\begin_layout Standard +Similarly for +\begin_inset Formula $Q_{k},$ +\end_inset + +we get +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +Q_{k}=\left[\begin{array}{ccccccc} +\Sigma^{\omega}\\ + & \Sigma^{a}\\ + & & \Sigma^{b^{a}}\\ + & & & \Sigma^{b^{\omega}}\\ + & & & & \Sigma^{int}\\ + & & & & & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\ + & & & & & \Sigma^{init_{21}} & \Sigma^{init_{22}} +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +and for +\begin_inset Formula $G_{k}$ +\end_inset + + we get +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +G_{k}=\left[\begin{array}{ccccccc} +\deriv{\theta}{\epsilon^{\omega}} & \deriv{\theta}{\epsilon^{a}} & \deriv{\theta}{\epsilon^{b^{a}}} & \deriv{\theta}{\epsilon^{b^{\omega}}} & \deriv{\theta}{\epsilon^{int}} & \deriv{\theta}{\epsilon_{init}^{b^{a}}} & \deriv{\theta}{\epsilon_{init}^{b^{\omega}}}\\ +\deriv p{\epsilon^{\omega}} & \deriv p{\epsilon^{a}} & \deriv p{\epsilon^{b^{a}}} & \deriv p{\epsilon^{b^{\omega}}} & \deriv p{\epsilon^{int}} & \deriv p{\epsilon_{init}^{b^{a}}} & \deriv p{\epsilon_{init}^{b^{\omega}}}\\ +\deriv v{\epsilon^{\omega}} & \deriv v{\epsilon^{a}} & \deriv v{\epsilon^{b^{a}}} & \deriv v{\epsilon^{b^{\omega}}} & \deriv v{\epsilon^{int}} & \deriv v{\epsilon_{init}^{b^{a}}} & \deriv v{\epsilon_{init}^{b^{\omega}}}\\ +\deriv{b^{a}}{\epsilon^{\omega}} & \deriv{b^{a}}{\epsilon^{a}} & \deriv{b^{a}}{\epsilon^{b^{a}}} & \deriv{b^{a}}{\epsilon^{b^{\omega}}} & \deriv{b^{a}}{\epsilon^{int}} & \deriv{b^{a}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{a}}{\epsilon_{init}^{b^{\omega}}}\\ +\deriv{b^{\omega}}{\epsilon^{\omega}} & \deriv{b^{\omega}}{\epsilon^{a}} & \deriv{b^{\omega}}{\epsilon^{b^{a}}} & \deriv{b^{\omega}}{\epsilon^{b^{\omega}}} & \deriv{b^{\omega}}{\epsilon^{int}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{a}}} & \deriv{b^{\omega}}{\epsilon_{init}^{b^{\omega}}} +\end{array}\right]=\left[\begin{array}{ccccccc} +\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\ +0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\ +0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\ +0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\ +0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0 +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +We can perform the block-wise computation of +\begin_inset Formula $G_{k}Q_{k}G_{k}^{T}$ +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc} +\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\\ +0 & \deriv p{\epsilon^{a}} & 0 & 0 & \deriv p{\epsilon^{int}} & \deriv p{\eta_{init}^{b^{a}}} & 0\\ +0 & \deriv v{\epsilon^{a}} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}} & 0\\ +0 & 0 & I_{3\times3} & 0 & 0 & 0 & 0\\ +0 & 0 & 0 & I_{3\times3} & 0 & 0 & 0 +\end{array}\right]\left[\begin{array}{ccccccc} +\Sigma^{\omega}\\ + & \Sigma^{a}\\ + & & \Sigma^{b^{a}}\\ + & & & \Sigma^{b^{\omega}}\\ + & & & & \Sigma^{int}\\ + & & & & & \Sigma^{init_{11}} & \Sigma^{init_{12}}\\ + & & & & & \Sigma^{init_{21}} & \Sigma^{init_{22}} +\end{array}\right]G_{k}^{T} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc} +\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\ +0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\ +0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\ +0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\ +0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0 +\end{array}\right]G_{k}^{T} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{multline*} +G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc} +\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega} & 0 & 0 & 0 & 0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\\ +0 & \deriv p{\epsilon^{a}}\Sigma^{a} & 0 & 0 & \deriv p{\epsilon^{int}}\Sigma^{int} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\ +0 & \deriv v{\epsilon^{a}}\Sigma^{a} & 0 & 0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}} & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\\ +0 & 0 & \Sigma^{b^{a}} & 0 & 0 & 0 & 0\\ +0 & 0 & 0 & \Sigma^{b^{\omega}} & 0 & 0 & 0 +\end{array}\right]\\ +\left[\begin{array}{ccccc} +\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0\\ +0 & \deriv p{\epsilon^{a}} & \deriv v{\epsilon^{a}} & 0 & 0\\ +0 & 0 & 0 & I_{3\times3} & 0\\ +0 & 0 & 0 & 0 & I_{3\times3}\\ +0 & \deriv p{\epsilon^{int}} & 0 & 0 & 0\\ +0 & \deriv p{\eta_{init}^{b^{a}}} & \deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\ +\deriv{\theta}{\eta_{init}^{b^{\omega}}} & 0 & 0 & 0 & 0 +\end{array}\right] +\end{multline*} + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{multline*} +=\\ +\left[\begin{array}{ccccc} +\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\ +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}+\deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}\\ + & +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\ +\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\ +0 & 0 & 0 & \Sigma^{b^{a}} & 0\\ +0 & 0 & 0 & 0 & \Sigma^{b^{\omega}} +\end{array}\right] +\end{multline*} + +\end_inset + + +\end_layout + +\begin_layout Standard +which we can break into 3 matrices for clarity, representing the main diagonal + and off-diagonal elements +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{multline*} +=\\ +\left[\begin{array}{ccccc} +\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}} & 0 & 0 & 0 & 0\\ +0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}} & 0 & 0 & 0\\ +0 & 0 & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}} & 0 & 0\\ +0 & 0 & 0 & \Sigma^{b^{a}} & 0\\ +0 & 0 & 0 & 0 & \Sigma^{b^{\omega}} +\end{array}\right]+\\ +\left[\begin{array}{ccccc} +\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & 0 & 0 & 0 & 0\\ +0 & \deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & 0 & 0 & 0\\ +0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\ +0 & 0 & 0 & 0 & 0\\ +0 & 0 & 0 & 0 & 0 +\end{array}\right]+\\ +\left[\begin{array}{ccccc} +0 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\ +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & 0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}} & 0 & 0\\ +\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}} & 0 & 0 & 0\\ +0 & 0 & 0 & 0 & 0\\ +0 & 0 & 0 & 0 & 0 +\end{array}\right] +\end{multline*} + +\end_inset + + \end_layout \begin_layout Standard \begin_inset CommandInset bibtex LatexCommand bibtex +btprint "btPrintCited" bibfiles "refs" options "plain" diff --git a/doc/ImuFactor.pdf b/doc/ImuFactor.pdf index 933c71a74..041d8bf1a 100644 Binary files a/doc/ImuFactor.pdf and b/doc/ImuFactor.pdf differ diff --git a/doc/refs.bib b/doc/refs.bib index 414773483..97960d853 100644 --- a/doc/refs.bib +++ b/doc/refs.bib @@ -1,26 +1,52 @@ +%% This BibTeX bibliography file was created using BibDesk. +%% https://bibdesk.sourceforge.io/ + +%% Created for Varun Agrawal at 2021-09-27 17:39:09 -0400 + + +%% Saved with string encoding Unicode (UTF-8) + + + +@article{Lupton12tro, + author = {Lupton, Todd and Sukkarieh, Salah}, + date-added = {2021-09-27 17:38:56 -0400}, + date-modified = {2021-09-27 17:39:09 -0400}, + doi = {10.1109/TRO.2011.2170332}, + journal = {IEEE Transactions on Robotics}, + number = {1}, + pages = {61-76}, + title = {Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions}, + volume = {28}, + year = {2012}, + Bdsk-Url-1 = {https://doi.org/10.1109/TRO.2011.2170332}} + +@inproceedings{Forster15rss, + author = {Christian Forster and Luca Carlone and Frank Dellaert and Davide Scaramuzza}, + booktitle = {Robotics: Science and Systems}, + date-added = {2021-09-26 20:44:41 -0400}, + date-modified = {2021-09-26 20:45:03 -0400}, + title = {IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation}, + year = {2015}} + @article{Iserles00an, - title = {Lie-group methods}, - author = {Iserles, Arieh and Munthe-Kaas, Hans Z and - N{\o}rsett, Syvert P and Zanna, Antonella}, - journal = {Acta Numerica 2000}, - volume = {9}, - pages = {215--365}, - year = {2000}, - publisher = {Cambridge Univ Press} -} + author = {Iserles, Arieh and Munthe-Kaas, Hans Z and N{\o}rsett, Syvert P and Zanna, Antonella}, + journal = {Acta Numerica 2000}, + pages = {215--365}, + publisher = {Cambridge Univ Press}, + title = {Lie-group methods}, + volume = {9}, + year = {2000}} @book{Murray94book, - title = {A mathematical introduction to robotic manipulation}, - author = {Murray, Richard M and Li, Zexiang and Sastry, S - Shankar and Sastry, S Shankara}, - year = {1994}, - publisher = {CRC press} -} + author = {Murray, Richard M and Li, Zexiang and Sastry, S Shankar and Sastry, S Shankara}, + publisher = {CRC press}, + title = {A mathematical introduction to robotic manipulation}, + year = {1994}} @book{Spivak65book, - title = {Calculus on manifolds}, - author = {Spivak, Michael}, - volume = {1}, - year = {1965}, - publisher = {WA Benjamin New York} -} \ No newline at end of file + author = {Spivak, Michael}, + publisher = {WA Benjamin New York}, + title = {Calculus on manifolds}, + volume = {1}, + year = {1965}}