Add feet position estimation in-the-loop

[sachinumans]

Create a filter that appends the state with the input to estimate both.

See page 167 of Verhaegen and Verdult, Filtering and System Identification: A Least Squares Approach.