diff --git a/include/IKFoM_toolkit/esekfom/esekfom.hpp b/include/IKFoM_toolkit/esekfom/esekfom.hpp index 00f29d1..1a49c94 100755 --- a/include/IKFoM_toolkit/esekfom/esekfom.hpp +++ b/include/IKFoM_toolkit/esekfom/esekfom.hpp @@ -309,7 +309,7 @@ class esekf{ seg_SO3(i) = -1 * f_(dim + i) * dt; } MTK::SO3 res; - res.w() = MTK::exp(res.vec(), seg_SO3, scalar_type(1/2)); + res.w() = MTK::exp(res.vec(), seg_SO3, scalar_type(0.5)); #ifdef USE_sparse res_temp_SO3 = res.toRotationMatrix(); for(int i = 0; i < 3; i++){ @@ -341,7 +341,7 @@ class esekf{ } MTK::vect<2, scalar_type> vec = MTK::vect<2, scalar_type>::Zero(); MTK::SO3 res; - res.w() = MTK::exp(res.vec(), seg_S2, scalar_type(1/2)); + res.w() = MTK::exp(res.vec(), seg_S2, scalar_type(0.5)); Eigen::Matrix Nx; Eigen::Matrix Mx; x_.S2_Nx_yy(Nx, idx);