* Chirikjian (2011) Chirikjian G (2011) _Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications_ , volume 2.  Springer Science & Business Media.
  * Civera et al. (2009) Civera J, Bueno DR, Davison AJ and Montiel JMM (2009) Camera self-calibration for sequential bayesian structure from motion.  In: _2009 IEEE International Conference on Robotics and Automation_. pp. 403–408.  10.1109/ROBOT.2009.5152719.
  * Eckenhoff et al. (2019) Eckenhoff K, Geneva P, Bloecker J and Huang G (2019) Multi-camera visual-inertial navigation with online intrinsic and extrinsic calibration.  In: _Proc. International Conference on Robotics and Automation_. Montreal, Canada.
  * Eckenhoff et al. (2021) Eckenhoff K, Geneva P and Huang G (2021) Mimc-vins: A versatile and resilient multi-imu multi-camera visual-inertial navigation system.  _IEEE Transactions on Robotics_ 37(5): 1360–1380.  10.1109/TRO.2021.3049445.
  * Faugeras et al. (1992) Faugeras OD, Luong QT and Maybank SJ (1992) Camera self-calibration: Theory and experiments.  In: Sandini G (ed.) _Computer Vision — ECCV’92_. Berlin, Heidelberg: Springer Berlin Heidelberg.  ISBN 978-3-540-47069-4, pp. 321–334.
  * Forster et al. (2017) Forster C, Carlone L, Dellaert F and Scaramuzza D (2017) On-manifold preintegration for real-time visual-inertial odometry.  _IEEE Transactions on Robotics_ 33(1): 1–21.
  * Furgale et al. (2013) Furgale P, Rehder J and Siegwart R (2013) Unified temporal and spatial calibration for multi-sensor systems.  In: _Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems_. pp. 1280–1286.
  * Geneva et al. (2020) Geneva P, Eckenhoff K, Lee W, Yang Y and Huang G (2020) Openvins: A research platform for visual-inertial estimation.  In: _Proc. of the IEEE International Conference on Robotics and Automation_. Paris, France.  URL https://github.com/rpng/open_vins.
  * Guo et al. (2014) Guo C, Kottas D, DuToit R, Ahmed A, Li R and Roumeliotis S (2014) Efficient visual-inertial navigation using a rolling-shutter camera with inaccurate timestamps.  In: _Proc. of the Robotics: Science and Systems Conference_. Berkeley, CA.
  * Guo and Roumeliotis (2013) Guo C and Roumeliotis S (2013) IMU-RGBD camera 3D pose estimation and extrinsic calibration: Observability analysis and consistency improvement.  In: _Proc. of the IEEE International Conference on Robotics and Automation_. Karlsruhe, Germany.
  * Hartley and Zisserman (2004) Hartley R and Zisserman A (2004) _Multiple View Geometry in Computer Vision_.  Cambridge University Press.
  * Heng et al. (2019) Heng L, Choi B, Cui Z, Geppert M, Hu S, Kuan B, Liu P, Nguyen R, Yeo YC, Geiger A et al. (2019) Project autovision: Localization and 3d scene perception for an autonomous vehicle with a multi-camera system.  In: _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, pp. 4695–4702.
  * Hesch et al. (2013) Hesch J, Kottas D, Bowman S and Roumeliotis S (2013) Consistency analysis and improvement of vision-aided inertial navigation.  _IEEE Transactions on Robotics_ 30(1): 158–176.
  * Hesch et al. (2014) Hesch JA, Kottas DG, Bowman SL and Roumeliotis SI (2014) Consistency analysis and improvement of vision-aided inertial navigation.  _IEEE Transactions on Robotics_ 30(1): 158–176.  10.1109/TRO.2013.2277549.
  * Huai et al. (2021) Huai J, Zhuang Y, Yuan Q and Lin Y (2021) Continuous-time spatiotemporal calibration of a rolling shutter camera-imu system.  _CoRR_ abs/2108.07200.
  * Huang (2012) Huang G (2012) _Improving the Consistency of Nonlinear Estimators: Analysis, Algorithms, and Applications_.  PhD Thesis, Department of Computer Science and Engineering, University of Minnesota.  URL https://conservancy.umn.edu/handle/11299/146717.
  * Huang (2019) Huang G (2019) Visual-inertial navigation: A concise review.  In: _Proc. International Conference on Robotics and Automation_. Montreal, Canada.
  * Huang et al. (2010) Huang G, Mourikis AI and Roumeliotis SI (2010) Observability-based rules for designing consistent EKF SLAM estimators.  _International Journal of Robotics Research_ 29(5): 502–528.  10.1177/0278364909353640.
  * Jeong et al. (2019) Jeong J, Cho Y, Shin YS, Roh H and Kim A (2019) Complex urban dataset with multi-level sensors from highly diverse urban environments.  _The International Journal of Robotics Research_ 38(6): 642–657.
  * Jones and Soatto (2011) Jones ES and Soatto S (2011) Visual-inertial navigation, mapping and localization: A scalable real-time causal approach.  _International Journal of Robotics Research_ 30(4): 407–430.
  * Jung et al. (2020) Jung JH, Heo S and Park CG (2020) Observability analysis of imu intrinsic parameters in stereo visual–inertial odometry.  _IEEE Transactions on Instrumentation and Measurement_ 69(10): 7530–7541.  10.1109/TIM.2020.2985174.
  * Karpenko et al. (2011) Karpenko A, Jacobs D, Baek J and Levoy M (2011) Digital video stabilization and rolling shutter correction using gyroscopes.  _Stanford University Computer Science Tech Report CSTR 2011-03_ 1(2): 13.
  * Kelly and Sukhatme (2011) Kelly J and Sukhatme GS (2011) Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration.  _International Journal of Robotics Research_ 30(1): 56–79.
  * Kukelova et al. (2020) Kukelova Z, Albl C, Sugimoto A, Schindler K and Pajdla T (2020) Minimal rolling shutter absolute pose with unknown focal length and radial distortion.  In: Vedaldi A, Bischof H, Brox T and Frahm JM (eds.) _Computer Vision – ECCV 2020_. Cham: Springer International Publishing.  ISBN 978-3-030-58558-7, pp. 698–714.
  * Lee et al. (2020) Lee W, Eckenhoff K, Yang Y, Geneva P and Huang G (2020) Visual-inertial-wheel odometry with online calibration.  In: _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_.
  * Leutenegger et al. (2015) Leutenegger S, Lynen S, Bosse M, Siegwart R and Furgale P (2015) Keyframe-based visual-inertial odometry using nonlinear optimization.  _International Journal of Robotics Research_ 34(3): 314–334.
  * Li and Mourikis (2013a) Li M and Mourikis A (2013a) High-precision, consistent EKF-based visual-inertial odometry.  _International Journal of Robotics Research_ 32(6): 690–711.
  * Li and Mourikis (2013b) Li M and Mourikis AI (2013b) Optimization-based estimator design for vision-aided inertial navigation.  In: _Robotics: Science and Systems_. Berlin, Germany, pp. 241–248.
  * Li and Mourikis (2014) Li M and Mourikis AI (2014) Online temporal calibration for Camera-IMU systems: Theory and algorithms.  _International Journal of Robotics Research_ 33(7): 947–964.
  * Li et al. (2014) Li M, Yu H, Zheng X and Mourikis AI (2014) High-fidelity sensor modeling and self-calibration in vision-aided inertial navigation.  In: _IEEE International Conference on Robotics and Automation (ICRA)_. pp. 409–416.
  * Martinelli (2011) Martinelli A (2011) State estimation based on the concept of continuous symmetry and observability analysis: The case of calibration.  _IEEE Transactions on Robotics_ 27(2): 239–255.
  * Martinelli (2012) Martinelli A (2012) Vision and IMU data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination.  _IEEE Transactions on Robotics_ 28(1): 44–60.
  * Martinelli (2014) Martinelli A (2014) Observability properties and deterministic algorithms in visual-inertial structure from motion.  _Found. Trends Robot_ 3(3): 139–209.  10.1561/2300000030.
  * Maybeck (1979) Maybeck PS (1979) _Stochastic Models, Estimation, and Control_ , volume 1.  London: Academic Press.
  * Meingast et al. (2005) Meingast M, Geyer C and Sastry SS (2005) Geometric models of rolling-shutter cameras.  _The 6th Workshop on Omnidirectional Vision_ abs/cs/0503076: 4.
  * Mirzaei and Roumeliotis (2008) Mirzaei FM and Roumeliotis SI (2008) A Kalman filter-based algorithm for IMU-camera calibration: Observability analysis and performance evaluation.  _IEEE Transactions on Robotics_ 24(5): 1143–1156.
  * Mourikis and Roumeliotis (2007) Mourikis AI and Roumeliotis SI (2007) A multi-state constraint Kalman filter for vision-aided inertial navigation.  In: _Proceedings of the IEEE International Conference on Robotics and Automation_. Rome, Italy, pp. 3565–3572.
  * Nguyen and Lhuillier (2017) Nguyen TT and Lhuillier M (2017) Self-calibration of omnidirectional multi-cameras including synchronization and rolling shutter.  _Computer Vision and Image Understanding_ 162: 166–184.  10.1016/j.cviu.2017.08.010.
  * Nikolic et al. (2016) Nikolic J, Burri M, Gilitschenski I, Nieto J and Siegwart R (2016) Non-parametric extrinsic and intrinsic calibration of visual-inertial sensor systems.  _IEEE Sensors Journal_ 16(13): 5433–5443.  10.1109/JSEN.2016.2556662.
  * Olson (2011) Olson E (2011) Apriltag: A robust and flexible visual fiducial system.  In: _Proc. of the IEEE International Conference on Robotics and Automation_. Shanghai, China, pp. 3400–3407.
  * OpenCV Developers Team (2021) OpenCV Developers Team (2021) Open source computer vision (OpenCV) library.  Available: http://opencv.org.
  * Oth et al. (2013) Oth L, Furgale P, Kneip L and Siegwart R (2013) Rolling shutter camera calibration.  In: _2013 IEEE Conference on Computer Vision and Pattern Recognition_. pp. 1360–1367.  10.1109/CVPR.2013.179.
  * Qian and Chellappa (2004) Qian G and Chellappa R (2004) Bayesian self-calibration of a moving camera.  _Computer Vision Image Understanding_ 95(3): 287–316.  10.1016/j.cviu.2004.03.009.
  * Qin et al. (2018) Qin T, Li P and Shen S (2018) VINS-Mono: A robust and versatile monocular visual-inertial state estimator.  _IEEE Transactions on Robotics_ 34(4): 1004–1020.
  * Qin and Shen (2018) Qin T and Shen S (2018) Online temporal calibration for monocular visual-inertial systems.  In: _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. pp. 3662–3669.  10.1109/IROS.2018.8593603.
  * Rehder et al. (2016) Rehder J, Nikolic J, Schneider T, Hinzmann T and Siegwart R (2016) Extending kalibr: Calibrating the extrinsics of multiple imus and of individual axes.  In: _IEEE International Conference on Robotics and Automation (ICRA)_. pp. 4304–4311.  10.1109/ICRA.2016.7487628.
  * Rehder and Siegwart (2017) Rehder J and Siegwart R (2017) Camera/imu calibration revisited.  _IEEE Sensors Journal_ 17(11): 3257–3268.  10.1109/JSEN.2017.2674307.
  * Schneider et al. (2019) Schneider T, Li M, Cadena C, Nieto J and Siegwart R (2019) Observability-aware self-calibration of visual and inertial sensors for ego-motion estimation.  _IEEE Sensors Journal_ 19(10): 3846–3860.
  * Schneider et al. (2019) Schneider T, Li M, Cadena C, Nieto J and Siegwart R (2019) Observability-aware self-calibration of visual and inertial sensors for ego-motion estimation.  _IEEE Sensors Journal_ 19(10): 3846–3860.  10.1109/JSEN.2019.2893809.
  * Schubert et al. (2019) Schubert D, Demmel N, von Stumberg L, Usenko V and Cremers D (2019) Rolling-shutter modelling for visual-inertial odometry.  In: _International Conference on Intelligent Robots and Systems (IROS)_.
  * Schubert et al. (2018) Schubert D, Goll T, Demmel N, Usenko V, Stückler J and Cremers D (2018) The tum vi benchmark for evaluating visual-inertial odometry.  In: _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, pp. 1680–1687.
  * Shen et al. (2014) Shen S, Mulgaonkar Y, Michael N and Kumar V (2014) Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft MAV.  In: _Proc. of the IEEE International Conference on Robotics and Automation_. Hong Kong, China, pp. 4974–4981.
  * Trawny and Roumeliotis (2005) Trawny N and Roumeliotis SI (2005) Indirect Kalman filter for 3D attitude estimation.  Technical report, University of Minnesota, Dept. of Comp. Sci. & Eng.  URL http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf.
  * Tsao and Jan (2019) Tsao SH and Jan SS (2019) Observability analysis and performance evaluation of ekf-based visual-inertial odometry with online intrinsic camera parameter calibration.  _IEEE Sensors Journal_ 19(7): 2695–2703.  10.1109/JSEN.2018.2886764.
  * Usenko et al. (2020) Usenko VC, Demmel N, Schubert D, Stückler J and Cremers D (2020) Visual-inertial mapping with non-linear factor recovery.  _IEEE Robotics and Automation Letters_ 5: 422–429.
  * Wu et al. (2017a) Wu K, Zhang T, Su D, Huang S and Dissanayake G (2017a) An invariant-ekf vins algorithm for improving consistency.  In: _Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems_. pp. 1578–1585.
  * Wu et al. (2015) Wu KJ, Ahmed AM, Georgiou GA and Roumeliotis SI (2015) A square root inverse filter for efficient vision-aided inertial navigation on mobile devices.  In: _Robotics: Science and Systems Conference (RSS)_.
  * Wu et al. (2017b) Wu KJ, Guo CX, Georgiou G and Roumeliotis SI (2017b) VINS on wheels.  In: _Proc. of the IEEE International Conference on Robotics and Automation_. pp. 5155–5162.
  * Xiao et al. (2019) Xiao Y, Ruan X, Chai J, Zhang X and Zhu X (2019) Online imu self-calibration for visual-inertial systems.  _Sensors_ 19(7).  10.3390/s19071624.
  * Yang et al. (2020a) Yang Y, Babu BPW, Chen C, Huang G and Ren L (2020a) Analytic combined imu integration for visual-inertial navigation.  In: _Proc. of the IEEE International Conference on Robotics and Automation_. Paris, France.
  * Yang et al. (2019) Yang Y, Geneva P, Eckenhoff K and Huang G (2019) Degenerate motion analysis for aided INS with online spatial and temporal calibration.  _IEEE Robotics and Automation Letters (RA-L)_ 4(2): 2070–2077.
  * Yang et al. (2020b) Yang Y, Geneva P, Zuo X and Huang G (2020b) Online imu intrinsic calibration: Is it necessary?  In: _Proc. of Robotics: Science and Systems (RSS)_. Corvallis, Or.
  * Yang and Huang (2019) Yang Y and Huang G (2019) Observability analysis of aided ins with heterogeneous features of points, lines and planes.  _IEEE Transactions on Robotics_ 35(6): 399–1418.
  * Yang et al. (2017) Yang Y, Maley J and Huang G (2017) Null-space-based marginalization: Analysis and algorithm.  In: _Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems_. Vancouver, Canada, pp. 6749–6755.
  * Zachariah and Jansson (2010) Zachariah D and Jansson M (2010) Joint calibration of an inertial measurement unit and coordinate transformation parameters using a monocular camera.  In: _2010 International Conference on Indoor Positioning and Indoor Navigation_. pp. 1–7.  10.1109/IPIN.2010.5646840.
  * Zhang et al. (2021) Zhang M, Zuo X, Chen Y, Liu Y and Li M (2021) Pose estimation for ground robots: On manifold representation, integration, reparameterization, and optimization.  _IEEE Transactions on Robotics_ 37(4): 1081–1099.  10.1109/TRO.2020.3043970.

