Fixes body index bug in ellipse constraint estimation