I am looking for open source implementations of an EKF for 6D pose estimation (Inertial Navigation System) using at minimum an IMU (accelerometer, gyroscope) + absolute position (or pose) sensor.

This seems to be such a recurring and important problem in robotics that I am surprised I cannot find a few reference implementations. Does everyone just quickly hack together his own EKF and move on to more interesting things? Is that not rather error-prone?

I would ideally like a well-tested implementation that can serve as a reference for fair evaluation of possible improvements.

  • $\begingroup$ @roboman, perhaps the reason the OP couldn't find much regarding the problem was that he didn't know it's called Inertial Navigation System? That's why I rejected your edit (even though a moderator accepted it), because I think you know about the problem and you could perhaps write up an answer, or at least a comment with a few pointers. $\endgroup$
    – Shahbaz
    Nov 17, 2014 at 12:35
  • $\begingroup$ @Shahbaz woha ..... I implemented one for a company few months ago , also I didn't conduct a search about INS open source , let me check $\endgroup$
    – RoboMan
    Nov 18, 2014 at 9:14

1 Answer 1


I believe this should tick all your boxes:


It's a ROS node for 6D pose estimation that has the following features:

  • Fusion of an arbitrary number of sensors. The nodes do not restrict the number of input sources. If, for example, your robot has multiple IMUs or multiple sources of odometry information, the state estimation nodes within robot_localization can support all of them.
  • Support for multiple ROS message types. All state estimation nodes in robot_localization can take in nav_msgs/Odometry, sensor_msgs/Imu, geometry_msgs/PoseWithCovarianceStamped, or geometry_msgs/TwistWithCovarianceStamped messages.
  • Per-sensor input customization. If a given sensor message contains data that you don't want to include in your state estimate, the state estimation nodes in robot_localization allow you to exclude that data on a per-sensor basis.
  • Continuous estimation. Each state estimation node in robot_localization begins estimating the vehicle's state as soon as it receives a single measurement. If there is a holiday in the sensor data (i.e., a long period in which no data is received), the filter will continue to estimate the robot's state via an internal motion model.

It has both an EKF implementation (ekf_localization_node) and a UKF (ukf_localization_node). Feel free to ask questions on the ROS Answers site.

  • $\begingroup$ Thanks, this looks almost perfect. How could I have missed it? It looks like a rather new package. The only problem I see is its reliance on Euler angles, which is probably not a problem for the authors since it seems to be aimed at wheeled robots. $\endgroup$
    – sebsch
    Nov 18, 2014 at 9:21
  • $\begingroup$ The interfaces to it (the input and output messages) use quaternions, but they are converted to Euler angles internally. $\endgroup$
    – automatom
    Nov 18, 2014 at 13:39
  • $\begingroup$ ...also, I've used it with a Parrot Drone, and it works fine. $\endgroup$
    – automatom
    Nov 18, 2014 at 15:01
  • $\begingroup$ @TheWumpus, it would be a good idea to quote the most relevant parts of the link in your answer, since sooner or later that page is either going to die or its contents changed. That way, the answer to the question would be lost. $\endgroup$
    – Shahbaz
    Nov 20, 2014 at 10:39
  • $\begingroup$ Fair enough, will do. For the record, though, I'm the maintainer of that link. :) $\endgroup$
    – automatom
    Nov 20, 2014 at 13:51

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.