##  15 Appendix A: IMU Intrinsic Jacobians

In the following derivations, we will compute the Jacobians for all the
variables that might appear in the IMU models, including scale/axis correction
for gyroscope $\mathbf{D}_{w}$ (6 parameters), scale/axis correction for
accelerometer $\mathbf{D}_{a}$ (6 parameters), rotation from gyroscope to IMU
frame ${}^{I}_{w}\mathbf{R}$, rotation from accelerometer to IMU frame
${}^{I}_{a}\mathbf{R}$ and gravitiy sensitivity $\mathbf{T}_{g}$ (9
parameters). We repeat the corrected IMU readings for easier derivation:

$\displaystyle{}^{I}\bm{\omega}$
$\displaystyle={}^{I}_{w}\mathbf{R}\mathbf{D}_{w}\left({}^{w}\bm{\omega}_{m}-\mathbf{T}_{g}{}^{I}\mathbf{a}-\mathbf{b}_{g}-\mathbf{n}_{g}\right)$
(98) $\displaystyle{}^{I}\mathbf{a}$
$\displaystyle={}^{I}_{a}\mathbf{R}\mathbf{D}_{a}\left({}^{a}\mathbf{a}_{m}-\mathbf{b}_{a}-\mathbf{n}_{a}\right)$
(99)

To simplify the derivations, we define ${}^{I}\hat{\mathbf{a}}$ and
${}^{I}\tilde{\mathbf{a}}$ as:

$\displaystyle{}^{I}{\mathbf{a}}$
$\displaystyle\simeq{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}\left({}^{a}\mathbf{a}_{m}-\hat{\mathbf{b}}_{a}\right)+{}^{I}_{a}\hat{\mathbf{R}}\mathbf{H}_{Da}\tilde{\mathbf{x}}_{Da}$
$\displaystyle~{}~{}~{}+\lfloor{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}\left({}^{a}\mathbf{a}_{m}-\hat{\mathbf{b}}_{a}\right)\rfloor\delta\bm{\theta}_{Ia}-{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}\tilde{\mathbf{b}}_{a}-{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}{\mathbf{n}}_{a}$
$\displaystyle{}^{I}\hat{\mathbf{a}}$
$\displaystyle={}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}\left({}^{a}\mathbf{a}_{m}-\hat{\mathbf{b}}_{a}\right)$
$\displaystyle{}^{I}\tilde{\mathbf{a}}$
$\displaystyle={}^{I}_{a}\hat{\mathbf{R}}\mathbf{H}_{Da}\tilde{\mathbf{x}}_{Da}+\lfloor{}^{I}\hat{\mathbf{a}}\rfloor\delta\bm{\theta}_{Ia}-{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}\tilde{\mathbf{b}}_{a}-{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}{\mathbf{n}}_{a}$

We define ${}^{I}\hat{\bm{\omega}}$ and ${}^{I}\tilde{\bm{\omega}}$ as:

