Two-stage approach to state and force estimation in rigid-link multibody systems