Skip to main content

Contact Inertial Odometry: Collisions are your Friends

Part of the Springer Proceedings in Advanced Robotics book series (SPAR,volume 20)


Autonomous exploration of unknown environments with aerial vehicles remains a challenge, especially in perceptually degraded conditions. Dust, fog, or a lack of visual or LiDAR-based features results in severe difficulties for state estimation algorithms, which failure can be catastrophic. In this work, we show that it is indeed possible to navigate in such conditions without any exteroceptive sensing by exploiting collisions instead of treating them as constraints. To this end, we present a novel contact-based inertial odometry (CIO) algorithm: it uses estimated external forces with the environment to detect collisions and generate pseudo-measurements of the robot velocity, enabling autonomous flight. To fully exploit this method, we first perform modeling of a hybrid ground and aerial vehicle which can withstand collisions at moderate speeds, for which we develop an external wrench estimation algorithm. Then, we present our CIO algorithm and develop a reactive planner and control law which encourage exploration by bouncing off obstacles. All components of this framework are validated in hardware experiments and we demonstrate that a quadrotor can traverse a cluttered environment using an IMU only. This work can be used on drones to recover from visual inertial odometry failure or on micro-drones that do not have the payload capacity to carry cameras, LiDARs or powerful computers.

T. Lew and T. Emmei—Both authors contributed equally to this manuscript.

Video: Experimental results are available at

This is a preview of subscription content, access via your institution.

Buying options

USD   29.95
Price excludes VAT (USA)
  • Available as PDF
  • Read on any device
  • Instant download
  • Own it forever
USD   169.00
Price excludes VAT (USA)
  • Available as EPUB and PDF
  • Read on any device
  • Instant download
  • Own it forever
Softcover Book
USD   219.99
Price excludes VAT (USA)
  • Compact, lightweight edition
  • Dispatched in 3 to 5 business days
  • Free shipping worldwide - see info
Hardcover Book
USD   219.99
Price excludes VAT (USA)
  • Durable hardcover edition
  • Dispatched in 3 to 5 business days
  • Free shipping worldwide - see info

Tax calculation will be finalised at checkout

Purchases are for personal use only

