WIP: IEKF estimator and minimal ROS port for PX4

[jgoppert]

@ecmnet @mhkabir @kd0aij @julianoes @bkueng This is a position and attitude estimator that borrows ideas from LPE. Hopefully I will get it passing the CI mission soon. Your input is appreciated.

Paper describing the IEKF: https://hal.archives-ouvertes.fr/hal-00494342/document

@LorenzMeier This is my current best stab at an estimator API to support easy development and algorithm comparison. I ported ROS minimally for the components I needed to PX4. It is similar to the example directory multiplatform code as you suggested, but instead of adding a common middle layer between px4 and ros, it ports ROS keeping the API the same. The benefit is that it should be an API already well known by researchers and they can compile their code without any middle-layer on ROS.

In order to port ROS, I had to add a string hashing library. This is pretty small and template based. For each Node I build up a hash table of all topics needed, and will do the same for parameters. I have to do this manually right now on the PX4 side, but I could probably automate that with some effort.

TODO

• Pass quad auto mission for CI in SITL
  • http://logs.px4.io/plot_app?log=4ad24a77-3ab7-471a-9df3-aa112f0850ec
• Pass VTOL auto mission for CI in SITL
  • URL: http://logs.px4.io/plot_app?log=485af2cc-de4d-412c-944a-626547988f28
  • URL: http://logs.px4.io/plot_app?log=6e508bc2-fb98-4d2f-b4c1-064334a19c39
  • URL: http://logs.px4.io/plot_app?log=2a44e876-009e-427c-a8f9-e7f97d37bceb
  • URL: http://logs.px4.io/plot_app?log=a38e4576-ff39-4b2a-8756-c6773f76bdf8
  • URL: http://logs.px4.io/plot_app?log=7951ada2-5e12-425f-a45d-42302762c6e9
  • URL: http://logs.px4.io/plot_app?log=fb746804-cc8f-4017-b667-b39edcede913
• Lidar correction
• Optical flow support
  • http://logs.px4.io/plot_app?log=b0cd0f14-6f3b-44a0-a805-4a3665fdf86c
• Vision/mocap support
• Landing detection support
• Parameter system
• Flight testing - In progress (only manual flight mode tested so far, working well)

IPython notebooks

• Simulation (This is pretty useful since github renders ipython notebooks, making it easy to analyze algorithms).
• IEKF Derivation (Some pretty ugly non-commutative algebra with quaternion products here if you are interested. I was able to re-derive all of the equations from the paper just to make sure I completely understood what was going on.)
• Measurement Jacobians

Quick Links to Code for Review

• Header
• Source
• Ros spin loop
• ROS port
• Hash table for ROS port

[mhkabir]

This will be great in conjunction with the new VIO interface : https://github.com/mhkabir/Firmware/tree/uasys-devel

[ecmnet]

@jgoppert Very interesting! What benefits are you expecting besides the replacement of the Q-attitude estimator? With LPE I find results heavily depending on the accelerator noise level.

[jgoppert]

@ecmnet

Motivation

• I started this filter because I wanted something that can handle more precise indoor navigation with vision position and attitude measurements than what LPE and attitude_estimator_q could provide.
• I considered using EKF2 but don't agree with a lot of the design choices (creating another layer of message passing and autogenerating code with closed source software) and don't find it hackable enough to get the changes in for my system in the short time period.

Benefits

• Improved performance:

  • By combining the attitude and position estimation there is more data and more chance for performance improvements (somthing lpe + attitude estimator q couldn't do). For instance, with this filter it is possible to fly without a magnetometer when using gps + accel + gyro outdors because it uses the positoin information to figure out the attitude. It also doesn't need accelerometer estimation of gravity direction to figure out the roll/pitch, but I added this to kick in when it isn't accelerating).

  • The filter is invariant: This means the system is linearized in the navigation frame such that the state transition and measurement matrices are constant as long as the angular velocity and acceleratoin are constant in the navigation frame. EKF2 doesn't employ this so the kalman filter gains will oscillate while rotating in a circle, while IEKF's will remain constant. This means the IEKF is closer to optimal and will have a chance for outperforming EKF2 if properly tuned.

• Hackability

  • This is meant to keep a lot of the same style as LPE where you can easily add measurements and states in a few minutes.
  • By keeping the estimator coded in pure ROS code it should be very portable.

[ecmnet]

@jgoppert Thanks for the explanation. Let me know if I can help somehow.

[jgoppert]

Latest commit adds wind states and airspeed measurement. Once I get it tuned, it should help vtol.

[ecmnet]

@jgoppert Just an idea, you might find it worth to think about: For direct estimator comparison / estimator settings comparison, it would be great to run two different estimators / two instances of an estimator in parallel. With that a direct benchmark of the whole estimator chain would be possible. As I am not that deep into the firmware, I cannot estimate what this would require. IEKF could be a starting point for that. What do you think?

[jgoppert]

@ecmnet I think that is a good idea. I think that on a pixhawk ekf2 and iekf couldn't run side by side (due to memory and cpu limitations), but we could do it on sitl. We would have to choose one estimator to publish the control state, or use a 3rd estimator to do that so the evaluation is more fair.

[ndepal]

I like that idea, @ecmnet. What do you think about, instead of running the filters at the same time, logging the sensor data and replaying it offline with the different estimators, the way that EKF2 supports it?
I think it would be desirable to achieve a generic method of doing this kind of replay for any estimator. This would probably not be too hard with uLog.

[mhkabir]

@ecmnet @ndepal @jgoppert We already support system-wide replay. I had been using it for benchmarking VIO for a while.

Instructions are here : https://github.com/PX4/Devguide/blob/master/advanced-replay.md. No idea why it doesn't show up the Devguide side-pane or why this link is broken : http://dev.px4.io/advanced-replay.html. Will find and fix.

[bkueng]

@mhkabir the replay page got dropped in PX4/PX4-Devguide@c053216 by @LorenzMeier. I assume this was a mistake, I added it again on master PX4/PX4-Devguide@79aaffe. Should be online soon.

[jgoppert]

SITL optical flow with terrain video:

https://www.youtube.com/watch?v=fx5MeMG-DM0&feature=youtu.be

[jgoppert]

Closing in favor of #7115