$\displaystyle{}^{I}\bm{\omega}$
$\displaystyle={}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\left({}^{w}\bm{\omega}_{m}-\hat{\mathbf{T}}_{g}{}^{I}\hat{\mathbf{a}}-\hat{\mathbf{b}}_{g}\right)$
$\displaystyle+\lfloor{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\left({}^{w}\bm{\omega}_{m}-\hat{\mathbf{T}}_{g}{}^{I}\hat{\mathbf{a}}-\hat{\mathbf{b}}_{g}\right)\rfloor\delta\bm{\theta}_{Iw}$
$\displaystyle+{}^{I}_{w}\hat{\mathbf{R}}\mathbf{H}_{Dw}\tilde{\mathbf{x}}_{Dw}-{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\left(\hat{\mathbf{T}}_{g}{}^{I}\tilde{\mathbf{a}}+\mathbf{H}_{Tg}\tilde{\mathbf{x}}_{Tg}\right)$
$\displaystyle-{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\left(\tilde{\mathbf{b}}_{g}+\mathbf{n}_{g}\right)$
$\displaystyle{}^{I}\hat{\bm{\omega}}$
$\displaystyle={}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\left({}^{w}\bm{\omega}_{m}-\hat{\mathbf{T}}_{g}{}^{I}\hat{\mathbf{a}}-\hat{\mathbf{b}}_{g}\right)$
$\displaystyle{}^{I}\tilde{\bm{\omega}}$
$\displaystyle=-{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\tilde{\mathbf{b}}_{g}+{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}\tilde{\mathbf{b}}_{a}$
$\displaystyle+{}^{I}_{w}\hat{\mathbf{R}}\mathbf{H}_{Dw}\tilde{\mathbf{x}}_{Dw}-{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}{}^{I}_{a}\hat{\mathbf{R}}\mathbf{H}_{Da}\tilde{\mathbf{x}}_{Da}$
$\displaystyle+\lfloor{}^{I}\hat{\bm{\omega}}\rfloor\delta\bm{\theta}_{Iw}-{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}\lfloor{}^{I}\hat{\mathbf{a}}\rfloor\delta\bm{\theta}_{Ia}$
$\displaystyle-{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\mathbf{H}_{Tg}\tilde{\mathbf{x}}_{Tg}-{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\mathbf{n}_{g}$
$\displaystyle+{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}\mathbf{n}_{a}$

where we have:

$\displaystyle\mathbf{H}_{Dw}$
$\displaystyle=\begin{bmatrix}{}^{w}\hat{w}_{1}\mathbf{e}_{1}&{}^{w}\hat{w}_{2}\mathbf{e}_{1}&{}^{w}\hat{w}_{2}\mathbf{e}_{2}&{}^{w}\hat{w}_{3}\mathbf{I}_{3}\end{bmatrix}$
(100) $\displaystyle\mathbf{H}_{Da}$
$\displaystyle=\begin{bmatrix}{}^{a}\hat{a}_{1}\mathbf{e}_{1}&{}^{a}\hat{a}_{2}\mathbf{e}_{1}&{}^{a}\hat{a}_{2}\mathbf{e}_{2}&{}^{a}\hat{a}_{3}\mathbf{I}_{3}\end{bmatrix}$
(101) $\displaystyle\mathbf{H}_{Tg}$
$\displaystyle=\begin{bmatrix}{}^{I}\hat{a}_{1}\mathbf{I}_{3}&{}^{I}\hat{a}_{2}\mathbf{I}_{3}&{}^{I}\hat{a}_{3}\mathbf{I}_{3}\end{bmatrix}$
(102)

By summarizing the above equations, we have:

$\displaystyle\begin{bmatrix}{}^{I_{k}}\tilde{\bm{\omega}}\\\
{}^{I_{k}}\tilde{\mathbf{a}}\end{bmatrix}$
$\displaystyle=\begin{bmatrix}\mathbf{H}_{b}&\mathbf{H}_{in}\end{bmatrix}\begin{bmatrix}\tilde{\mathbf{x}}_{b}\\\
\tilde{\mathbf{x}}_{in}\end{bmatrix}+\mathbf{H}_{n}\begin{bmatrix}\mathbf{n}_{g}\\\
\mathbf{n}_{a}\end{bmatrix}$ (103)

where we have defined:

$\displaystyle\mathbf{H}_{b}$
$\displaystyle=\mathbf{H}_{n}=\begin{bmatrix}-{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}&{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}\\\
\mathbf{0}_{3}&-{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}\end{bmatrix}$
(104) $\displaystyle\mathbf{H}_{in}$
$\displaystyle=\begin{bmatrix}\mathbf{H}_{w}&\mathbf{H}_{a}&\mathbf{H}_{Iw}&\mathbf{H}_{Ia}&\mathbf{H}_{g}\end{bmatrix}$
(105)

with:

$\displaystyle\mathbf{H}_{w}$
$\displaystyle=\begin{bmatrix}{}^{I}_{w}\hat{\mathbf{R}}\mathbf{H}_{Dw}\\\
\mathbf{0}_{3}\end{bmatrix}$ (106) $\displaystyle\mathbf{H}_{a}$
$\displaystyle=\begin{bmatrix}-{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}{}^{I}_{a}\hat{\mathbf{R}}\mathbf{H}_{Da}\\\
{}^{I}_{a}\hat{\mathbf{R}}\mathbf{H}_{Da}\end{bmatrix}$ (107)
$\displaystyle\mathbf{H}_{Iw}$
$\displaystyle=\begin{bmatrix}\lfloor{}^{I}\hat{\bm{\omega}}\rfloor\\\
\mathbf{0}_{3}\end{bmatrix}$ (108) $\displaystyle\mathbf{H}_{Ia}$
$\displaystyle=\begin{bmatrix}-{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}\lfloor{}^{I}\hat{\mathbf{a}}\rfloor\\\
\lfloor{}^{I}\hat{\mathbf{a}}\rfloor\end{bmatrix}$ (109)
$\displaystyle\mathbf{H}_{g}$
$\displaystyle=\begin{bmatrix}-{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\mathbf{H}_{Tg}\\\
\mathbf{0}_{3}\end{bmatrix}$ (110)

Hence, $\Delta\mathbf{R}_{k}$ from Eq. (43), can be written as:

$\displaystyle\Delta\mathbf{R}_{k}$
$\displaystyle\simeq\exp\left({}^{I_{k}}\bm{\omega}\delta
t_{k}\right)=\exp\left(({}^{I_{k}}\hat{\bm{\omega}}+{}^{I_{k}}\tilde{\bm{\omega}})\delta
t_{k}\right)$ (111)
$\displaystyle=\exp\left(\Delta\hat{\bm{\theta}}_{k}\right)\exp\left(\mathbf{J}_{r}(\Delta\hat{\bm{\theta}}_{k}){}^{I_{k}}\tilde{\bm{\omega}}\delta
t_{k}\right)$ (112)

where $\Delta\hat{\bm{\theta}}_{k}={}^{I_{k}}\hat{\bm{\omega}}\delta t_{k}$.

$\Delta\mathbf{p}_{k}$ from Eq. (44) can be written as:

$\displaystyle\Delta\mathbf{p}_{k}$
$\displaystyle=\scalebox{0.9}{\mbox{$\displaystyle\int^{t_{k+1}}_{t_{k}}\int^{s}_{t_{k}}{}^{I_{k}}_{I_{\tau}}\mathbf{R}{}^{I_{\tau}}\mathbf{a}d\tau
ds$}}$
$\displaystyle\simeq\int^{t_{k+1}}_{t_{k}}\int^{s}_{t_{k}}\exp\left({}^{I_{k}}\bm{\omega}\delta\tau\right){}^{I_{k}}\mathbf{a}d\tau
ds$
$\displaystyle=\int^{t_{k+1}}_{t_{k}}\int^{s}_{t_{k}}\exp\left(\left({}^{I_{k}}\hat{\bm{\omega}}+{}^{I_{k}}\tilde{\bm{\omega}}\right)\delta\tau\right)\left({}^{I_{k}}\hat{\mathbf{a}}+{}^{I_{k}}\tilde{\mathbf{a}}\right)d\tau
ds$
$\displaystyle\simeq\underbrace{\int^{t_{k+1}}_{t_{k}}\int^{s}_{t_{k}}\exp\left({}^{I_{k}}\hat{\bm{\omega}}\delta\tau\right){}^{I_{k}}\hat{\mathbf{a}}d\tau
ds}_{\Delta\hat{\mathbf{p}}_{k}}$
$\displaystyle-\underbrace{\int^{t_{k+1}}_{t_{k}}\int^{s}_{t_{k}}\exp\left({}^{I_{k}}\hat{\bm{\omega}}\delta\tau\right)\lfloor{}^{I_{k}}\hat{\mathbf{a}}\rfloor\mathbf{J}_{r}({}^{I_{k}}\bm{\omega}\delta\tau)\delta\tau
d\tau ds}_{\bm{\Xi}_{4}}{}^{I_{k}}\tilde{\bm{\omega}}$
$\displaystyle+\underbrace{\int^{t_{k+1}}_{t_{k}}\int^{s}_{t_{k}}\exp\left({}^{I_{k}}\hat{\bm{\omega}}\delta\tau\right)d\tau
ds}_{\bm{\Xi}_{2}}{}^{I_{k}}\tilde{\mathbf{a}}$

$\Delta\mathbf{v}_{k}$ from Eq. (45) can be written as:

$\displaystyle\Delta\mathbf{v}_{k}$
$\displaystyle=\scalebox{0.9}{\mbox{$\displaystyle\int^{t_{k+1}}_{t_{k}}{}^{I_{k}}_{I_{\tau}}\mathbf{R}{}^{I_{\tau}}\mathbf{a}d\tau$}}$
$\displaystyle\simeq\int^{t_{k+1}}_{t_{k}}\exp\left({}^{I_{k}}\bm{\omega}\delta\tau\right){}^{I_{k}}\mathbf{a}d\tau$
$\displaystyle=\int^{t_{k+1}}_{t_{k}}\exp\left(\left({}^{I_{k}}\hat{\bm{\omega}}+{}^{I_{k}}\tilde{\bm{\omega}}\right)\delta\tau\right)\left({}^{I_{k}}\hat{\mathbf{a}}+{}^{I_{k}}\tilde{\mathbf{a}}\right)d\tau$
$\displaystyle\simeq\underbrace{\int^{t_{k+1}}_{t_{k}}\exp\left({}^{I_{k}}\hat{\bm{\omega}}\delta\tau\right){}^{I_{k}}\hat{\mathbf{a}}d\tau}_{\Delta\hat{\mathbf{v}}_{k}}$
$\displaystyle-\underbrace{\int^{t_{k+1}}_{t_{k}}\exp\left({}^{I_{k}}\hat{\bm{\omega}}\delta\tau\right)\lfloor{}^{I_{k}}\hat{\mathbf{a}}\rfloor\mathbf{J}_{r}({}^{I_{k}}\bm{\omega}\delta\tau)\delta\tau
d\tau}_{\bm{\Xi}_{3}}{}^{I_{k}}\tilde{\bm{\omega}}$
$\displaystyle+\underbrace{\int^{t_{k+1}}_{t_{k}}\exp\left({}^{I_{k}}\hat{\bm{\omega}}\delta\tau\right)d\tau}_{\bm{\Xi}_{1}}{}^{I_{k}}\tilde{\mathbf{a}}$

By summarizing the above derivations, we have:

$\displaystyle\Delta\mathbf{R}_{k}$
$\displaystyle=\Delta\hat{\mathbf{R}}_{k}\exp\left(\mathbf{J}_{r}(\Delta\hat{\bm{\theta}}_{k}){}^{I_{k}}\tilde{\bm{\omega}}\delta
t_{k}\right)$ (113) $\displaystyle\Delta\mathbf{p}_{k}$
$\displaystyle=\Delta\hat{\mathbf{p}}_{k}-\bm{\Xi}_{4}{}^{I_{k}}\tilde{\bm{\omega}}+\bm{\Xi}_{2}{}^{I_{k}}\tilde{\mathbf{a}}$
(114) $\displaystyle\Delta\mathbf{v}_{k}$
$\displaystyle=\Delta\hat{\mathbf{v}}_{k}-\bm{\Xi}_{3}{}^{I_{k}}\tilde{\bm{\omega}}+\bm{\Xi}_{1}{}^{I_{k}}\tilde{\mathbf{a}}$
(115)

The linearized model for IMU dynamics can be written as:

$\displaystyle\tilde{\mathbf{x}}_{n_{k+1}}$
$\displaystyle=\bm{\Phi}_{nn}\tilde{\mathbf{x}}_{n_{k}}+\bm{\Phi}_{wa}\begin{bmatrix}{}^{I_{k}}\tilde{\bm{\omega}}\\\
{}^{I_{k}}\tilde{\mathbf{a}}\end{bmatrix}$ (116)

where:

$\displaystyle\bm{\Phi}_{nn}$
$\displaystyle=\begin{bmatrix}\Delta\hat{\mathbf{R}}^{\top}_{k}&\mathbf{0}_{3}&\mathbf{0}_{3}\\\
-{}^{G}_{I_{k}}\hat{\mathbf{R}}\lfloor\Delta\hat{\mathbf{p}}_{k}\rfloor&\mathbf{I}_{3}&\mathbf{I}_{3}\delta
t_{k}\\\
-{}^{G}_{I_{k}}\hat{\mathbf{R}}\lfloor\Delta\hat{\mathbf{v}}_{k}\rfloor&\mathbf{0}_{3}&\mathbf{I}_{3}\end{bmatrix}$
(117) $\displaystyle\bm{\Phi}_{wa}$
$\displaystyle=\begin{bmatrix}\mathbf{J}_{r}(\Delta\hat{\bm{\theta}}_{k})\delta
t_{k}&\mathbf{0}_{3}\\\
-{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{4}&{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{2}\\\
-{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{3}&{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{1}\end{bmatrix}$
(118)

By plugging Eq. (103) into Eq. (116) and adding biases, the overall linearized
system for IMU state can be written as:

$\displaystyle\tilde{\mathbf{x}}_{I_{k+1}}$
$\displaystyle=\bm{\Phi}_{I(k+1,k)}\tilde{\mathbf{x}}_{I_{k}}+\mathbf{G}_{k}\mathbf{n}_{dI}$
(119)

where $\mathbf{n}_{dI}=\begin{bmatrix}\mathbf{n}^{\top}_{dg}\
\mathbf{n}^{\top}_{da}\ \mathbf{n}^{\top}_{dwg}\
\mathbf{n}^{\top}_{dwa}\end{bmatrix}^{\top}$ denotes the discretized IMU
noises; $\bm{\Phi}_{I(k+1,k)}$ and $\mathbf{G}_{k}$ are computed as:

$\displaystyle\bm{\Phi}_{I(k+1,k)}$
$\displaystyle=\begin{bmatrix}\bm{\Phi}_{nn}&\bm{\Phi}_{wa}\mathbf{H}_{b}&\bm{\Phi}_{wa}\mathbf{H}_{in}\\\
\mathbf{0}_{6\times 9}&\mathbf{I}_{6}&\mathbf{0}_{6\times m}\\\
\mathbf{0}_{m\times 9}&\mathbf{0}_{m\times 6}&\mathbf{I}_{m}\end{bmatrix}$
(120) $\displaystyle\mathbf{G}_{k}$
$\displaystyle=\begin{bmatrix}\bm{\Phi}_{wa}\mathbf{H}_{n}&\mathbf{0}_{9\times
6}\\\ \mathbf{0}_{6}&\mathbf{I}_{6}\delta t_{k}\\\ \mathbf{0}_{m\times
6}&\mathbf{0}_{m\times 6}\end{bmatrix}$ (121)

Note that
$\mathbf{n}_{d*}\sim\mathcal{N}(\mathbf{0},\frac{\sigma^{2}_{*}\mathbf{I}_{3}}{\delta
t_{k}})$ and hence the covariance for $\mathbf{n}_{dI}$ can be written as:

$\displaystyle\mathbf{Q}_{dI}$
$\displaystyle=\begin{bmatrix}\frac{\sigma^{2}_{g}}{\delta
t_{k}}\mathbf{I}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}\\\
\mathbf{0}_{3}&\frac{\sigma^{2}_{a}}{\delta
t_{k}}\mathbf{I}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}\\\
\mathbf{0}_{3}&\mathbf{0}_{3}&\frac{\sigma^{2}_{wg}}{\delta
t_{k}}\mathbf{I}_{3}&\mathbf{0}_{3}\\\
\mathbf{0}_{3}&\mathbf{0}_{3}&\mathbf{0}_{3}&\frac{\sigma^{2}_{wa}}{\delta
t_{k}}\mathbf{I}_{3}\end{bmatrix}$ (122)

##  16 Appendix B: Camera Jacobians

We will show the detailed derivations for the Jacobians shown in Eq. (76). The
camera intrinsic Jacobians $\mathbf{H}_{Cin}$ can be written as:

$\displaystyle\mathbf{H}_{Cin}=\begin{bmatrix}\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial{\begin{bmatrix}\tilde{f_{u}}~{}\tilde{f_{v}}~{}\tilde{c_{u}}~{}\tilde{c_{v}}\end{bmatrix}}^{\top}}&\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial{\begin{bmatrix}\tilde{k_{1}}~{}\tilde{k_{2}}~{}\tilde{p_{1}}~{}\tilde{p_{2}}\end{bmatrix}}^{\top}}\end{bmatrix}$
(123)
$\displaystyle\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial{\begin{bmatrix}\tilde{f_{u}}~{}\tilde{f_{v}}~{}\tilde{c_{u}}~{}\tilde{c_{v}}\end{bmatrix}}^{\top}}=\begin{bmatrix}u_{d}&0&1&0\\\
0&v_{d}&0&1\end{bmatrix}$ (124)
$\displaystyle\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial{\begin{bmatrix}\tilde{k_{1}}~{}\tilde{k_{2}}\end{bmatrix}}^{\top}}=\begin{bmatrix}f_{u}u_{n}r^{2}&f_{u}u_{n}r^{4}\\\
f_{v}v_{n}r^{2}&f_{v}v_{n}r^{4}\end{bmatrix}$ (125)
$\displaystyle\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial{\begin{bmatrix}\tilde{p_{1}}~{}\tilde{p_{2}}\end{bmatrix}}^{\top}}=\begin{bmatrix}2f_{u}u_{n}v_{n}&f_{u}(r^{2}+2u^{2}_{n})\\\
f_{v}(r^{2}+2v^{2}_{n})&2f_{v}u_{n}v_{n}\end{bmatrix}$ (126)

We continue to compute
$\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial\tilde{\mathbf{z}}_{n}}$ and
$\frac{\partial\tilde{\mathbf{z}}_{n}}{\partial{}^{C}\tilde{\mathbf{p}}_{f}}$
for $\mathbf{H}_{\mathbf{p}_{f}}$ within Eq. (76) as:

$\displaystyle\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial\tilde{\mathbf{z}}_{n}}$
$\displaystyle=\begin{bmatrix}h_{11}&h_{12}\\\ h_{21}&h_{22}\end{bmatrix}$
(127)
$\displaystyle\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial{}^{C}\tilde{\mathbf{p}}_{f}}$
$\displaystyle=\frac{1}{{}^{C}z^{2}_{f}}\begin{bmatrix}{}^{C}z_{f}&0&-{}^{C}x_{f}\\\
0&{}^{C}z_{f}&-{}^{C}y_{f}\end{bmatrix}$ (128)

with $\mathbf{h}_{11}$, $\mathbf{h}_{12}$, $\mathbf{h}_{21}$ and
$\mathbf{h}_{22}$ defined as:

$\displaystyle\mathbf{h}_{11}$
$\displaystyle=f_{u}(d+2k_{1}u^{2}_{n}+4k_{2}u^{2}_{n}r^{2}+2p_{1}v_{n}+6p_{2}u_{n})$
$\displaystyle\mathbf{h}_{12}$
$\displaystyle=f_{u}(2k_{1}u_{n}v_{n}+4k_{2}u_{n}v_{n}r^{2}+2p_{1}u_{n}+2p_{2}v_{n})$
$\displaystyle\mathbf{h}_{21}$
$\displaystyle=f_{v}(2k_{1}u_{n}v_{n}+4k_{2}u_{n}v_{n}r^{2}+2p_{1}u_{n}+2p_{2}v_{n})$
$\displaystyle\mathbf{h}_{22}$
$\displaystyle=f_{v}(d+2k_{1}v^{2}_{n}+4k_{2}v^{2}_{n}r^{2}+6p_{1}v_{n}+2p_{2}u_{n})$

The Jacobians of ${}^{C}\mathbf{p}_{f}$ regarding to the IMU state
$\mathbf{x}_{I}$ are written as:

$\displaystyle\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{\mathbf{x}}_{I}}$
$\displaystyle=\begin{bmatrix}\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{\mathbf{x}}_{n}}&\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{\mathbf{x}}_{b}}&\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{\mathbf{x}}_{in}}\end{bmatrix}$
(129)
$\displaystyle\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{\mathbf{x}}_{n}}$
$\displaystyle={}^{C}_{I}\hat{\mathbf{R}}{}^{I}_{G}\hat{\mathbf{R}}\begin{bmatrix}\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I}\rfloor{}^{G}_{I}\hat{\mathbf{R}}&-\mathbf{I}_{3}&\mathbf{0}_{3}\end{bmatrix}$
(130)
$\displaystyle\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{\mathbf{x}}_{b}}$
$\displaystyle=\mathbf{0}_{3\times
6},~{}~{}\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{\mathbf{x}}_{in}}=\mathbf{0}_{3\times
24}$ (131)

