AICO/BayesianIK without damping broken (NaN)

[wxmerkt]

When the damping is set to 0, AICO and BayesianIK produce NaN. The source of this is the update of Sinv in the forward message.

The underlying reason is our inversion assumes a positive-definite matrix, but Sinv is all zeros.