Learn about institutional subscriptions


  1. 1.

    The appendix is available at


  1. Santamaria-Navarro, A., Loianno, G., Andrade-Cetto, J.S.V.K.: Autonomous navigation of micro aerial vehicles using high-rate and low-cost sensors. Auton. Robots 42(6) 1263–1280 (2018)

    Google Scholar 

  2. Abeywardena, D., Kodagoda, S., Dissanayake, G., Munasinghe, R.: Improved state estimation in Quadrotor MAVs: a novel drift-free velocity estimator. IEEE Robot. Autom. Magaz. 20(4), 32–39 (2013)

    Google Scholar 

  3. Agha-Mohammadi, A., Agarwal, S., Kim, S.K., Chakravorty, S., Amato, N.: SLAP: simultaneous localization and planning under uncertainty via dynamic replanning in belief space. IEEE Trans. Robot. 34, 1195–1214 (2018)

    Google Scholar 

  4. Agha-Mohammadi, A., Chakravorty, S., Amato, N.: FIRM: Sampling-based feedback motion-planning under motion uncertainty and imperfect measurements. Int. J. Robot. Res. 33(2), 268–304 (2014)

    Google Scholar 

  5. Barrau, A., Bonnabel, S.: The invariant extended kalman filter as a stable observer. IEEE Trans. Autom. Control 62(4), 1797–1812 (2017)

    Google Scholar 

  6. Briod, A., et al.: Contact-based navigation for an autonomous flying robot. In: IEEE International Conference on Intelligent Robots and Systems, pp. 3987–3992 (2013)

    Google Scholar 

  7. Dissanayake, G., Sukkarieh, S., Nebot, E., Durrant-Whyte, H.: The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications. IEEE Trans. Robot. Automation 17(5), 731–747 (2001)

    Google Scholar 

  8. Fan, D.D., Thakker, R., Bartlett, T., Miled, M.B., Kim, L., Theodorou, E., Agha-Mohammadi, A.: Autonomous hybrid ground/aerial mobility in unknown environments. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (2019)

    Google Scholar 

  9. Gryazina, E., Polyak, B.: Random sampling: billiard walk algorithm. European J. Oper. Res (2014)

    Google Scholar 

  10. Kalantari, A., Spenko, M.: Hybrid aerial and terrestrial vehicle (2015)

    Google Scholar 

  11. Kalantari, A., et al.: Drivocopter: a concept hybrid aerial/ground vehicle for long-endurance mobility. In: IEEE Aerospace Conference (2020)

    Google Scholar 

  12. Khedekar, N., Mascarich, F., Papachristos, C., Dang, T., Alexis, K.: Contact-based navigation path planning for aerial robots. In: IEEE International Conference on Robotics and Automation (2019)

    Google Scholar 

  13. Koval, M.C., Pollard, N.S., Srinivasa, S.S.: Manifold representations for state estimation in contact manipulation. In: Inaba, M., Corke, P. (eds.) Robotics Research. Springer Tracts in Advanced Robotics, vol. 114. Springer, Cham (2016).

  14. Kuindersma, S., et al.: Optimization-based locomotion planning, estimation, and control design for the atlas humanoid robot. Auton. Robots 40(3), 429–455 (2016)

    Google Scholar 

  15. Lee, T., Leok, M., McClamroch, N.H.: Geometric tracking control of a quadrotor uav on se (3). In: IEEE Conference on Decision and Control (2010)

    Google Scholar 

  16. Moore, T., Stouch, D.: A generalized extended kalman filter implementation for the robot operating system. In: International Conference on Intelligent Autonomous Systems (2014)

    Google Scholar 

  17. Mur-Artal, R., Montiel, J.M.M., Tardos, J.D.: ORB-SLAM: a versatile and accurate monocular SLAM system. IEEE Trans. Robotics 31(5) (2015)

    Google Scholar 

  18. Nisar, B., Foehn, P., Falanga, D., Scaramuzza, D.: VIMO: simultaneous visual inertial model-based odometry and force estimation. IEEE Robot. Automation Lett. (2019)

    Google Scholar 

  19. Oh, S., Kong, K.: High-precision robust force control of a series elastic actuator. IEEE/ASME Trans. Mech. 22(1), 71–80 (2017)

    Google Scholar 

  20. Paden, B., Cáp, M., Yong, S.Z., Yershov, D.S., Frazzoli, E.: A survey of motion planning and control techniques for self-driving urban vehicles. IEEE Trans. Intell. Vehicles 1, 33–55 (2016)

    Google Scholar 

  21. Petrovskaya, A., Khatib, O.: Global localization of objects via touch. IEEE Trans. Robot. 27(3), 569–585 (2011)

    Google Scholar 

  22. Richter, C., Bry, A., Roy, N.: Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments. In: Inaba, M., Corke, P. (eds.) Robotics Research. Springer Tracts in Advanced Robotics, vol. 114. Springer, Cham (2016).

  23. Roithmayr, C.M., Hodges, D.H.: Dynamics: theory and Application of Kane’s method. J. Comput. Nonlinear Dynam. 11(6) (2016)

    Google Scholar 

  24. Sabet, S., Agha-Mohammadi, A., Tagliabue, A., Elliott, D.S., Nikravesh, P.E.: Rollocopter: an energy-aware hybrid aerial-ground mobility for extreme terrains. In: IEEE Aerospace Conference (2019)

    Google Scholar 

  25. Svacha, J., Loianno, G., Kumar, V.: Inertial yaw-independent velocity and attitude estimation for high-speed quadrotor flight. IEEE Robot. Automation Lett. 4(2) (2019)

    Google Scholar 

  26. Tomic, T., Ott, C., Haddadin, S.: External wrench estimation, collision detection, and reflex reaction for flying robots. IEEE Trans. Robot. 33(6), 1–17 (2017)

    Google Scholar 

  27. Wagstaff, B., Kelly, J.: LSTM-based zero-velocity detection for robust inertial navigation. In: International Conference on Indoor Positioning and Indoor Navigation (2018)

    Google Scholar 

Download references


