Add methods to Matrix class.
Interpolate estimated states in fork control.