The Jacobians of ${}^{C}\mathbf{p}_{f}$ regarding to the IMU-camera spatial-
temporal calibration state $\mathbf{x}_{IC}$ are written as:

$\displaystyle\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{\mathbf{x}}_{IC}}$
$\displaystyle=\begin{bmatrix}\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\delta\bm{\theta}_{IC}}&\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial{}^{C}\tilde{\mathbf{p}}_{I}}&\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{t}_{d}}&\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{t}_{r}}\end{bmatrix}$
(132)
$\displaystyle\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\delta\bm{\theta}_{IC}}$
$\displaystyle=\lfloor{}^{C}_{I}\hat{\mathbf{R}}{}^{I}_{G}\hat{\mathbf{R}}\left({}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I}\right)\rfloor$
(133)
$\displaystyle\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial{}^{C}\tilde{\mathbf{p}}_{I}}$
$\displaystyle=\mathbf{I}_{3}$ (134)
$\displaystyle\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{t}_{d}}$
$\displaystyle=\scalebox{0.9}{\mbox{$\displaystyle{}^{C}_{I}\hat{\mathbf{R}}{}^{I}_{G}\hat{\mathbf{R}}\left(\lfloor\left({}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I}\right)\rfloor{}^{G}_{I}\hat{\mathbf{R}}{}^{I}\hat{\bm{\omega}}-{}^{G}\hat{\mathbf{v}}_{I}\right)$}}$
(135)
$\displaystyle\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{t}_{r}}$
$\displaystyle=\frac{m}{M}\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{t}_{d}}$
(136)