This research was carried out at the Jet Propulsion Laboratory, California Institute of Technology, under a contract with the National Aeronautics and Space Administration. T. Lew is partially supported by the 242 Program of the Hubert Tuor Foundation. Tomoki Emmei is supported by JSPS KAKENHI Grant Number 18J14169. The authors thank Dr. Matthew J. Anderson, Leon Kim and members of the CoSTAR team for their incredible support with experiments and hardware.

Author information

Authors and Affiliations


Corresponding author

Correspondence to Thomas Lew .

Editor information

Editors and Affiliations

1 Electronic supplementary material

Below is the link to the electronic supplementary material.

Supplementary material 1 (zip 9011 KB)


A Contact Position Estimation

1.1 A.1 Analytical Solution

This section presents the analytical solution of the contact point estimation method presented in Sect. 2.2. For conciseness, we denote \(\tilde{F}_e^z:=(F_e^z-m_t g)\). The positions of the estimated contact forces on the left and right wheels are decomposed into two sets of equations depending on whether \(F_y<0\) or \(F_y\ge 0\).

First, if \(F_y < 0\), the solution for the position of the contact point on the left wheel is given as

$$\begin{aligned} p^l_{x}&=\, \frac{-a_1c_1 -\mathrm {sgn}(F_e^x)|b_1|\sqrt{R^2(a_1^2+b_1^2)-c_1^2}}{a_1^2+b_1^2}\end{aligned}$$
$$\begin{aligned} p^l_{z}&=\, \frac{-a_1c_1 - \mathrm {sgn}(\tilde{F}_e^z)|a_1|\sqrt{R^2(a_1^2+b_1^2) -c^2}}{a_1^2+b_1^2}, \end{aligned}$$

with \(a_1 = M_{e}^x + L\tilde{F}_e^z\), \(b_1 = M_{e}^z - LF_{e}^x\) and \(c_1 = 2LM^l_w\). Using these equations, the estimated contact point on the right wheel is derived as

$$\begin{aligned} p^r_{x}&=\, \frac{-A_1C_1 - \mathrm {sgn}(F_e^x)|B_1|\sqrt{r^2(A_1^2+B_1^2) -C_1^2}}{A_1^2+B_1^2}\end{aligned}$$
$$\begin{aligned} p^r_{z}&=\, \frac{-A_1C_1 - \mathrm {sgn}(\tilde{F}_e^z)|A_1|\sqrt{R^2(A_1^2+B_1^2)-C_1^2}}{A_1^2+B_1^2}, \end{aligned}$$

with \(A_1 = (- L^2\tilde{F}_e^{z2} + F_{e}^yp^l_zL\tilde{F}_e^z + M_{e}^{x2} + F_{e}^yp^l_zM_{e}^x)\), \(B_1 = M_{e}^xM_{e}^z + L^2F_{e}^x\tilde{F}_e^z + LM_{e}^xF_{e}^x + LM_{e}^z\tilde{F}_e^z + 2LM_\mathrm{w}^lF_{e}^y + M_{e}^zF_{e}^yp^l_z - LF_{e}^xF_{e}^yp^l_z\) and \(C_1 = - 2LM_{e}^xM_\mathrm{w}^r - 2L^2M_\mathrm{w}^r\tilde{F}_e^z\).

On the other hand, if \(F_e^y \ge 0\), the position of the contact point on the left and right wheels are computed as

$$\begin{aligned} p^r_{x}&=\, \frac{-a_2c_2 -\mathrm {sgn}(F_e^x)|b_2|\sqrt{R^2(a_2^2+b_2^2)-c_2^2}}{a_2^2+b_2^2}\end{aligned}$$
$$\begin{aligned} p^r_{z}&=\, \frac{-a_2c_2 - \mathrm {sgn}(\tilde{F}_e^z)|a_2|\sqrt{R^2(a_2^2+b_2^2) -c^2}}{a^2+b^2}\end{aligned}$$
$$\begin{aligned} p^l_{x}&=\, \frac{-A_2C_2 - \mathrm {sgn}(F_e^x)|B_2|\sqrt{R^2(A_2^2+B_2^2) -C_2^2}}{A_2^2+B_2^2}\end{aligned}$$
$$\begin{aligned} p^l_{z}&=\, \frac{-A_2C_2 - \mathrm {sgn}(\tilde{F}_e^z)|A_2|\sqrt{R^2(A_2^2+B_2^2)-C_2^2}}{A_2^2+B_2^2}, \end{aligned}$$

