diff --git a/doc/ImuFactor.lyx b/doc/ImuFactor.lyx index 4b71a29ed..f76ede023 100644 --- a/doc/ImuFactor.lyx +++ b/doc/ImuFactor.lyx @@ -1433,7 +1433,7 @@ Given the above solutions to the differential equations, we add noise modeling \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 \\ +\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 \\ @@ -1908,13 +1908,13 @@ G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc} 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\\ +\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\ +0 & \deriv p{\epsilon^{a}}^{T} & \deriv v{\epsilon^{a}}^{T} & 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 +0 & \deriv p{\epsilon^{int}}^{T} & 0 & 0 & 0\\ +0 & \deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\ +\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & 0 & 0 & 0 \end{array}\right] \end{multline*} @@ -1928,10 +1928,10 @@ G_{k}Q_{k}G_{k}^{T}=\left[\begin{array}{ccccccc} \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\\ +\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}^{T}+\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\ +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}\\ + & +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\ +\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\ 0 & 0 & 0 & \Sigma^{b^{a}} & 0\\ 0 & 0 & 0 & 0 & \Sigma^{b^{\omega}} \end{array}\right] @@ -1952,23 +1952,23 @@ which we can break into 3 matrices for clarity, representing the main diagonal \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\\ +\deriv{\theta}{\epsilon^{\omega}}\Sigma^{\omega}\deriv{\theta}{\epsilon^{\omega}}^{T} & 0 & 0 & 0 & 0\\ +0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T} & 0 & 0 & 0\\ +0 & 0 & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T} & 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\\ +\deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{22}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & 0 & 0 & 0\\ +0 & \deriv p{\epsilon^{int}}\Sigma^{int}\deriv p{\epsilon^{int}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\ +0 & 0 & \deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 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 & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv p{\eta_{init}^{b^{a}}}^{T} & \deriv{\theta}{\eta_{init}^{b^{\omega}}}\Sigma^{init_{21}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\ +\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & 0 & \deriv p{\epsilon^{a}}\Sigma^{a}\deriv v{\epsilon^{a}}^{T}+\deriv p{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv v{\eta_{init}^{b^{a}}}^{T} & 0 & 0\\ +\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{12}}\deriv{\theta}{\eta_{init}^{b^{\omega}}}^{T} & \deriv v{\epsilon^{a}}\Sigma^{a}\deriv p{\epsilon^{a}}^{T}+\deriv v{\eta_{init}^{b^{a}}}\Sigma^{init_{11}}\deriv p{\eta_{init}^{b^{a}}}^{T} & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 0 \end{array}\right] @@ -1979,6 +1979,73 @@ which we can break into 3 matrices for clarity, representing the main diagonal \end_layout +\begin_layout Subsubsection* +Covariance Discretization +\end_layout + +\begin_layout Standard +So far, all the covariances are assumed to be continuous since the state + and measurement models are considered to be continuous-time stochastic + processes. + However, we sample measurements in a discrete-time fashion, necessitating + the need to convert the covariances to their discrete time equivalents. +\end_layout + +\begin_layout Standard +The IMU is modeled as a first order Gauss-Markov process, with a measurement + noise and a process noise. + Following +\begin_inset CommandInset citation +LatexCommand cite +after "Alg. 1 Page 57" +key "Nikolic16thesis" +literal "false" + +\end_inset + + and +\begin_inset CommandInset citation +LatexCommand cite +after "Eqns 129-130" +key "Trawny05report_IndirectKF" +literal "false" + +\end_inset + +, the measurement noises +\begin_inset Formula $[\epsilon^{a},\epsilon^{\omega},\epsilon_{init}]$ +\end_inset + + are simply scaled by +\begin_inset Formula $\frac{1}{\Delta t}$ +\end_inset + +, and the process noises +\begin_inset Formula $[\epsilon^{int},\epsilon^{b^{a}},\epsilon^{b^{\omega}}]$ +\end_inset + + are scaled by +\begin_inset Formula $\Delta t$ +\end_inset + + where +\begin_inset Formula $\Delta t$ +\end_inset + + is the time interval between 2 consecutive samples. + For a thorough explanation of the discretization process, please refer + to +\begin_inset CommandInset citation +LatexCommand cite +after "Section 8.1" +key "Simon06book" +literal "false" + +\end_inset + +. +\end_layout + \begin_layout Standard \begin_inset CommandInset bibtex LatexCommand bibtex diff --git a/doc/ImuFactor.pdf b/doc/ImuFactor.pdf index 041d8bf1a..a4ec57fb7 100644 Binary files a/doc/ImuFactor.pdf and b/doc/ImuFactor.pdf differ diff --git a/doc/refs.bib b/doc/refs.bib index 97960d853..ec42fb032 100644 --- a/doc/refs.bib +++ b/doc/refs.bib @@ -50,3 +50,23 @@ title = {Calculus on manifolds}, volume = {1}, year = {1965}} + +@phdthesis{Nikolic16thesis, + title={Characterisation, calibration, and design of visual-inertial sensor systems for robot navigation}, + author={Nikolic, Janosch}, + year={2016}, + school={ETH Zurich} +} + +@book{Simon06book, + title={Optimal state estimation: Kalman, H infinity, and nonlinear approaches}, + author={Simon, Dan}, + year={2006}, + publisher={John Wiley \& Sons} +} + +@inproceedings{Trawny05report_IndirectKF, + title={Indirect Kalman Filter for 3 D Attitude Estimation}, + author={Nikolas Trawny and Stergios I. Roumeliotis}, + year={2005} +}