diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake
index 8b981ba..29dea30 100644
--- a/cmake/FindEigen3.cmake
+++ b/cmake/FindEigen3.cmake
@@ -2,8 +2,7 @@
 #  Eigen3_FOUND
 
 find_path( Eigen3_INCLUDE_DIR Eigen/Core
-    /usr/include/eigen3
-    /usr/local/include/eigen3
+    PATH_SUFFIXES eigen3
 )
 
 include(LibFindMacros)
diff --git a/include/kalman/UnscentedKalmanFilterBase.hpp b/include/kalman/UnscentedKalmanFilterBase.hpp
index 25e7fb2..e8bdccb 100644
--- a/include/kalman/UnscentedKalmanFilterBase.hpp
+++ b/include/kalman/UnscentedKalmanFilterBase.hpp
@@ -29,13 +29,13 @@
 #include "MeasurementModel.hpp"
 
 namespace Kalman {
-    
+
     /**
      * @brief Abstract Base for Unscented Kalman Filters
      *
      * This implementation is based upon [The square-root unscented Kalman filter for state and parameter-estimation](http://dx.doi.org/10.1109/ICASSP.2001.940586) by Rudolph van der Merwe and Eric A. Wan.
      * Whenever "the paper" is referenced within this file then this paper is meant.
-     * 
+     *
      * @param StateType The vector-type of the system state (usually some type derived from Kalman::Vector)
      */
     template<class StateType>
@@ -45,60 +45,60 @@ namespace Kalman {
         // Public typedefs
         //! Kalman Filter base type
         typedef KalmanFilterBase<StateType> Base;
-        
+
         //! Numeric Scalar Type inherited from base
         using typename Base::T;
-        
+
         //! State Type inherited from base
         using typename Base::State;
-    
+
         //! Measurement Model Type
         template<class Measurement, template<class> class CovarianceBase>
         using MeasurementModelType = MeasurementModel<State, Measurement, CovarianceBase>;
-        
+
         //! System Model Type
         template<class Control, template<class> class CovarianceBase>
         using SystemModelType = SystemModel<State, Control, CovarianceBase>;
-        
+
     protected:
         // Protected typedefs
         //! The number of sigma points (depending on state dimensionality)
         static constexpr int SigmaPointCount = 2 * State::RowsAtCompileTime + 1;
-        
+
         //! Vector containg the sigma scaling weights
         typedef Vector<T, SigmaPointCount> SigmaWeights;
-        
+
         //! Matrix type containing the sigma state or measurement points
         template<class Type>
         using SigmaPoints = Matrix<T, Type::RowsAtCompileTime, SigmaPointCount>;
 
     protected:
         // Member variables
-        
+
         //! State Estimate
         using Base::x;
-        
+
         //! Sigma weights (m)
         SigmaWeights sigmaWeights_m;
         //! Sigma weights (c)
         SigmaWeights sigmaWeights_c;
-        
+
         //! Sigma points (state)
         SigmaPoints<State> sigmaStatePoints;
-        
+
         // Weight parameters
         T alpha;    //!< Scaling parameter for spread of sigma points (usually \f$ 1E-4 \leq \alpha \leq 1 \f$)
         T beta;     //!< Parameter for prior knowledge about the distribution (\f$ \beta = 2 \f$ is optimal for Gaussian)
         T kappa;    //!< Secondary scaling parameter (usually 0)
         T gamma;    //!< \f$ \gamma = \sqrt{L + \lambda} \f$ with \f$ L \f$ being the state dimensionality
         T lambda;   //!< \f$ \lambda = \alpha^2 ( L + \kappa ) - L\f$ with \f$ L \f$ being the state dimensionality
-        
+
     protected:
         /**
          * Constructor
-         * 
+         *
          * See paper for parameter explanation
-         * 
+         *
          * @param _alpha Scaling parameter for spread of sigma points (usually 1e-4 <= alpha <= 1)
          * @param _beta Parameter for prior knowledge about the distribution (beta = 2 is optimal for Gaussian)
          * @param _kappa Secondary scaling parameter (usually 0)
@@ -107,7 +107,7 @@ namespace Kalman {
         {
             // Pre-compute all weights
             computeWeights();
-            
+
             // Setup state and covariance
             x.setZero();
         }
@@ -124,11 +124,11 @@ namespace Kalman {
         {
             // Pass each sigma point through non-linear state transition function
             computeSigmaPointTransition(s, u);
-            
+
             // Compute predicted state from predicted sigma points
             return computePredictionFromSigmaPoints<State>(sigmaStatePoints);
         }
-        
+
         /**
          * @brief Compute predicted measurement using measurement model and predicted sigma measurements
          *
@@ -141,11 +141,11 @@ namespace Kalman {
         {
             // Predict measurements for each sigma point
             computeSigmaPointMeasurements<Measurement>(m, sigmaMeasurementPoints);
-            
+
             // Predict measurement from sigma measurement points
             return computePredictionFromSigmaPoints<Measurement>(sigmaMeasurementPoints);
         }
-        
+
         /**
          * @brief Compute sigma weights
          */
@@ -154,33 +154,33 @@ namespace Kalman {
             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) );
-            
+            T W_i   = T(1) / ( T(2) *  (L + lambda) );
+
             // Make sure W_i > 0 to avoid square-root of negative number
             assert( W_i > T(0) );
-            
+
             sigmaWeights_m[0] = W_m_0;
             sigmaWeights_c[0] = W_c_0;
-            
+
             for(int i = 1; i < SigmaPointCount; ++i)
             {
                 sigmaWeights_m[i] = W_i;
                 sigmaWeights_c[i] = W_i;
             }
         }
-        
+
         /**
          * @brief Predict expected sigma states from current sigma states using system model and control input
-         * 
+         *
          * @note This covers equation (18) of Algorithm 3.1 in the Paper
          *
          * @param [in] s The System Model
@@ -194,10 +194,10 @@ namespace Kalman {
                 sigmaStatePoints.col(i) = s.f( sigmaStatePoints.col(i), u );
             }
         }
-        
+
         /**
          * @brief Predict the expected sigma measurements from predicted sigma states using measurement model
-         * 
+         *
          * @note This covers equation (23) of Algorithm 3.1 in the Paper
          *
          * @param [in] m The Measurement model
@@ -211,10 +211,10 @@ namespace Kalman {
                 sigmaMeasurementPoints.col(i) = m.h( sigmaStatePoints.col(i) );
             }
         }
-        
+
         /**
          * @brief Compute state or measurement prediciton from sigma points using pre-computed sigma weights
-         * 
+         *
          * @note This covers equations (19) and (24) of Algorithm 3.1 in the Paper
          *
          * @param [in] sigmaPoints The computed sigma points of the desired type (state or measurement)