with \(a_2 = M_{e}^x - L\tilde{F}_e^z\), \(b_2 = M_{e}^z + LF_{e}^x\), \(c_2 = -2LM^r_w\), \(A_2 = (M_{e}^x - L\tilde{F}_e^z)(M_{e}^x + L\tilde{F}_e^z + F_{e}^yp^r_z)\), \(B_2 = M_{e}^xM_{e}^z + L^2F_{e}^x\tilde{F}_e^z - LM_{e}^xF_{e}^x - LM_{e}^z\tilde{F}_e^z - 2LM_\mathrm{w}^rF_{e}^y + M_{e}^zF_{e}^yp^r_z + LF_{e}^xF_{e}^yp^r_z\) and \(C_2 = 2LM_{e}^xM_\mathrm{w}^l - 2L^2M_\mathrm{w}^l\tilde{F}_e^z\) .

1.2 A.2 Results: Contact Point Estimation

Experiments are conducted to validate our proposed contact point estimation method. The results are shown in Fig. 12.

Fig. 12.
figure 12

Estimated contact point positions in rolling mode, with ground truth indicated by straight lines. The height of the contact point (\(\mathrm {p_z^l},\mathrm {p_z^r}\)) is negative since the origin is set to the center of the robot wheels. Each data point corresponds to a different experiment.

In each experiment, the hybrid vehicle Rollocopter is driven on a flat ground and collided frontally with a fixed box-shaped obstacle with a height of 15cm. From the known dimensions of the box and of the wheels, it is possible to determine the true contact point position. As shown in Fig. 12, the estimated positions \(p^i_x\) and \(p^i_z\) are close to the their true value for each one of the 6 experiments, validating our approach. This method could be used in future work to determine whether the contact point is caused by a collision in front of or behind the vehicle, or if it is detected due to rough terrain, providing additional useful information, e.g. for path planning.

B Derivation of Nonholonomic Model

This section derives additional nonholonomic constraints for the rolling mode of the hybrid vehicle.

1.1 B.1 Derivation of Nonholonomic Constraint

To derive (5), we first express the velocities at the contact points on the ground as

$$\begin{aligned} \boldsymbol{v_c^l}&= \begin{pmatrix} v_x \\ v_y \\ 0 \end{pmatrix} {\,{+}\,}\begin{pmatrix} 0\\ 0\\ \omega _z \end{pmatrix} {{\,\times \,}} \begin{pmatrix} 0 \\ L \\ 0 \end{pmatrix} {\,{+}\,}\begin{pmatrix} 0\\ \omega _y {+} \gamma _l\\ 0 \end{pmatrix} {{\,\times \,}} \begin{pmatrix} 0 \\ 0 \\ -R \end{pmatrix} {=} \begin{pmatrix} v_x {-} L\omega _z {-} R(\omega _y{+}\gamma _l)\\ v_y\\ 0 \end{pmatrix} \end{aligned}$$
$$\begin{aligned} \boldsymbol{v_c^r}&= \begin{pmatrix} v_x \\ v_y \\ 0 \end{pmatrix} {\,{+}\,}\begin{pmatrix} 0\\ 0\\ \omega _z \end{pmatrix} {{\,\times \,}} \begin{pmatrix} 0 \\ -L \\ 0 \end{pmatrix} {\,{+}\,}\begin{pmatrix} 0\\ \omega _y {\,{+}\,}\gamma _r\\ 0 \end{pmatrix} {{\,\times \,}} \begin{pmatrix} 0 \\ 0 \\ -R \end{pmatrix} {=} \begin{pmatrix} v_x {+} L\omega _z {-} R(\omega _y{+} \gamma _l)\\ v_y\\ 0 \end{pmatrix} . \end{aligned}$$

Assuming that the wheels of the hybrid vehicle remain in contact with the ground and that no slip occurs, As long as the wheels keep contact with the ground and no slip occurs, \(\mathbf {v}_c^l=\mathbf {v}_c^r = \mathbf {0}\) hold. Therefore, (19) can be equivalently expressed as

