VIMO: Simultaneous Visual Inertial Model-based Odometry and Force Estimation

Introduction and Overview

Given some information about the commands provided to the actuators of a robot and a dynamic model of the robot we, in theory, have enough information to make basic predictions about the motion of the robot. Can this information be useful for state estimation?

This paper claims to be able to incorporate control inputs to a drone in order to:

While the method should be robot agnostic, the paper is focused on MAVs. The control input we have access to is commanded rotor speed. We use a simple dynamic model which converts the rotor speed into a commanded thrust vector.

Background and Related Work

Deterministic Approaches

akb=Tkb+fekb\mathbf{a}_k^b = \mathbf{T}_k^b+\mathbf{f}^b_{e_k}

Assume that the measured acceleration from the IMU is composed of a commanded thrust and additive external force. Naively estimate external force as:

fekb=akbTkb\mathbf{f}^b_{e_k} = \mathbf{a}_k^b - \mathbf{T}_k^b

Probabilistic Approaches

Sliding Window Smoothing

Factor Graph Structure

Factor graph from paper:

My best guess at factor graph structure of dynamics factor:

VIO factor graph is a modification of the VINS-Mono factor graph.

  • IMU pre-integration factor
  • Inverse depth projection factors
  • Dynamics factor (new)

Pre-integrated Dynamics Factor

Like IMU preintegration, the goal of the preintegrated dynamics factor is to create a residual which can be evaluates without re-integration of control inputs when the linearization point changes.


Like the IMU pre-integration factor, the residual of the dynamics factor connects consecutive poses and velocities.

Dynamics Model

A simple model of a MAV which considers how the poses evolves according to three forces:

  1. Rotor thrust
  1. External forces (wind, drag, etc)
  1. Gravity

Model translational dynamics using:

We can estimate the trust vector by calculating the force generated by each motor. This is a function of motor speed and propeller. (Assuming there is a mass normalization not shown in this equation).


Sim (Gazebo)

Blackbird Dataset

Real World