numerical issue in `UnscentedKalmanFilterBase::computeWeights()`

[jwdinius]

The current implementation is

            ...
            T L = T(State::RowsAtCompileTime);
            lambda = alpha * alpha * ( L + kappa ) - L;
            gamma = std::sqrt( L + lambda );
            
            // Make sure L != -lambda to avoid division by zero
            assert( std::abs(L + lambda) > 1e-6 );
            
            // Make sure L != -kappa to avoid division by zero
            assert( std::abs(L + kappa) > 1e-6 );
            
            T W_m_0 = lambda / ( L + lambda );
            T W_c_0 = W_m_0 + (T(1) - alpha*alpha + beta);
            T W_i   = T(1) / ( T(2) * alpha*alpha * (L + kappa) );

For numerical reasons (i.e. not doing the divide when you could do a multiply), a better implementation would be:
The proposed implementation is

            ...
            T L = T(State::RowsAtCompileTime);
            T alphaSquared = alpha * alpha;
            lambda = alphaSquared * ( L + kappa ) - L;
            gamma = std::sqrt( L + lambda );
            
            // Make sure L != -lambda to avoid division by zero
            assert( std::abs(L + lambda) > 1e-6 );
            
            // Make sure L != -kappa to avoid division by zero
            assert( std::abs(L + kappa) > 1e-6 );
            
            T W_m_0 = lambda / ( L + lambda );
            T W_c_0 = W_m_0 + (T(1) - alphaSquared + beta);
            T W_i   = T(0.5) / (L + lambda);

I made this change when I found that the weights computed by this implementation were significantly different from this for alpha=1e-3, beta=2, kappa=0. Note: the proposed changes were shown to match the desired values.

[lucvt001]

I think this should be merged. When I try the UKF with the original code, the covariance explodes even after the first predict step. With this fix, everything is ok. So perhaps the weights initialization is indeed problematic.