$$\begin{aligned} v_x = \frac{R}{2}(\gamma _r + \gamma _l+2\omega _y),\ \ v_y = 0,\ \ \omega _z = \frac{R}{2L}(\gamma _r - \gamma _l) , \end{aligned}$$

which is equivalent to (5).

1.2 B.2 Derivation of Force Estimation for Rolling Mode

In this section, we show how to take into account nonholonomic constraints in our force estimation method. First, the nonholonomic motion of the hybrid vehicle implies that

$$\begin{aligned} v_y,v_z,\dot{v}_y,\omega _x = 0 . \end{aligned}$$

Also, by including the left and right wheel rolling resistance forces \(F_d^l,F_d^r\) acting in the rolling direction, the dynamics of the hybrid vehicle in (3) are rewritten as

$$\begin{aligned} m_t\dot{v}_x&=F_{in}^x+F_{e}^x-F_{d}^l - F_{d}^r \end{aligned}$$
$$\begin{aligned} m_tv_x\omega _z&=F_{e}^y \end{aligned}$$
$$\begin{aligned} (I_t^z+2m_wL^2)\dot{\omega }_z&=(M_{in}^z+M_{e}^z-L(F_{d}^r-F_{d}^l)). \end{aligned}$$

Furthermore, by analyzing the dynamics of each wheel in Eq. (3c), we have

$$\begin{aligned} I_\mathrm{w}(\dot{\gamma }_i + \dot{\omega }_y)= RF_{d}^i. \end{aligned}$$

Therefore, using the nonholonomic constraint (5) to replace \(v_x,\dot{v}_x,\omega _z,\dot{\omega }_z\) and the previous result to replace \(F_d^i\), the equations above are rewritten as

$$\begin{aligned} \left( \frac{m_tR}{2} + \frac{2I_\mathrm{w}}{R}\right) (\dot{\gamma }_r+\dot{\gamma }_l+2\dot{\omega _y}) =F_{in}^x + F_{e}^x \end{aligned}$$
$$\begin{aligned} \frac{m_tR^2}{4L}(\gamma _r-\gamma _l)(\gamma _r+\gamma _l+2\omega _y)=F_{e}^y. \end{aligned}$$

Also, (22c) can be rewritten by replacing \(F_d^l,F_d^r,\dot{\omega }_z\) using Eqs. (23) and (5) as

$$\begin{aligned} \left( \frac{R}{2L}I_t^z+m_wLR+\frac{I_\mathrm{w}L}{R}\right) (\dot{\gamma }_r - \dot{\gamma }_l)= M_{in}^z+ M_{e}^z . \end{aligned}$$

Finally, the external wrench \(\{F_{e}^x ,F_{e}^y, M_{e}^z \}\) can be computed as

$$\begin{aligned} F_{e}^x&= \left( \frac{m_tR}{2} + \frac{2I_\mathrm{w}}{R}\right) (\dot{\gamma }_r + \dot{\gamma }_l +2\dot{\omega }_y) -F_{in}^x\end{aligned}$$
$$\begin{aligned} F_{e}^y&= \frac{m_tR^2}{4L}(\gamma _r-\gamma _l)(\gamma _r+\gamma _l+2\omega _y)\end{aligned}$$
$$\begin{aligned} M_{e}^z&= \left( \frac{R}{2L}I_t^z+m_wLR+\frac{I_\mathrm{w}L}{R}\right) (\dot{\gamma }_r - \dot{\gamma }_l)-M_{in}^z . \end{aligned}$$

Rights and permissions

Reprints and Permissions

Copyright information

© 2022 The Author(s), under exclusive license to Springer Nature Switzerland AG

About this paper

Check for updates. Verify currency and authenticity via CrossMark

Cite this paper

Lew, T. et al. (2022). Contact Inertial Odometry: Collisions are your Friends. In: Asfour, T., Yoshida, E., Park, J., Christensen, H., Khatib, O. (eds) Robotics Research. ISRR 2019. Springer Proceedings in Advanced Robotics, vol 20. Springer, Cham.

Download citation