Invariant ekf

[Nanda-Kishore-V]

Changes:

• Fix bugs in EKF and contact estimation
• Optimize performance of invariant EKF in simulation
• Use rear contact point (since the toe touches the ground with the rear contact point in simulation)
• Modify the logic of contact estimation, and create two versions of it -- one for controllers, the other one for ekf
• Demand the existence of body index in GetBodyIndexFromName()

[mposa]

Per a face to face discussion with Nanda last week, we weren't sure where the lightly-modified third-party EKF code should be located. The goals are to:

• Easily incorporate upstream changes.
• Clearly mark which code is derived from Ross' code, and does not need to align with our code style practices.
• Allow, if necessary, users to change the EKF code during testing.

The best option we came up with was to fork Ross Hartley's code, and make changes in that branch (which will make it easy to later merge upstream changes). Include this repository as a pegged external with optional local installation, like we do with Drake.

@yminchen @Nanda-Kishore-V @yangwill Thoughts on this? We wanted to sleep on it, to ensure we aren't adding too much complexity. Having too many of these external repositories might, ultimately, get confusing.

[mposa]

@yminchen @Nanda-Kishore-V @yangwill Following up on this. It'd be good to get this into a form everyone's happy with, and merge it.

[yminchen]

@mposa @Nanda-Kishore-V, @yangwill and I discussed. We think that it's a good idea to fork Ross's code, add our own changes, and use this repo as a pegged external.

[Nanda-Kishore-V]

What's new: invariant-ekf is moved to its own (forked) repository. The default behavior is to use the pegged version of this invariant-ekf repository. However, it can also be downloaded as a local repository for making changes.