Note that when computing the Jacobians for $t_{d}$ and $t_{r}$, we are using
the following linearization:

$\displaystyle{}^{G}_{I{(t)}}\mathbf{R}$
$\displaystyle\simeq{}^{G}_{I{(\hat{t})}}\hat{\mathbf{R}}\exp(\delta\bm{\theta}_{I})\exp({}^{I}\hat{\bm{\omega}}\tilde{t}_{d}+\frac{m}{M}{}^{I}\hat{\bm{\omega}}\tilde{t}_{r})$
(137) $\displaystyle{}^{G}\mathbf{p}_{I(t)}$
$\displaystyle\simeq{}^{G}\hat{\mathbf{p}}_{I(\hat{t})}+{}^{G}\tilde{\mathbf{p}}_{I}+{}^{G}\hat{\mathbf{v}}_{I}\tilde{t}_{d}+\frac{m}{M}{}^{G}\hat{\mathbf{v}}_{I}\tilde{t}_{r}$
(138)

The Jacobians of ${}^{C}\mathbf{p}_{f}$ regarding to the feature state
$\mathbf{x}_{f}$ is written as:

$\displaystyle\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\tilde{\mathbf{x}}_{f}}$
$\displaystyle=\frac{\partial{}^{C}\tilde{\mathbf{p}}_{f}}{\partial\delta{}^{G}\tilde{\mathbf{p}}_{f}}={}^{C}_{I}\hat{\mathbf{R}}{}^{I}_{G}\hat{\mathbf{R}}$
(139)

##  17 Appendix C: Observability Matrix

We show the detailed derivations for $\mathbf{M}_{n}$, $\mathbf{M}_{b}$,
$\mathbf{M}_{in}$, $\mathbf{M}_{IC}$, $\mathbf{M}_{Cin}$ and $\mathbf{M}_{f}$.
The $\mathbf{M}_{n}$ is computed as:

$\displaystyle\mathbf{M}_{n}=$
$\displaystyle\mathbf{H}_{\mathbf{p}_{f}}{}^{C}_{I}\hat{\mathbf{R}}{}^{I_{k}}_{G}\hat{\mathbf{R}}\begin{bmatrix}\bm{\Gamma}_{1}&\bm{\Gamma}_{2}&\bm{\Gamma}_{3}\end{bmatrix}$
(140)

with:

$\displaystyle\bm{\Gamma}_{1}$
$\displaystyle=\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{1}}-{}^{G}\hat{\mathbf{v}}_{I_{1}}\delta
t_{k}+\frac{1}{2}{}^{G}\mathbf{g}\delta
t^{2}_{k}\rfloor{}^{G}_{I_{1}}\hat{\mathbf{R}}$ $\displaystyle\bm{\Gamma}_{2}$
$\displaystyle=-\mathbf{I}_{3}$ $\displaystyle\bm{\Gamma}_{3}$
$\displaystyle=-\mathbf{I}_{3}\delta t_{k}$

The $\mathbf{M}_{b}$ is computed as:

$\displaystyle\mathbf{M}_{b}=$
$\displaystyle\mathbf{H}_{\mathbf{p}_{f}}{}^{C}_{I}\hat{\mathbf{R}}{}^{I_{k}}_{G}\hat{\mathbf{R}}\begin{bmatrix}\bm{\Gamma}_{4}&\bm{\Gamma}_{5}\end{bmatrix}$
(141)

with:

$\displaystyle\bm{\Gamma}_{4}$
$\displaystyle=-\bigg{(}\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\hat{\bm{\theta}}_{k}\right)\delta
t_{k}$
$\displaystyle~{}~{}~{}~{}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{4}\bigg{)}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}$
$\displaystyle\bm{\Gamma}_{5}$
$\displaystyle=\bigg{(}\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\hat{\bm{\theta}}_{k}\right){}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}\delta
t_{k}$
$\displaystyle~{}~{}~{}~{}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\left(\bm{\Xi}_{4}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}+\bm{\Xi}_{2}\right)\bigg{)}{}^{I}_{a}\hat{\mathbf{R}}\hat{\mathbf{D}}_{a}$

The $\mathbf{M}_{in}$ can be computed as:

$\displaystyle\mathbf{M}_{in}=$
$\displaystyle\mathbf{H}_{\mathbf{p}_{f}}{}^{C}_{I}\hat{\mathbf{R}}{}^{I_{k}}_{G}\hat{\mathbf{R}}\begin{bmatrix}\bm{\Gamma}_{6}&\bm{\Gamma}_{7}&\bm{\Gamma}_{8}&\bm{\Gamma}_{9}\end{bmatrix}$
(142)

with:

$\displaystyle\bm{\Gamma}_{6}$
$\displaystyle=\bigg{(}\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\hat{\bm{\theta}}_{k}\right)\delta
t_{k}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{4}\bigg{)}\mathbf{H}_{Dw}$
$\displaystyle\bm{\Gamma}_{7}$
$\displaystyle=-\bigg{(}\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\hat{\bm{\theta}}_{k}\right){}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}\delta
t_{k}$
$\displaystyle~{}~{}~{}~{}~{}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\left(\bm{\Xi}_{4}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}+\bm{\Xi}_{2}\right)\bigg{)}{}^{I}_{a}\hat{\mathbf{R}}\mathbf{H}_{Da}$
$\displaystyle\bm{\Gamma}_{8}$
$\displaystyle=-\bigg{(}\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\hat{\bm{\theta}}_{k}\right){}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}\delta
t_{k}$
$\displaystyle~{}~{}~{}~{}~{}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\big{(}\bm{\Xi}_{4}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}+\bm{\Xi}_{2}\big{)}\bigg{)}\lfloor{}^{I_{k}}\hat{\mathbf{a}}\rfloor$
$\displaystyle\bm{\Gamma}_{9}$
$\displaystyle=-\bigg{(}\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\hat{\bm{\theta}}_{k}\right)\delta
t_{k}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{4}\bigg{)}\times$
$\displaystyle~{}~{}~{}~{}~{}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\mathbf{H}_{Tg}$

