replace matlab code by c++ using RcppArmadillo package