The realizaton of traditional ekf slam

The predict result

APE w.r.t. translation part (m) (with SE(3) Umeyama alignment)

   max      5.609457
  mean      1.747407
median      1.458172
   min      0.438668
  rmse      2.071460
   sse      8577.604845
   std      1.112438

The ekf result

APE w.r.t. translation part (m) (with SE(3) Umeyama alignment)

   max      0.080390
  mean      0.020854
median      0.017032
   min      0.001137
  rmse      0.025817
   sse      1.332391
   std      0.015220