The $\mathbf{M}_{IC}$ can be computed as:

$\displaystyle\mathbf{M}_{IC}$
$\displaystyle=\mathbf{H}_{\mathbf{p}_{f}}{}^{C}_{I}\hat{\mathbf{R}}{}^{I_{k}}_{G}\hat{\mathbf{R}}\begin{bmatrix}\bm{\Gamma}_{10}&\bm{\Gamma}_{11}&\bm{\Gamma}_{12}&\bm{\Gamma}_{13}\end{bmatrix}$
(143) $\displaystyle\bm{\Gamma}_{10}$
$\displaystyle=\lfloor\left({}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\right)\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}{}^{I}_{C}\hat{\mathbf{R}}$
(144) $\displaystyle\bm{\Gamma}_{11}$
$\displaystyle={}^{G}_{I_{k}}\hat{\mathbf{R}}{}^{I}_{C}\hat{\mathbf{R}}$ (145)
$\displaystyle\bm{\Gamma}_{12}$
$\displaystyle=\lfloor\left({}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\right)\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}{}^{I_{k}}\hat{\bm{\omega}}-{}^{G}\hat{\mathbf{v}}_{I_{k}}$
(146) $\displaystyle\bm{\Gamma}_{13}$
$\displaystyle=\frac{m}{M}\bm{\Gamma}_{12}$ (147)

The $\mathbf{M}_{Cin}$ and $\mathbf{M}_{f}$ can be written as:

$\displaystyle\mathbf{M}_{Cin}=\mathbf{H}_{Cin}$ (148)
$\displaystyle\mathbf{M}_{f}=\mathbf{H}_{\mathbf{p}_{f}}{}^{C}_{I}\hat{\mathbf{R}}{}^{I_{k}}_{G}\hat{\mathbf{R}}$
(149)

##  18 Appendix D: Proof of Lemma 1

For Eq. (79), we first verify $\mathcal{O}\mathbf{N}=\mathbf{0}$ as:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\left(\bm{\Gamma}_{1}{}^{I_{1}}_{G}\hat{\mathbf{R}}-\bm{\Gamma}_{2}\lfloor{}^{G}\hat{\mathbf{p}}_{I_{1}}\rfloor-\bm{\Gamma}_{3}\lfloor{}^{G}\hat{\mathbf{v}}_{I_{1}}\rfloor-\lfloor{}^{G}\hat{\mathbf{p}}_{f}\rfloor\right){}^{G}\mathbf{g}=0$
$\displaystyle\Leftrightarrow$
$\displaystyle\left(\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{1}}-{}^{G}\hat{\mathbf{v}}_{I_{1}}\delta
t_{k}+\frac{1}{2}{}^{G}\mathbf{g}\delta
t^{2}_{k}\rfloor\right){}^{G}\mathbf{g}$
$\displaystyle+\left(\lfloor{}^{G}\hat{\mathbf{p}}_{I_{1}}\rfloor+\lfloor{}^{G}\hat{\mathbf{v}}_{I_{1}}\rfloor\delta
t_{k}-\lfloor{}^{G}\hat{\mathbf{p}}_{f}\rfloor\right){}^{G}\mathbf{g}=\mathbf{0}$

Hence, we can conclude that the observability matrix $\mathcal{O}$ has at
least 4 unobservable directions.

In the following, we will try to show that there are only 4 unobservable
directions under general situations. With abusing of notion, we can rewrite
the observability matrix as:

$\displaystyle\mathcal{O}$
$\displaystyle=\begin{bmatrix}\mathcal{O}^{\top}_{1}&\dots&\mathcal{O}^{\top}_{k}\end{bmatrix}^{\top}$
$\displaystyle=\begin{bmatrix}\mathbf{M}_{n,1}&\mathbf{M}_{b,1}&\mathbf{M}_{in,1}&\mathbf{M}_{IC,1}&\mathbf{M}_{Cin,1}&\mathbf{M}_{f,1}\\\
\vdots&\vdots&\vdots&\vdots&\vdots&\vdots\\\
\mathbf{M}_{n,k}&\mathbf{M}_{b,k}&\mathbf{M}_{in,k}&\mathbf{M}_{IC,k}&\mathbf{M}_{Cin,k}&\mathbf{M}_{f,k}\end{bmatrix}$

By adjusting the column of $\mathbf{M}_{f}$ in $\mathcal{O}$, we can get
$\mathcal{O}^{\prime}$:

$\displaystyle\mathcal{O}^{\prime}$
$\displaystyle\triangleq\left[\begin{array}[]{c|c|c|c
}\mathcal{O}_{I}&\mathcal{O}_{in}&\mathcal{O}_{IC}&\mathcal{O}_{f}\end{array}\right]$
(151) $\displaystyle\triangleq\left[\begin{array}[]{ccc|c|c|c
}\mathbf{M}_{n,1}&\mathbf{M}_{b,1}&\mathbf{M}_{f,1}&\mathbf{M}_{in,1}&\mathbf{M}_{IC,1}&\mathbf{M}_{Cin,1}\\\
\vdots&\vdots&\vdots&\vdots&\vdots&\vdots\\\
\mathbf{M}_{n,k}&\mathbf{M}_{b,k}&\mathbf{M}_{f,k}&\mathbf{M}_{in,k}&\mathbf{M}_{IC,k}&\mathbf{M}_{Cin,k}\end{array}\right]$
(155)

It is clear that the column rank of $\mathcal{O}^{\prime}$ is the same as
$\mathcal{O}$.

$\mathcal{O}_{I}$ corresponds to the IMU navigation state, IMU bias state and
feature state. $\mathcal{O}_{I}$ is equivalent to the standard VINS
observability matrix in (Hesch et al., 2013) and it has null space of 4DoF.

$\mathcal{O}_{in}$ corresponds to the IMU intrinsic parameters. By checking
the Eq. (142), it is clearly that $\mathcal{O}_{in}$ will be affected by time-
varying ${}^{w}\bm{\omega}(t)$ (in $\mathbf{H}_{Dw}$), ${}^{a}\mathbf{a}(t)$
(in $\mathbf{H}_{Da}$) and ${}^{I}\mathbf{a}(t)$ (in
$\lfloor{}^{I}\mathbf{a}\rfloor$ and $\mathbf{H}_{Tg}$). Under generate
motions, $\mathcal{O}_{in}$ can be of full column rank.

$\mathcal{O}_{IC}$ corresponds to the IMU-camera spatial and temporal
calibration parameters. By checking the Eq. (143), we can see that the
$\mathcal{O}_{IC}$ is affected by the time-varying IMU pose
$\\{{}^{I}_{G}\mathbf{R}(t),{}^{G}\mathbf{p}_{I}(t)\\}$ and the IMU kinematics
$\\{{}^{I}\bm{\omega}(t),{}^{I}\mathbf{v}(t)\\}$. In addition, $\Gamma_{13}$
in $\mathbf{M}_{IC}$ are also affected by the point feature measurement
through $\frac{m}{M}$, of which $m$ will change under general measurement
assumptions. Hence, $\mathbf{O}_{IC}$ can be of full column rank with random
motions.

$\mathcal{O}_{Cin}$ corresponds to the camera intrinsic parameters. It is
clear that $\mathcal{O}_{Cin}$ is only affected the environmental structure
and is of full column rank as long as $\\{u_{n},v_{n}\\}$ varies in different
image tracks.

Since $\mathbf{O}_{in}$, $\mathbf{O}_{IC}$ and $\mathbf{O}_{Cin}$ are affected
by different system parameters, and under general motion conditions,
$\left[\mathbf{O}_{in}~{}~{}\mathbf{O}_{IC}~{}~{}\mathbf{O}_{Cin}\right]$ is
also of full column rank. Therefore, the column rank of $\mathcal{O}^{\prime}$
is determined by $\mathbf{O}_{I}$. Since $\mathbf{O}_{I}$ has 4 DoF null
space, the $\mathcal{O}^{\prime}$ also has 4 DoF. Hence, we can conclude that
$\mathcal{O}$ only has 4 DoF null space. We also verify this conclusion
through simulation results shown in Fig 4.

##  19 Appendix E: Null Space Proofs

In this section, we provide the verification for the null spaces listed in
this paper.

###  19.1 Verification of Lemma 2

For $\mathbf{N}_{w1}$, we have:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{w1}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{4}\hat{\mathbf{D}}^{-1}_{w}{}^{I}_{w}\hat{\mathbf{R}}^{\top}\mathbf{e}_{1}{}^{w}\omega_{1}+\bm{\Gamma}_{6}\times\begin{bmatrix}1~{}0~{}0~{}0~{}0~{}0\end{bmatrix}^{\top}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle(\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\bm{\theta}_{k}\right)\delta
t_{k}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{4})\times$
$\displaystyle\left(-\mathbf{e}_{1}{}^{w}\omega_{1}+\mathbf{e}_{1}{}^{w}\omega_{1}\right)=\mathbf{0}$

For $\mathbf{N}_{w2}$, we have:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{w2}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{4}\hat{\mathbf{D}}^{-1}_{w}{}^{I}_{w}\hat{\mathbf{R}}^{\top}\left[\mathbf{e}_{1}~{}\mathbf{e}_{2}\right]{}^{w}\omega_{2}+\bm{\Gamma}_{6}\times\begin{bmatrix}0~{}1~{}0~{}0~{}0~{}0\\\
0~{}0~{}1~{}0~{}0~{}0\end{bmatrix}^{\top}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle(\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\bm{\theta}_{k}\right)\delta
t_{k}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{4})\times$
$\displaystyle\left(-\left[\mathbf{e}_{1}~{}\mathbf{e}_{2}\right]{}^{w}\omega_{2}+\left[\mathbf{e}_{1}~{}\mathbf{e}_{2}\right]{}^{w}\omega_{2}\right)=\mathbf{0}$

