I added a second pose sensor in here. It messed up some other stuff that I am not using, but the state propagation seems to be working well with two sensor inputs.
Time delay compensated single and multi sensor fusion framework based on an EKF. Please see the wiki for more information: https://github.com/ethz-asl/ethzasl_msf/wiki
##Documentation The API is documented here: http://ethz-asl.github.io/ethzasl_msf
##Contributing You are welcome contributing to the package by opening a pull-request: Please make yourself familiar with the Google c++ style guide: http://google-styleguide.googlecode.com/svn/trunk/cppguide.xml
##License The source code is released under the Apache 2.0 license