  
  step = [0.1;0;0]
  params1 = ts_square([5], [0;0;0], step);
  res = icp(params1);
  