For $\mathbf{N}_{w3}$, we have:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{w3}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{4}\hat{\mathbf{D}}^{-1}_{w}{}^{I}_{w}\hat{\mathbf{R}}^{\top}\mathbf{I}_{3}{}^{w}\omega_{3}+\bm{\Gamma}_{6}\times\begin{bmatrix}\mathbf{0}_{3}~{}~{}\mathbf{I}_{3}\end{bmatrix}^{\top}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle(\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\bm{\theta}_{k}\right)\delta
t_{k}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{4})\times$
$\displaystyle\left(-\mathbf{I}_{3}{}^{w}\omega_{3}+\mathbf{I}_{3}{}^{w}\omega_{3}\right)=\mathbf{0}$

###  19.2 Verification of Lemma 3

We first verify the first column of $\mathbf{N}_{a1}$:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{a1}\mathbf{e}_{1}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{5}\hat{\mathbf{D}}^{-1}_{a}\mathbf{e}_{1}{}^{a}a_{1}+\bm{\Gamma}_{7}\times[1~{}0~{}0~{}0~{}0~{}0]^{\top}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bigg{(}\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\bm{\theta}_{k}\right){}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}\delta
t_{k}$
$\displaystyle~{}~{}~{}~{}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\left(\bm{\Xi}_{4}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}+\bm{\Xi}_{2}\right)\bigg{)}{}^{I}_{a}\hat{\mathbf{R}}\times$
$\displaystyle\left(\mathbf{e}_{1}{}^{a}a_{1}-\mathbf{e}_{1}{}^{a}a_{1}\right)=\mathbf{0}$

We then verify the second column of $\mathbf{N}_{a1}$:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{a1}\mathbf{e}_{2}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{5}\hat{\mathbf{D}}^{-1}_{a}\mathbf{e}_{2}d_{a1}{}^{a}a_{1}+\bm{\Gamma}_{7}\times[0~{}d_{a3}~{}-d_{a2}~{}d_{a5}~{}-d_{a4}~{}0]^{\top}$
$\displaystyle-\bm{\Gamma}_{8}{}^{I}_{a}\hat{\mathbf{R}}\mathbf{e}_{3}=\mathbf{0}$
$\displaystyle\Leftrightarrow$ $\displaystyle\begin{bmatrix}0\\\
d_{a1}{}^{a}a_{1}\\\
0\end{bmatrix}-\begin{bmatrix}d_{a3}{}^{a}a_{2}+d_{a5}{}^{a}a_{3}\\\
-d_{a2}{}^{a}a_{2}-d_{a4}{}^{a}a_{3}\\\ 0\end{bmatrix}$
$\displaystyle+\begin{bmatrix}d_{a3}{}^{a}a_{2}+d_{a5}{}^{a}a_{3}\\\
-(d_{a1}{}^{a}a_{1}+d_{a2}{}^{a}a_{2}+d_{a4}{}^{a}a_{3})\\\
0\end{bmatrix}=\mathbf{0}$

The third column of $\mathbf{N}_{a1}$ can be verified as:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{a1}\mathbf{e}_{3}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{5}\hat{\mathbf{D}}^{-1}_{a}\mathbf{e}_{3}d_{a1}d_{a3}{}^{a}a_{1}+\bm{\Gamma}_{8}{}^{I}_{a}\mathbf{R}(\mathbf{e}_{1}d_{a2}+\mathbf{e}_{2}d_{a3})+$
$\displaystyle\bm{\Gamma}_{7}\times[0~{}0~{}0~{}d_{a6}d_{a3}~{}~{}-d_{a2}d_{a6}~{}~{}d_{a2}d_{a5}-d_{a4}d_{a3}]=\mathbf{0}$
$\displaystyle\Leftrightarrow$ $\displaystyle\begin{bmatrix}0\\\ 0\\\
d_{a1}d_{a3}{}^{a}a_{1}\end{bmatrix}-\begin{bmatrix}d_{a6}d_{a3}{}^{a}a_{3}\\\
-d_{a2}d_{a6}{}^{a}a_{3}\\\
d_{a2}d_{a5}{}^{a}a_{3}-d_{a4}d_{a3}{}^{a}a_{3}\end{bmatrix}$
$\displaystyle-\begin{bmatrix}-d_{a6}d_{a3}{}^{a}a_{3}\\\
d_{a2}d_{a6}{}^{a}a_{3}\\\
d_{a1}d_{a3}{}^{a}a_{1}-d_{a5}d_{a2}{}^{a}a_{3}+d_{a4}d_{a3}{}^{a}a_{3}\end{bmatrix}=\mathbf{0}$

The first two columns of $\mathbf{N}_{a2}$ can be verified as:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{a2}[\mathbf{e}_{1}~{}\mathbf{e}_{2}]=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{5}\hat{\mathbf{D}}^{-1}_{a}[\mathbf{e}_{1}~{}\mathbf{e}_{2}]{}^{a}a_{2}+\bm{\Gamma}_{7}\times\begin{bmatrix}0~{}1~{}0~{}0~{}0~{}0\\\
0~{}0~{}1~{}0~{}0~{}0\end{bmatrix}^{\top}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bigg{(}\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\bm{\theta}_{k}\right){}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}\delta
t_{k}$
$\displaystyle~{}~{}~{}~{}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\left(\bm{\Xi}_{4}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}+\bm{\Xi}_{2}\right)\bigg{)}{}^{I}_{a}\hat{\mathbf{R}}\times$
$\displaystyle\left([\mathbf{e}_{1}~{}\mathbf{e}_{2}]{}^{a}a_{2}-[\mathbf{e}_{1}~{}\mathbf{e}_{2}]{}^{a}a_{2}\right)=\mathbf{0}$

The third column of $\mathbf{N}_{a3}$ can be verified as:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{a2}\mathbf{e}_{1}d_{a3}{}^{a}a_{2}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{5}\hat{\mathbf{D}}^{-1}_{a}\mathbf{e}_{1}d_{a3}{}^{a}a_{2}+\bm{\Gamma}_{7}\times[0~{}0~{}0~{}0~{}d_{a6}~{}-d_{a5}]^{\top}$
$\displaystyle-\bm{\Gamma}_{8}{}^{I}_{a}\mathbf{R}\mathbf{e}_{1}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\scalebox{0.85}{\mbox{$\displaystyle\begin{bmatrix}0\\\ 0\\\
d_{a3}{}^{a}a_{2}\end{bmatrix}-\begin{bmatrix}0\\\ d_{a6}{}^{a}a_{3}\\\
-d_{a5}{}^{a}a_{3}\end{bmatrix}+\begin{bmatrix}0\\\ d_{a6}{}^{a}a_{3}\\\
-(d_{a3}{}^{a}a_{2}+d_{a5}{}^{a}a_{3})\end{bmatrix}$}}=\mathbf{0}$

The verification of $\mathbf{N}_{a3}$ can be described as:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{a3}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{5}\hat{\mathbf{D}}^{-1}_{a}\mathbf{I}_{3}{}^{a}a_{3}+\bm{\Gamma}_{7}\times[\mathbf{0}_{3}~{}~{}\mathbf{I}_{3}]^{\top}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bigg{(}\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\bm{\theta}_{k}\right){}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}\delta
t_{k}$
$\displaystyle~{}~{}~{}~{}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\left(\bm{\Xi}_{4}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\hat{\mathbf{T}}_{g}+\bm{\Xi}_{2}\right)\bigg{)}{}^{I}_{a}\hat{\mathbf{R}}\times$
$\displaystyle\left(\mathbf{I}_{3}{}^{a}a_{3}-\mathbf{I}_{3}{}^{a}a_{3}\right)=\mathbf{0}$

###  19.3 Verification of Lemma 4

The $\mathbf{N}_{g1}$ can be verified as:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{g1}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{4}\mathbf{I}_{3}{}^{I}a_{1}-\bm{\Gamma}_{9}\times[\mathbf{I}_{3}~{}\mathbf{0}_{3}~{}\mathbf{0}_{3}]^{\top}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle-(\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\bm{\theta}_{k}\right)\delta
t_{k}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{4})\times$
$\displaystyle~{}~{}~{}~{}~{}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\left(\mathbf{I}_{3}{}^{I}a_{1}-\mathbf{I}_{3}{}^{I}a_{1}\right)=\mathbf{0}$

The $\mathbf{N}_{g2}$ can be verified as:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{g2}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{4}\mathbf{I}_{3}{}^{I}a_{2}-\bm{\Gamma}_{9}\times[\mathbf{0}_{3}~{}\mathbf{I}_{3}~{}\mathbf{0}_{3}]^{\top}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle-(\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\bm{\theta}_{k}\right)\delta
t_{k}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{4})\times$
$\displaystyle~{}~{}~{}~{}~{}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\left(\mathbf{I}_{3}{}^{I}a_{2}-\mathbf{I}_{3}{}^{I}a_{2}\right)=\mathbf{0}$

The $\mathbf{N}_{g3}$ can be verified as:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{g3}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\bm{\Gamma}_{4}\mathbf{I}_{3}{}^{I}a_{3}-\bm{\Gamma}_{9}\times[\mathbf{0}_{3}~{}\mathbf{0}_{3}~{}\mathbf{I}_{3}]^{\top}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle-(\lfloor{}^{G}\hat{\mathbf{p}}_{f}-{}^{G}\hat{\mathbf{p}}_{I_{k}}\rfloor{}^{G}_{I_{k}}\hat{\mathbf{R}}\mathbf{J}_{r}\left(\Delta\bm{\theta}_{k}\right)\delta
t_{k}+{}^{G}_{I_{k}}\hat{\mathbf{R}}\bm{\Xi}_{4})\times$
$\displaystyle~{}~{}~{}~{}~{}{}^{I}_{w}\hat{\mathbf{R}}\hat{\mathbf{D}}_{w}\left(\mathbf{I}_{3}{}^{I}a_{3}-\mathbf{I}_{3}{}^{I}a_{3}\right)=\mathbf{0}$

