Handle numerical errors properly

[mherb]

No description provided.

[LuTaonuwhu]

Do the numerical errors mean non-positive definite of square root S in SR-UKF and non-positive definite of covariance matrix P in UKF?
for exmaple, here
S.rankUpdate( U.col(i), -1 );

            if( S.info() == Eigen::NumericalIssue )
            {
                // TODO: handle numerical issues in some sensible way
                return false;
            }

[mherb]

Yes, this is about these kind of errors which usually occur when the covariance matrix is not positive definite.

One approach to handle these would be to force the covariance to be positive definite for instance using the method proposed by Higham (adapted matlab implementation here).

Unfortunately I do not have the time right now to implement this, but contributions are of course very welcome.

[LuTaonuwhu]

Thanks for your information mherb, I have also notice a solution like this. I will try to fix it.

[jwdinius]

But what if the matrix is negative definite? There should be a check before even attempting a matrix rescaling; something like:

  Eigen::SelfAdjointEigenSolver< Covariance<StateType> > eigenSolver(this->P);
  if (eigenSolver.info() != Eigen::SUCCESS || eigenSolver.eigenvalues().minCoeff() < 0) {
    //! either the solver failed, which could mean the matrix is not symmetric (self-adjoint) OR
    //! there are negative eigenvalues, meaning the covariance matrix is not pos semidefinite
    //! either way, an exception should be raised
  }

I like this approach because of the ability to touch the internal filter state and covariance via the init and setCovariance public methods within the filter base class. If the caller does something bogus, the implementation could complain about it.