Open iron76 opened 9 years ago
The observed numerical instability might be related to the fact joint space inertia matrix can be highly ill-conditioned for a large number of rigid links. Some empirical studies in this direction have been conducted by R. Featherstone (http://royfeatherstone.org/papers/empirical.pdf). A possible solution might be the inertia-weighted scaling proposed in the paper.
In the current implementation, the prior on d (dynamic variables) is specified only for a subset of the vector itself (external forces
fx
and joint accelerationsd2q
). It can be proven that this prior is sufficient to guarantee invertibility of the matrix to be inverted in the estimation procedure. It turns out that, even if invertible, the matrix is badly conditioned. The issue can be solved implementing a prior on the entire vector d.