###  19.4 Verification of Lemma 6

Note that:

$\displaystyle\mathbf{M}_{Cin}\times[f_{u}~{}f_{v}~{}0~{}0~{}2k_{1}~{}4k_{2}~{}p_{1}~{}p_{2}]^{\top}$
$\displaystyle=\scalebox{0.85}{\mbox{$\displaystyle\begin{bmatrix}f_{u}\left(u_{d}+2k_{1}u_{n}r^{2}+4k_{2}u_{n}r^{4}+2p_{1}u_{n}v_{n}+p_{2}(r^{2}+2u^{2}_{n})\right)\\\
f_{v}\left(v_{d}+2k_{1}v_{n}r^{2}+4k_{2}v_{n}r^{4}+p_{1}(r^{2}+2u^{2}_{n})+2p_{2}u_{n}v_{n}\right)\end{bmatrix}$}}$
$\displaystyle=\scalebox{0.85}{\mbox{$\displaystyle\begin{bmatrix}f_{u}\left(u_{n}+3k_{1}u_{n}r^{2}+5k_{2}u_{n}r^{4}+4p_{1}u_{n}v_{n}+2p_{2}(r^{2}+2u^{2}_{n})\right)\\\
f_{v}\left(v_{n}+3k_{1}v_{n}r^{2}+5k_{2}v_{n}r^{4}+2p_{1}(r^{2}+2u^{2}_{n})+4p_{2}u_{n}v_{n}\right)\end{bmatrix}$}}$

At the same time, with one-axis rotation assumption, we have:

$\displaystyle\mathbf{M}_{f}{}^{G}\mathbf{k}$
$\displaystyle=\mathbf{H}_{\mathbf{p}_{f}}{}^{C}_{I}\hat{\mathbf{R}}{}^{I_{k}}_{G}\hat{\mathbf{R}}\cdot{}^{G}_{I_{1}}\hat{\mathbf{R}}{}^{I}_{C}\hat{\mathbf{R}}{}^{C}\mathbf{k}{}^{C}{z}_{f}$
$\displaystyle=\mathbf{H}_{\mathbf{p}_{f}}\mathbf{e}_{3}{}^{C}z_{f}$
$\displaystyle=\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial\tilde{\mathbf{z}}_{n}}\begin{bmatrix}1&0&-u_{n}\\\
0&1&-v_{n}\end{bmatrix}\mathbf{e}_{3}$
$\displaystyle=-\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial\tilde{\mathbf{z}}_{n}}\begin{bmatrix}u_{n}\\\
v_{n}\end{bmatrix}$

Note that
${}^{I_{k}}_{G}\hat{\mathbf{R}}={}^{I_{k}}_{I_{1}}\hat{\mathbf{R}}{}^{I_{1}}_{G}\hat{\mathbf{R}}$
and ${}^{I_{k}}_{I_{1}}\hat{\mathbf{R}}{}^{I}\mathbf{k}={}^{I}\mathbf{k}$ (due
to one-axis rotation). We can easily verify that:

$\displaystyle\frac{\partial\tilde{\mathbf{z}}_{C}}{\partial\tilde{\mathbf{z}}_{n}}\begin{bmatrix}u_{n}\\\
v_{n}\end{bmatrix}=$

Therefore, the verification of $\mathbf{N}_{Cin}$ can be written as:

$\displaystyle\Leftrightarrow$
$\displaystyle\mathcal{O}_{k}\mathbf{N}_{Cin}=\mathbf{0}$
$\displaystyle\Leftrightarrow$
$\displaystyle\mathbf{M}_{Cin}\times[f_{u}~{}f_{v}~{}0~{}0~{}2k_{1}~{}4k_{2}~{}p_{1}~{}p_{2}]^{\top}+\mathbf{M}_{f}{}^{G}\mathbf{k}=\mathbf{0}$

##  20 Appendix F: Interpolation Jacobians

We perturb ${}^{G}_{I_{ci-1}}\mathbf{R}$ and ${}^{G}_{I_{ci}}\mathbf{R}$ as:

$\displaystyle{}^{G}_{I_{ci-1}}\mathbf{R}$
$\displaystyle={}^{G}_{I_{ci-1}}\hat{\mathbf{R}}\exp\left(\delta\bm{\theta}_{I_{ci-1}}\right)$
(156) $\displaystyle{}^{G}_{I_{ci}}\mathbf{R}$
$\displaystyle={}^{G}_{I_{ci}}\hat{\mathbf{R}}\exp\left(\delta\bm{\theta}_{I_{ci}}\right)$
(157)

By representing
$\bm{\theta}_{i-1,i}=\log\left({}^{G}_{I_{ci-1}}\mathbf{R}^{\top}{}^{G}_{I_{ci}}\mathbf{R}\right)$,
we have the linearization for the interpolation as:

$\displaystyle\delta\bm{\theta}_{I(t)}$
$\displaystyle\simeq\scalebox{0.95}{\mbox{$\displaystyle\left(\exp(-\hat{\lambda}\hat{\bm{\theta}}_{i-1,i})-\mathbf{J}_{r}(\hat{\lambda}\hat{\bm{\theta}}_{i-1,i})\mathbf{J}^{-1}_{l}(\hat{\bm{\theta}}_{i-1,i})\right)\delta\bm{\theta}_{I_{ci-1}}$}}$
$\displaystyle+\mathbf{J}_{r}(\hat{\lambda}\hat{\bm{\theta}}_{i-1,i})\mathbf{J}^{-1}_{r}(\hat{\bm{\theta}}_{i-1,i})\delta\bm{\theta}_{I_{ci}}+\frac{m}{M}\bm{\omega}_{i-1,i}\tilde{t}_{r}$
$\displaystyle{}^{G}\tilde{\mathbf{p}}_{I(t)}$
$\displaystyle\simeq(1-\hat{\lambda}){}^{G}\tilde{\mathbf{p}}_{I_{ci-1}}+\hat{\lambda}{}^{G}\tilde{\mathbf{p}}_{I_{ci}}+\scalebox{0.95}{\mbox{$\displaystyle\frac{m}{M}\mathbf{v}_{i-1,i}\tilde{t}_{r}$}}$

where:

$\displaystyle\hat{\bm{\omega}}_{i-1,i}$
$\displaystyle=\frac{\hat{\bm{\theta}}_{i-1,i}}{t_{ci}-t_{ci-1}}$ (158)
$\displaystyle\hat{\mathbf{v}}_{i-1,i}$
$\displaystyle=\frac{{}^{G}\hat{\mathbf{p}}_{I_{ci}}-{}^{G}\hat{\mathbf{p}}_{I_{ci-1}}}{t_{ci}-t_{ci-1}}$
(159)

##  21 Appendix G: Degenerate Motion Simulation Results

The complete calibration plots for IMU/camera intrinsic and IMU-camera spatial
calibration under one-axis rotation, constant $a_{x}$ acceleration and planar
motion are shown in Figure 25, 26 and 27. All the temporal calibration results
for IMU-camera time offset and rolling shutter readout time on the four
simulated trajectories are shown in Figure 9.

Figure 25:  Calibration results for the proposed system evaluated on tum_room
with one-axis rotation using imu22 and radtan. 3 sigma bounds (dotted lines)
and estimation errors (solid lines) for six different runs (different colors)
with different realization of the measurement noise and initial perturbations.
Note that the estimation errors and 3 $\sigma$ bounds for $d_{w1}$, $d_{w2}$,
$d_{w3}$ and the IMU-CAM position calibration along the rotation axis can not
converge.

Figure 26:  Calibration results for the proposed system evaluated the sine_3d
with constant acceleration along x direction using imu22 and radtan. 3 sigma
bounds (dotted lines) and estimation errors (solid lines) for six different
runs (different colors) with different realization of the measurement noise
and initial perturbations. The estimation errors and 3$\sigma$ bounds for
$d_{a1}$, pitch and yaw of ${}^{I}_{a}\mathbf{R}$ cannot converge. Note that
$t_{g1}$, $t_{g2}$ and $t_{g3}$ are also unobservable.

Figure 27:  Calibration results of the proposed system evaluated on udel_gore
with planar motion using imu22 and radtan. 3 sigma bounds (dotted lines) and
estimation errors (solid lines) for six different runs (different colors) with
different realization of the measurement noise and initial perturbations. With
planar motion, the estimation errors and 3 $\sigma$ bounds of $d_{w1}$,
$d_{w2}$, $d_{w3}$, $t_{g7}$, $t_{g8}$, $t_{g9}$ and the IMU-CAM position
cannot converge. Due to lack of motion excitation, the parameters of
$\mathbf{D}_{a}$ and ${}^{I}_{a}\mathbf{R}$ converge much slower than the
other motion cases.

This work was partially supported by the University of Delaware (UD) College
of Engineering, the NSF (IIS-1924897), and Google ARCore. Geneva was partially
supported by the University Doctoral Fellowship. Thanks Dr. Kun Fu for 3D
printing of the VI-Rig. Thanks Dr. Kevin Eckenhoff for the rolling shutter
implementation from MIMC-VINS project (Eckenhoff et al., 2021). Thanks David
Schubert and Nikolaus Demmel from TUM for the helpful discussion on rolling-
shutter visual-inertial datasets (Schubert et al., 2019).