diff --git a/modules/planning/conf/planning_config.pb.txt b/modules/planning/conf/planning_config.pb.txt
index 59e44aaf47743274251c639f04200507ea095adf..3bee6c826676ec66d60f3af0ff46e2e0f4df7512 100644
--- a/modules/planning/conf/planning_config.pb.txt
+++ b/modules/planning/conf/planning_config.pb.txt
@@ -127,10 +127,10 @@ em_planner_config {
 }
 
 reference_line_smoother_config {
-    num_spline: 5
+    num_spline: 7
     spline_order: 6
-    num_evaluated_points: 13
-    boundary_bound: 0.2
+    num_evaluated_points: 30
+    boundary_bound: 1.0
     derivative_bound: 1.0
     second_derivative_bound: 1.0
     third_derivative_bound: 1.0
diff --git a/modules/planning/reference_line/reference_line_smoother.cc b/modules/planning/reference_line/reference_line_smoother.cc
index 4951d6157a300d73a4240b4a7ad53acc08a5a499..731221a6bf51bdf39e3034d1e7cba626fd8a10cc 100644
--- a/modules/planning/reference_line/reference_line_smoother.cc
+++ b/modules/planning/reference_line/reference_line_smoother.cc
@@ -57,7 +57,7 @@ bool ReferenceLineSmoother::Smooth(
     ReferenceLine* const smoothed_reference_line) {
   Reset();
   std::vector<ReferencePoint> ref_points;
-  if (!sampling(raw_reference_line)) {
+  if (!Sampling(raw_reference_line)) {
     AERROR << "Fail to sample reference line smoother points!";
     return false;
   }
@@ -65,7 +65,7 @@ bool ReferenceLineSmoother::Smooth(
   spline_solver_.reset(
       new Spline2dSolver(t_knots_, smoother_config_.spline_order()));
 
-  if (!apply_constraint(raw_reference_line)) {
+  if (!ApplyConstraint(raw_reference_line)) {
     AERROR << "Add constraint for spline smoother failed";
     return false;
   }
@@ -130,7 +130,7 @@ bool ReferenceLineSmoother::Smooth(
   return true;
 }
 
-bool ReferenceLineSmoother::sampling(const ReferenceLine& raw_reference_line) {
+bool ReferenceLineSmoother::Sampling(const ReferenceLine& raw_reference_line) {
   const double length = raw_reference_line.Length();
   const double resolution = length / smoother_config_.num_spline();
   double accumulated_s = 0.0;
@@ -150,7 +150,7 @@ bool ReferenceLineSmoother::sampling(const ReferenceLine& raw_reference_line) {
   return true;
 }
 
-bool ReferenceLineSmoother::apply_constraint(
+bool ReferenceLineSmoother::ApplyConstraint(
     const ReferenceLine& raw_reference_line) {
   const double t_length = t_knots_.back() - t_knots_.front();
   const double dt = t_length / (smoother_config_.num_evaluated_points() - 1);
@@ -161,8 +161,7 @@ bool ReferenceLineSmoother::apply_constraint(
     evaluated_t.push_back(accumulated_eval_t);
   }
   std::vector<common::PathPoint> path_points;
-  if (!extract_evaluated_points(raw_reference_line, evaluated_t,
-                                &path_points)) {
+  if (!ExtractEvaluatedPoints(raw_reference_line, evaluated_t, &path_points)) {
     AERROR << "Extract evaluated points failed";
     return false;
   }
@@ -203,7 +202,7 @@ bool ReferenceLineSmoother::apply_constraint(
   }
 
   if (!spline_solver_->mutable_constraint()
-           ->AddSecondDerivativeSmoothConstraint()) {
+           ->AddThirdDerivativeSmoothConstraint()) {
     AERROR << "Add jointness constraint failed";
     return false;
   }
@@ -236,12 +235,12 @@ bool ReferenceLineSmoother::ApplyKernel() {
 
 bool ReferenceLineSmoother::Solve() { return spline_solver_->Solve(); }
 
-bool ReferenceLineSmoother::extract_evaluated_points(
+bool ReferenceLineSmoother::ExtractEvaluatedPoints(
     const ReferenceLine& raw_reference_line, const std::vector<double>& vec_t,
     std::vector<common::PathPoint>* const path_points) const {
   for (const auto t : vec_t) {
     double s = 0.0;
-    if (!get_s_from_param_t(t, &s)) {
+    if (!GetSFromParamT(t, &s)) {
       AERROR << "get s from " << t << " failed";
       return false;
     }
@@ -256,12 +255,12 @@ bool ReferenceLineSmoother::extract_evaluated_points(
   return true;
 }
 
-bool ReferenceLineSmoother::get_s_from_param_t(const double t,
-                                               double* const s) const {
+bool ReferenceLineSmoother::GetSFromParamT(const double t,
+                                           double* const s) const {
   if (t_knots_.size() < 2 || Double::Compare(t, t_knots_.back(), 1e-8) > 0) {
     return false;
   }
-  std::uint32_t lower = find_index(t);
+  std::uint32_t lower = FindIndex(t);
   std::uint32_t upper = lower + 1;
   double weight = 0.0;
   if (Double::Compare(t_knots_[upper], t_knots_[lower], 1e-8) > 0) {
@@ -272,7 +271,7 @@ bool ReferenceLineSmoother::get_s_from_param_t(const double t,
   return true;
 }
 
-std::uint32_t ReferenceLineSmoother::find_index(const double t) const {
+std::uint32_t ReferenceLineSmoother::FindIndex(const double t) const {
   auto upper_bound = std::upper_bound(t_knots_.begin() + 1, t_knots_.end(), t);
   return std::min(t_knots_.size() - 1,
                   static_cast<std::size_t>(upper_bound - t_knots_.begin())) -
diff --git a/modules/planning/reference_line/reference_line_smoother.h b/modules/planning/reference_line/reference_line_smoother.h
index 41e7f9d5f31ab2a14ebef44b6cf1b3ba84834641..0174b3d3a5193e2ddb4004befb1ce61bb3fa2a46 100644
--- a/modules/planning/reference_line/reference_line_smoother.h
+++ b/modules/planning/reference_line/reference_line_smoother.h
@@ -45,21 +45,21 @@ class ReferenceLineSmoother {
  private:
   void Reset();
 
-  bool sampling(const ReferenceLine& raw_reference_line);
+  bool Sampling(const ReferenceLine& raw_reference_line);
 
-  bool apply_constraint(const ReferenceLine& raw_reference_line);
+  bool ApplyConstraint(const ReferenceLine& raw_reference_line);
 
   bool ApplyKernel();
 
   bool Solve();
 
-  bool extract_evaluated_points(
+  bool ExtractEvaluatedPoints(
       const ReferenceLine& raw_reference_line, const std::vector<double>& vec_t,
       std::vector<common::PathPoint>* const path_points) const;
 
-  bool get_s_from_param_t(const double t, double* const s) const;
+  bool GetSFromParamT(const double t, double* const s) const;
 
-  std::uint32_t find_index(const double t) const;
+  std::uint32_t FindIndex(const double t) const;
 
  private:
   ReferenceLineSmootherConfig smoother_config_;
diff --git a/modules/planning/reference_line/reference_line_smoother_test.cc b/modules/planning/reference_line/reference_line_smoother_test.cc
index ff66cd59ac87f8d0619a943983808ff31282a19b..9dcb296aba0a2ec2d5b8df4d235415455550eb29 100644
--- a/modules/planning/reference_line/reference_line_smoother_test.cc
+++ b/modules/planning/reference_line/reference_line_smoother_test.cc
@@ -74,7 +74,7 @@ TEST_F(ReferenceLineSmootherTest, smooth) {
   ReferenceLine smoothed_reference_line;
   EXPECT_FLOAT_EQ(153.87421, reference_line_->Length());
   EXPECT_TRUE(smoother_.Smooth(*reference_line_, &smoothed_reference_line));
-  EXPECT_FLOAT_EQ(153.3642, smoothed_reference_line.Length());
+  EXPECT_FLOAT_EQ(153.52716, smoothed_reference_line.Length());
 }
 
 }  // namespace planning
diff --git a/modules/planning/testdata/garage_test/result_follow_0.pb.txt b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
index 73a3384a3413b70eca18e644e35332363dd2f6c5..60ac9dafeb390858f5251141bad3339344da11c9 100755
--- a/modules/planning/testdata/garage_test/result_follow_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
@@ -9,7 +9,7 @@ trajectory_point {
     x: 586344.8230049999
     y: 4140700.15992
     theta: 1.3255010179499997
-    kappa: -0.026050217221752481
+    kappa: -0.026050217221752485
     s: 4.0779554212611036e-17
     dkappa: 0
     ddkappa: 0
@@ -20,10 +20,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586344.89329499984
-    y: 4140700.4310858184
-    theta: 1.3189302731683865
-    kappa: -0.023978165037994721
+    x: 586344.89329739776
+    y: 4140700.4310851973
+    theta: 1.3188527560044321
+    kappa: -0.024193230457514418
     s: 0.28012780139567195
     dkappa: 0
     ddkappa: 0
@@ -34,10 +34,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586344.96361456
-    y: 4140700.702365675
-    theta: 1.3123566323350411
-    kappa: -0.021905241454724556
+    x: 586344.963619357
+    y: 4140700.7023644317
+    theta: 1.3122015633453723
+    kappa: -0.022335462739315061
     s: 0.56037341026330767
     dkappa: 0
     ddkappa: 0
@@ -48,10 +48,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.03602726932
-    y: 4140700.9730017064
-    theta: 1.3065083353029374
-    kappa: -0.01996298353697204
+    x: 586345.03612708452
+    y: 4140700.972974631
+    theta: 1.3062053101274713
+    kappa: -0.020614271075258962
     s: 0.84053704408195573
     dkappa: 0
     ddkappa: 0
@@ -62,10 +62,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.11022243614
-    y: 4140701.2428796608
-    theta: 1.30129954768076
-    kappa: -0.018136625542645543
+    x: 586345.11042846739
+    y: 4140701.2428233624
+    theta: 1.3007662718778132
+    kappa: -0.019009690776826488
     s: 1.120428153348308
     dkappa: 0
     ddkappa: 0
@@ -76,10 +76,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.18447178975
-    y: 4140701.5122709847
-    theta: 1.2961670468657287
-    kappa: -0.016325745025777841
+    x: 586345.18485094781
+    y: 4140701.5121664596
+    theta: 1.2954126852247858
+    kappa: -0.017423785845482338
     s: 1.399865421576701
     dkappa: 0
     ddkappa: 0
@@ -90,10 +90,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.2613890284
-    y: 4140701.7802625964
-    theta: 1.2921466521587457
-    kappa: -0.014722211824453591
+    x: 586345.26207915682
+    y: 4140701.7800686238
+    theta: 1.2910128382448496
+    kappa: -0.016039207008496523
     s: 1.6786767652991148
     dkappa: 0
     ddkappa: 0
@@ -104,10 +104,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.33808866329
-    y: 4140702.0474960422
-    theta: 1.288137607290238
-    kappa: -0.013123215120152746
+    x: 586345.3390888822
+    y: 4140702.0472128745
+    theta: 1.2866254106226789
+    kappa: -0.014658545232764856
     s: 1.9566993340651737
     dkappa: 0
     ddkappa: 0
@@ -118,10 +118,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.41586440639
-    y: 4140702.3134337626
-    theta: 1.2847402991758456
-    kappa: -0.011639831144571039
+    x: 586345.41737712093
+    y: 4140702.3129996085
+    theta: 1.2827766215034204
+    kappa: -0.01339346123175573
     s: 2.2337795104421456
     dkappa: 0
     ddkappa: 0
@@ -132,10 +132,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.49435672839
-    y: 4140702.5780302705
-    theta: 1.2818136207519719
-    kappa: -0.010246523588317968
+    x: 586345.49642322026
+    y: 4140702.5774312085
+    theta: 1.279305137704402
+    kappa: -0.012210048787275394
     s: 2.5097729100149428
     dkappa: 0
     ddkappa: 0
@@ -146,10 +146,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.57257850666
-    y: 4140702.8414322203
-    theta: 1.2789426637082597
-    kappa: -0.0088676594796019675
+    x: 586345.57528234855
+    y: 4140702.8406426511
+    theta: 1.2759097852164396
+    kappa: -0.011044958245047209
     s: 2.7845443813861208
     dkappa: 0
     ddkappa: 0
@@ -160,10 +160,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.65192884975
-    y: 4140703.1030884869
-    theta: 1.2769256844947352
-    kappa: -0.0076581331591804493
+    x: 586345.655630605
+    y: 4140703.1019942081
+    theta: 1.273230104405997
+    kappa: -0.010036151938023728
     s: 3.0579680061758805
     dkappa: 0
     ddkappa: 0
@@ -174,10 +174,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.73085417063
-    y: 4140703.3633432505
-    theta: 1.2749195051214826
-    kappa: -0.0064550853935444351
+    x: 586345.735548494
+    y: 4140703.3619458945
+    theta: 1.2705647694566311
+    kappa: -0.0090327490743298128
     s: 3.3299270990220653
     dkappa: 0
     ddkappa: 0
@@ -188,10 +188,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.809857563
-    y: 4140703.62193044
-    theta: 1.2733149579398628
-    kappa: -0.0053352365962293586
+    x: 586345.81581407634
+    y: 4140703.6201434862
+    theta: 1.2682533261503257
+    kappa: -0.0081105530190158648
     s: 3.6003142075801629
     dkappa: 0
     ddkappa: 0
@@ -202,10 +202,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.88891402446
-    y: 4140703.878754992
-    theta: 1.2721153148081714
-    kappa: -0.0042995209830261547
+    x: 586345.89628736279
+    y: 4140703.8765276219
+    theta: 1.2662502557599553
+    kappa: -0.0072595761896685337
     s: 3.8690311125233054
     dkappa: 0
     ddkappa: 0
@@ -216,10 +216,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.9674529325
-    y: 4140704.13389821
-    theta: 1.2709235259835061
-    kappa: -0.0032705858158520074
+    x: 586345.9762338202
+    y: 4140704.1312333061
+    theta: 1.2642603025954311
+    kappa: -0.0064141703900419218
     s: 4.1359888275422687
     dkappa: 0
     ddkappa: 0
@@ -230,10 +230,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.04595145909
-    y: 4140704.3871291606
-    theta: 1.2703341746626338
-    kappa: -0.0023710566629685337
+    x: 586346.0567069531
+    y: 4140704.3838436091
+    theta: 1.2627995482509893
+    kappa: -0.00569200993329039
     s: 4.4011075993454725
     dkappa: 0
     ddkappa: 0
@@ -244,10 +244,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.12393937609
-    y: 4140704.6385193449
-    theta: 1.2698140646094866
-    kappa: -0.0014913853934295045
+    x: 586346.13666137075
+    y: 4140704.6346152807
+    theta: 1.2613784392304821
+    kappa: -0.0049816854058544018
     s: 4.66431690765898
     dkappa: 0
     ddkappa: 0
@@ -258,10 +258,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.20138202549
-    y: 4140704.888015267
-    theta: 1.2694407919647579
-    kappa: -0.00064922625601389
+    x: 586346.21625638835
+    y: 4140704.8834326332
+    theta: 1.2601068443633177
+    kappa: -0.004309688275831761
     s: 4.9255554652264992
     dkappa: 0
     ddkappa: 0
@@ -272,10 +272,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.2783371011
-    y: 4140705.1355445031
-    theta: 1.2694848159279131
-    kappa: 9.6755219876171433e-05
+    x: 586346.29576988146
+    y: 4140705.1301519293
+    theta: 1.2591555106855499
+    kappa: -0.003716678669248919
     s: 5.1847712178093825
     dkappa: 0
     ddkappa: 0
@@ -286,10 +286,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.35467894084
-    y: 4140705.38110124
-    theta: 1.2695284890750125
-    kappa: 0.00083679215393122263
+    x: 586346.37464975123
+    y: 4140705.3749051793
+    theta: 1.2582117578915306
+    kappa: -0.0031283946095178785
     s: 5.441921344186623
     dkappa: 0
     ddkappa: 0
@@ -300,10 +300,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.43028983928
-    y: 4140705.6246868502
-    theta: 1.2698385382939925
-    kappa: 0.0015104960216794124
+    x: 586346.45318384131
+    y: 4140705.6175639974
+    theta: 1.2575010536452254
+    kappa: -0.002601104572862882
     s: 5.6969722561548632
     dkappa: 0
     ddkappa: 0
@@ -314,10 +314,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.50518437172
-    y: 4140705.8662713063
-    theta: 1.2703609721913807
-    kappa: 0.0021299999709593815
+    x: 586346.5312515432
+    y: 4140705.8581417911
+    theta: 1.2569379752952317
+    kappa: -0.0021135412651665814
     s: 5.9498995985283853
     dkappa: 0
     ddkappa: 0
@@ -328,10 +328,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.57944561425
-    y: 4140706.1058129836
-    theta: 1.2708789884665692
-    kappa: 0.0027442655463217349
+    x: 586346.60865912354
+    y: 4140706.0966853164
+    theta: 1.2563796582819462
+    kappa: -0.0016301006735840217
     s: 6.2006882491391169
     dkappa: 0
     ddkappa: 0
@@ -342,10 +342,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.6527037971
-    y: 4140706.3434199514
-    theta: 1.2716930033153693
-    kappa: 0.0032779018904231612
+    x: 586346.68563279184
+    y: 4140706.3331148364
+    theta: 1.2560923209640049
+    kappa: -0.0012189452148884357
     s: 6.4493323188366292
     dkappa: 0
     ddkappa: 0
@@ -356,10 +356,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.72523094364
-    y: 4140706.5790116731
-    theta: 1.2725817736104754
-    kappa: 0.0037864282541915173
+    x: 586346.761982432
+    y: 4140706.5674956786
+    theta: 1.2558528040134642
+    kappa: -0.0008229394774776083
     s: 6.6958351514881373
     dkappa: 0
     ddkappa: 0
@@ -370,10 +370,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.79713178636
-    y: 4140706.8125689575
-    theta: 1.2734628684688671
-    kappa: 0.0042905632695936042
+    x: 586346.83767456666
+    y: 4140706.79985195
+    theta: 1.2556216874612793
+    kappa: -0.00043207735130742386
     s: 6.9402093239785021
     dkappa: 0
     ddkappa: 0
@@ -384,10 +384,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.86775184271
-    y: 4140707.0443150187
-    theta: 1.2746410836580691
-    kappa: 0.0047075078544023558
+    x: 586346.91278228629
+    y: 4140707.0301827677
+    theta: 1.2556313952227487
+    kappa: -0.00010960795646966959
     s: 7.1824766462102252
     dkappa: 0
     ddkappa: 0
@@ -398,10 +398,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.93773094751
-    y: 4140707.2740864027
-    theta: 1.2758257394825276
-    kappa: 0.0051163847413054787
+    x: 586346.98724646424
+    y: 4140707.2585400539
+    theta: 1.2556410198056773
+    kappa: 0.000210098439503535
     s: 7.4226681611034548
     dkappa: 0
     ddkappa: 0
@@ -412,10 +412,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.0070836849
-    y: 4140707.5019206395
-    theta: 1.277010073326883
-    kappa: 0.0055187819412675938
+    x: 586347.061070636
+    y: 4140707.4849650157
+    theta: 1.2556751273216231
+    kappa: 0.00052010875268424731
     s: 7.66082414459598
     dkappa: 0
     ddkappa: 0
@@ -426,10 +426,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.07501645247
-    y: 4140707.7281094752
-    theta: 1.278430018809938
-    kappa: 0.0058416804698106853
+    x: 586347.13421662641
+    y: 4140707.7095221682
+    theta: 1.2558806139628176
+    kappa: 0.00077870888305537006
     s: 7.8969941056432393
     dkappa: 0
     ddkappa: 0
@@ -440,10 +440,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.14239485015
-    y: 4140707.9524524827
-    theta: 1.2798383792544936
-    kappa: 0.0061619439637126214
+    x: 586347.206765704
+    y: 4140707.932246807
+    theta: 1.2560844237228628
+    kappa: 0.0010351986900849479
     s: 8.13123678621831
     dkappa: 0
     ddkappa: 0
@@ -454,10 +454,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.20918604219
-    y: 4140708.1750303707
-    theta: 1.2812457634132788
-    kappa: 0.0064758937168112648
+    x: 586347.27870966424
+    y: 4140708.153213081
+    theta: 1.2563113629850031
+    kappa: 0.0012825567243364947
     s: 8.3636201613119141
     dkappa: 0
     ddkappa: 0
@@ -468,10 +468,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.27453564666
-    y: 4140708.3961782362
-    theta: 1.2828234060104198
-    kappa: 0.006720526303228537
+    x: 586347.34993650182
+    y: 4140708.3725386085
+    theta: 1.2566764629458158
+    kappa: 0.0014879034490872956
     s: 8.5942214389324221
     dkappa: 0
     ddkappa: 0
@@ -482,10 +482,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.33940472268
-    y: 4140708.6156999585
-    theta: 1.2843894515150662
-    kappa: 0.0069633600582316608
+    x: 586347.42063959443
+    y: 4140708.5902513918
+    theta: 1.2570388782901589
+    kappa: 0.001691740219074162
     s: 8.82312706010584
     dkappa: 0
     ddkappa: 0
@@ -496,10 +496,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.40382038325
-    y: 4140708.8336872882
-    theta: 1.2859445467959434
-    kappa: 0.0072044964773066746
+    x: 586347.49081963429
+    y: 4140708.8064517789
+    theta: 1.2574117268338636
+    kappa: 0.0018903002511178396
     s: 9.0504326988758272
     dkappa: 0
     ddkappa: 0
@@ -510,10 +510,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.46671584761
-    y: 4140709.0505618304
-    theta: 1.2876359247106028
-    kappa: 0.0073783404868829564
+    x: 586347.56028881541
+    y: 4140709.0213109031
+    theta: 1.2578941149903242
+    kappa: 0.0020542844968845448
     s: 9.27624326230368
     dkappa: 0
     ddkappa: 0
@@ -524,10 +524,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.52922324068
-    y: 4140709.2661110917
-    theta: 1.2893174235845641
-    kappa: 0.0075509154276782592
+    x: 586347.6293331607
+    y: 4140709.2348560658
+    theta: 1.2583735532206397
+    kappa: 0.0022172659037045257
     s: 9.50067289046834
     dkappa: 0
     ddkappa: 0
@@ -538,10 +538,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.59138038184
-    y: 4140709.48045255
-    theta: 1.2909894962372548
-    kappa: 0.0077225233672882816
+    x: 586347.69799062493
+    y: 4140709.4472046555
+    theta: 1.2588503048604893
+    kappa: 0.0023793340654563212
     s: 9.7238449564664009
     dkappa: 0
     ddkappa: 0
@@ -552,10 +552,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.65220461355
-    y: 4140709.6940062279
-    theta: 1.2927398833771995
-    kappa: 0.0078404928276611142
+    x: 586347.76596103958
+    y: 4140709.6585927838
+    theta: 1.2594241594808444
+    kappa: 0.0025120775460417038
     s: 9.9458920664120853
     dkappa: 0
     ddkappa: 0
@@ -566,10 +566,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.7126165852
-    y: 4140709.9066554289
-    theta: 1.2944946907446448
-    kappa: 0.0079505404480675872
+    x: 586347.83362818928
+    y: 4140709.8690457381
+    theta: 1.2599961520723582
+    kappa: 0.0026440389171689389
     s: 10.16695605943727
     dkappa: 0
     ddkappa: 0
@@ -580,10 +580,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.77279515809
-    y: 4140710.11848307
-    theta: 1.2962427153269815
-    kappa: 0.0080601629048271221
+    x: 586347.90103391034
+    y: 4140710.0786856175
+    theta: 1.260565934643352
+    kappa: 0.0027754904619355812
     s: 10.387165982441362
     dkappa: 0
     ddkappa: 0
@@ -594,10 +594,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.831920696
-    y: 4140710.3297683019
-    theta: 1.2980237040065883
-    kappa: 0.0081316013529449081
+    x: 586347.9678439647
+    y: 4140710.2876688195
+    theta: 1.2612056613659894
+    kappa: 0.0028883412453789285
     s: 10.606568783901897
     dkappa: 0
     ddkappa: 0
@@ -608,10 +608,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.8904908892
-    y: 4140710.5403974866
-    theta: 1.2998146568869116
-    kappa: 0.0081871430336883169
+    x: 586348.0343459018
+    y: 4140710.495929746
+    theta: 1.261857580552014
+    kappa: 0.00299714968073379
     s: 10.825189736356586
     dkappa: 0
     ddkappa: 0
@@ -622,10 +622,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.94885826716
-    y: 4140710.75029731
-    theta: 1.3015994068929084
-    kappa: 0.0082424923861900683
+    x: 586348.10061755788
+    y: 4140710.7034695111
+    theta: 1.2625072421598269
+    kappa: 0.0031055813371585
     s: 11.043053654043652
     dkappa: 0
     ddkappa: 0
@@ -636,10 +636,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.00642660167
-    y: 4140710.9596569729
-    theta: 1.303388948167101
-    kappa: 0.0082733644845372767
+    x: 586348.16638107318
+    y: 4140710.9104020675
+    theta: 1.2631986382385563
+    kappa: 0.0032052886293070988
     s: 11.260184892901835
     dkappa: 0
     ddkappa: 0
@@ -650,10 +650,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.06320212549
-    y: 4140711.1684995452
-    theta: 1.305183495790704
-    kappa: 0.0082797585638792956
+    x: 586348.23176479549
+    y: 4140711.1167116631
+    theta: 1.2639131997888349
+    kappa: 0.00329983162465596
     s: 11.476607350570399
     dkappa: 0
     ddkappa: 0
@@ -664,10 +664,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.119797859
-    y: 4140711.3766807788
-    theta: 1.3069723620198206
-    kappa: 0.0082861323951832367
+    x: 586348.29694146826
+    y: 4140711.32236794
+    theta: 1.2646254985338421
+    kappa: 0.0033940752320761377
     s: 11.692344466389123
     dkappa: 0
     ddkappa: 0
@@ -678,10 +678,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.1759060648
-    y: 4140711.5843071551
-    theta: 1.3087528569373059
-    kappa: 0.0082811932791480025
+    x: 586348.36172470287
+    y: 4140711.5274538263
+    theta: 1.2653603689393913
+    kappa: 0.0034858660779278658
     s: 11.907419221398307
     dkappa: 0
     ddkappa: 0
@@ -692,10 +692,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.23093628418
-    y: 4140711.7915606443
-    theta: 1.3105197258607373
-    kappa: 0.0082434768535047769
+    x: 586348.42602359876
+    y: 4140711.7320216279
+    theta: 1.2661303991827502
+    kappa: 0.0035741206647248768
     s: 12.121854138338767
     dkappa: 0
     ddkappa: 0
@@ -706,10 +706,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.285807965
-    y: 4140711.9982170481
-    theta: 1.3122815086942574
-    kappa: 0.0082058690865325062
+    x: 586348.49013725342
+    y: 4140711.9360000826
+    theta: 1.2668982112279124
+    kappa: 0.0036621209955593
     s: 12.335671281651839
     dkappa: 0
     ddkappa: 0
@@ -720,10 +720,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.34052665194
-    y: 4140712.2042972506
-    theta: 1.314038372538727
-    kappa: 0.0081683661779804965
+    x: 586348.55400107079
+    y: 4140712.1394320382
+    theta: 1.2676720978132019
+    kappa: 0.0037500808136097853
     s: 12.54889225747938
     dkappa: 0
     ddkappa: 0
@@ -734,10 +734,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.39395959966
-    y: 4140712.4101204667
-    theta: 1.3157524299275787
-    kappa: 0.00809661573429431
+    x: 586348.61722247524
+    y: 4140712.3424624712
+    theta: 1.2684982493844783
+    kappa: 0.0038391585717437206
     s: 12.76153821366376
     dkappa: 0
     ddkappa: 0
@@ -748,10 +748,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.44723168039
-    y: 4140712.6154128029
-    theta: 1.3174613040802499
-    kappa: 0.0080244011694521056
+    x: 586348.68027907284
+    y: 4140712.54496364
+    theta: 1.2693222479028357
+    kappa: 0.00392800412004172
     s: 12.973629839747872
     dkappa: 0
     ddkappa: 0
@@ -762,10 +762,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.500369609
-    y: 4140712.8201881624
-    theta: 1.3191658715650765
-    kappa: 0.0079523684586380022
+    x: 586348.74317687843
+    y: 4140712.7469548616
+    theta: 1.2701441708496122
+    kappa: 0.0040166259334065736
     s: 13.185187366975128
     dkappa: 0
     ddkappa: 0
@@ -776,10 +776,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.55261823314
-    y: 4140713.024660707
-    theta: 1.3208263445827018
-    kappa: 0.00786186560356721
+    x: 586348.80544729659
+    y: 4140712.9486020259
+    theta: 1.2710176846251013
+    kappa: 0.0041117091266212307
     s: 13.396230568289459
     dkappa: 0
     ddkappa: 0
@@ -790,10 +790,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.60438409355
-    y: 4140713.2287460715
-    theta: 1.3224640006892963
-    kappa: 0.0077627392313384417
+    x: 586348.8674775674
+    y: 4140713.14980537
+    theta: 1.2718997775185814
+    kappa: 0.0042078933326993133
     s: 13.606778758335309
     dkappa: 0
     ddkappa: 0
@@ -804,10 +804,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.65603288542
-    y: 4140713.4323698971
-    theta: 1.3240979530929737
-    kappa: 0.0076638370334907334
+    x: 586348.92936755647
+    y: 4140713.3505536932
+    theta: 1.2727798753190647
+    kappa: 0.0043038600181077667
     s: 13.816850793457649
     dkappa: 0
     ddkappa: 0
@@ -818,10 +818,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.70718480123
-    y: 4140713.6356463949
-    theta: 1.3256962437510427
-    kappa: 0.0075397995243615444
+    x: 586348.99081647163
+    y: 4140713.5509584788
+    theta: 1.2736912231105739
+    kappa: 0.0043940085448801371
     s: 14.02646507170196
     dkappa: 0
     ddkappa: 0
@@ -832,10 +832,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.75751485385
-    y: 4140713.8386755586
-    theta: 1.3272315166002668
-    kappa: 0.0073688908521974883
+    x: 586349.05184345064
+    y: 4140713.7510326225
+    theta: 1.2746323978226317
+    kappa: 0.004478600853036782
     s: 14.235639532814247
     dkappa: 0
     ddkappa: 0
@@ -846,10 +846,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.80774328706
-    y: 4140714.0412947927
-    theta: 1.3287636919765839
-    kappa: 0.0071983272548137474
+    x: 586349.11274721264
+    y: 4140713.9507028046
+    theta: 1.2755716725140227
+    kappa: 0.0045630223642889513
     s: 14.444391658241035
     dkappa: 0
     ddkappa: 0
@@ -860,10 +860,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.85787419684
-    y: 4140714.2435206231
-    theta: 1.3302928876377187
-    kappa: 0.0070280948232300322
+    x: 586349.17342678958
+    y: 4140714.1500174315
+    theta: 1.2765170802910675
+    kappa: 0.0046425040551857444
     s: 14.652738471129364
     dkappa: 0
     ddkappa: 0
@@ -874,10 +874,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.90692764032
-    y: 4140714.4456104911
-    theta: 1.3316608100548564
-    kappa: 0.0067454976638372712
+    x: 586349.23345061857
+    y: 4140714.3491246812
+    theta: 1.2775014810616918
+    kappa: 0.0046973783030656692
     s: 14.860696536326795
     dkappa: 0
     ddkappa: 0
@@ -888,10 +888,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.95589153492
-    y: 4140714.6473386381
-    theta: 1.3330260191427075
-    kappa: 0.0064632179485584543
+    x: 586349.29336689063
+    y: 4140714.5478751492
+    theta: 1.278484118866031
+    kappa: 0.0047521542214943062
     s: 15.068281960381405
     dkappa: 0
     ddkappa: 0
@@ -902,10 +902,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.00477122422
-    y: 4140714.8487198655
-    theta: 1.3343888788289773
-    kappa: 0.0061814236809115989
+    x: 586349.3531801221
+    y: 4140714.7462838185
+    theta: 1.2794650658795219
+    kappa: 0.0048068359396015933
     s: 15.275510391541793
     dkappa: 0
     ddkappa: 0
@@ -916,10 +916,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.0530424586
-    y: 4140715.0498958821
-    theta: 1.3356033281945008
-    kappa: 0.0058301720810119027
+    x: 586349.41235932638
+    y: 4140714.9445256353
+    theta: 1.2804671655840836
+    kappa: 0.0048401772019241729
     s: 15.482397019757075
     dkappa: 0
     ddkappa: 0
@@ -930,10 +930,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.10092446231
-    y: 4140715.250829116
-    theta: 1.3367292118683161
-    kappa: 0.0054380234965993694
+    x: 586349.4713066268
+    y: 4140715.1424954971
+    theta: 1.2814735671369213
+    kappa: 0.0048679744309758129
     s: 15.688956576676883
     dkappa: 0
     ddkappa: 0
@@ -944,10 +944,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.14873395732
-    y: 4140715.4514580714
-    theta: 1.3378533907734598
-    kappa: 0.0050464687519166055
+    x: 586349.53016466182
+    y: 4140715.340165569
+    theta: 1.2824784444639428
+    kappa: 0.00489572956603644
     s: 15.895203335651376
     dkappa: 0
     ddkappa: 0
@@ -958,10 +958,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.19631563686
-    y: 4140715.6518336846
-    theta: 1.338901771218417
-    kappa: 0.0046287746396490815
+    x: 586349.58866551809
+    y: 4140715.5376295387
+    theta: 1.2834863292153103
+    kappa: 0.0049137453571456869
     s: 16.10115111173122
     dkappa: 0
     ddkappa: 0
@@ -972,10 +972,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.24333052884
-    y: 4140715.8520498667
-    theta: 1.3397143744832056
-    kappa: 0.0041272811389402517
+    x: 586349.64668988634
+    y: 4140715.7349366406
+    theta: 1.2844993120984993
+    kappa: 0.0049176309737614761
     s: 16.306813261667607
     dkappa: 0
     ddkappa: 0
@@ -986,10 +986,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.2902830746
-    y: 4140716.052000544
-    theta: 1.3405259006493218
-    kappa: 0.003626452666604
+    x: 586349.70463730884
+    y: 4140715.9319820949
+    theta: 1.2855109522766113
+    kappa: 0.0049215114376777967
     s: 16.51220268391225
     dkappa: 0
     ddkappa: 0
@@ -1000,10 +1000,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.337176118
-    y: 4140716.2516978262
-    theta: 1.3413363978009776
-    kappa: 0.0031262588881549032
+    x: 586349.76250858745
+    y: 4140716.1287786271
+    theta: 1.2865212897121636
+    kappa: 0.0049253004121010847
     s: 16.717331818617374
     dkappa: 0
     ddkappa: 0
@@ -1014,10 +1014,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.38365217578
-    y: 4140716.4512375561
-    theta: 1.3418113481836516
-    kappa: 0.0025307849726806959
+    x: 586349.81963877031
+    y: 4140716.3255330333
+    theta: 1.287525635592768
+    kappa: 0.0049076306213152133
     s: 16.922212647635725
     dkappa: 0
     ddkappa: 0
@@ -1028,10 +1028,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.43000544864
-    y: 4140716.6505628177
-    theta: 1.3422215974364839
-    kappa: 0.0019176133240128082
+    x: 586349.87670292752
+    y: 4140716.5220600488
+    theta: 1.288528822165202
+    kappa: 0.0048899812516237946
     s: 17.126856694520573
     dkappa: 0
     ddkappa: 0
@@ -1042,10 +1042,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.47630759492
-    y: 4140716.8496682295
-    theta: 1.3426313941769616
-    kappa: 0.00130511798718189
+    x: 586349.93370414455
+    y: 4140716.7183703007
+    theta: 1.2895309016518108
+    kappa: 0.0048723513487124578
     s: 17.331275024525695
     dkappa: 0
     ddkappa: 0
@@ -1056,10 +1056,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.522509089
-    y: 4140717.0485761808
-    theta: 1.3428458954823057
-    kappa: 0.0006737807117112467
+    x: 586349.99024591048
+    y: 4140716.9145892141
+    theta: 1.290520259413714
+    kappa: 0.0048433318734990415
     s: 17.535478244605397
     dkappa: 0
     ddkappa: 0
@@ -1070,10 +1070,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.56859440159
-    y: 4140717.2473006984
-    theta: 1.3427982213740468
-    kappa: 1.6881025937542861e-05
+    x: 586350.04645001073
+    y: 4140717.11069222
+    theta: 1.2915004220967536
+    kappa: 0.00480631911057596
     s: 17.739476503414505
     dkappa: 0
     ddkappa: 0
@@ -1084,10 +1084,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.61463560024
-    y: 4140717.4458349925
-    theta: 1.342750592900267
-    kappa: -0.00063938986327866283
+    x: 586350.10260031139
+    y: 4140717.3066075118
+    theta: 1.2924796467872091
+    kappa: 0.004769341776955727
     s: 17.943279491308353
     dkappa: 0
     ddkappa: 0
@@ -1098,10 +1098,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.66063547763
-    y: 4140717.6441878919
-    theta: 1.342701657352485
-    kappa: -0.001292696631472263
+    x: 586350.15859829518
+    y: 4140717.502372751
+    theta: 1.2934530244747919
+    kappa: 0.0047297726355479408
     s: 18.146896440342797
     dkappa: 0
     ddkappa: 0
@@ -1112,10 +1112,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.706744001
-    y: 4140717.8423335692
-    theta: 1.3423685782671961
-    kappa: -0.0014477747806479156
+    x: 586350.21401050966
+    y: 4140717.6981205265
+    theta: 1.29439924352017
+    kappa: 0.0046762861872056711
     s: 18.350336124274222
     dkappa: 0
     ddkappa: 0
@@ -1126,10 +1126,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.75281423284
-    y: 4140718.0403146921
-    theta: 1.3420357757402757
-    kappa: -0.0016027241427653185
+    x: 586350.26937670633
+    y: 4140717.8937057396
+    theta: 1.2953446777103201
+    kappa: 0.0046228441575153836
     s: 18.553606858559522
     dkappa: 0
     ddkappa: 0
@@ -1140,10 +1140,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.79884796473
-    y: 4140718.2381389611
-    theta: 1.3417032369030164
-    kappa: -0.0017575507437017882
+    x: 586350.32469903817
+    y: 4140718.0891359975
+    theta: 1.2962893621220848
+    kappa: 0.0045694444681521982
     s: 18.756716548261942
     dkappa: 0
     ddkappa: 0
@@ -1154,10 +1154,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.8449908921
-    y: 4140718.4357803119
-    theta: 1.3413304103363088
-    kappa: -0.0018773782429807957
+    x: 586350.37953816343
+    y: 4140718.2845429513
+    theta: 1.2972032938007214
+    kappa: 0.0045057612190340258
     s: 18.959672921060449
     dkappa: 0
     ddkappa: 0
@@ -1168,10 +1168,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.891207273
-    y: 4140718.6332548494
-    theta: 1.3409278322345171
-    kappa: -0.0019712892272336778
+    x: 586350.43416611548
+    y: 4140718.4798579123
+    theta: 1.2981048807778732
+    kappa: 0.0044381061429722528
     s: 19.162483539793055
     dkappa: 0
     ddkappa: 0
@@ -1182,10 +1182,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.93739211943
-    y: 4140718.8305946467
-    theta: 1.3405255288008622
-    kappa: -0.0020651361341852952
+    x: 586350.488756794
+    y: 4140718.6750396057
+    theta: 1.2990058525887314
+    kappa: 0.0043704972292920741
     s: 19.365155776873934
     dkappa: 0
     ddkappa: 0
@@ -1196,10 +1196,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.9835877791
-    y: 4140719.027797136
-    theta: 1.3401148153807492
-    kappa: -0.0021579762520206949
+    x: 586350.54314652737
+    y: 4140718.87014099
+    theta: 1.2998916940859293
+    kappa: 0.0042995262745979456
     s: 19.567696814293438
     dkappa: 0
     ddkappa: 0
@@ -1210,10 +1210,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.03001407045
-    y: 4140719.2248178776
-    theta: 1.3396492007253875
-    kappa: -0.0022447417127676212
+    x: 586350.59706199891
+    y: 4140719.0652453071
+    theta: 1.3007382662761278
+    kappa: 0.0042195312382524927
     s: 19.770113643618092
     dkappa: 0
     ddkappa: 0
@@ -1224,10 +1224,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.0764134333
-    y: 4140719.4217243409
-    theta: 1.339183856020238
-    kappa: -0.0023314568473221456
+    x: 586350.650946198
+    y: 4140719.26023646
+    theta: 1.3015843479819358
+    kappa: 0.0041395826010796567
     s: 19.9724130659906
     dkappa: 0
     ddkappa: 0
@@ -1238,10 +1238,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.122787384
-    y: 4140719.6185229616
-    theta: 1.3387187662603894
-    kappa: -0.0024181244893521554
+    x: 586350.70480088552
+    y: 4140719.4551208178
+    theta: 1.3024299656402321
+    kappa: 0.004059677750526426
     s: 20.174601692129837
     dkappa: 0
     ddkappa: 0
@@ -1252,10 +1252,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.1693664965
-    y: 4140719.8151657991
-    theta: 1.3382144293301041
-    kappa: -0.0024993285072697951
+    x: 586350.75818395265
+    y: 4140719.6500265533
+    theta: 1.3032271340146591
+    kappa: 0.0039717104846000483
     s: 20.376685942330841
     dkappa: 0
     ddkappa: 0
@@ -1266,10 +1266,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.21602584119
-    y: 4140720.0116888052
-    theta: 1.3376926093940302
-    kappa: -0.0025780603002809278
+    x: 586350.81144327577
+    y: 4140719.8448645133
+    theta: 1.304013334197148
+    kappa: 0.0038819998984618168
     s: 20.578672046464835
     dkappa: 0
     ddkappa: 0
@@ -1280,10 +1280,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.26266390912
-    y: 4140720.2081221952
-    theta: 1.3371710274003388
-    kappa: -0.0026567561912217612
+    x: 586350.86467831233
+    y: 4140720.0396136269
+    theta: 1.3047991757808959
+    kappa: 0.0037923302207755203
     s: 20.780566043979217
     dkappa: 0
     ddkappa: 0
@@ -1294,10 +1294,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.309369749
-    y: 4140720.4044507528
-    theta: 1.3366373996844512
-    kappa: -0.0027332555256403988
+    x: 586350.91771018959
+    y: 4140720.234328602
+    theta: 1.305561435002728
+    kappa: 0.0036998737852758706
     s: 20.98237378389755
     dkappa: 0
     ddkappa: 0
@@ -1308,10 +1308,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.35633681261
-    y: 4140720.600634173
-    theta: 1.3360648303819276
-    kappa: -0.0028028215501816924
+    x: 586350.97040058207
+    y: 4140720.4290529247
+    theta: 1.3062821202957162
+    kappa: 0.0036024387207685303
     s: 21.184100924819578
     dkappa: 0
     ddkappa: 0
@@ -1322,10 +1322,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.403286384
-    y: 4140720.7967445273
-    theta: 1.3354924741423855
-    kappa: -0.0028723616657031518
+    x: 586351.02307135065
+    y: 4140720.6237047254
+    theta: 1.3070025374557479
+    kappa: 0.0035050399447660381
     s: 21.385752934921214
     dkappa: 0
     ddkappa: 0
@@ -1336,10 +1336,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.45021969173
-    y: 4140720.9927869476
-    theta: 1.3349203163607934
-    kappa: -0.0029418776922485752
+    x: 586351.0757238738
+    y: 4140720.8182890974
+    theta: 1.3077227045902828
+    kappa: 0.0034076749080917531
     s: 21.58733509195455
     dkappa: 0
     ddkappa: 0
@@ -1350,10 +1350,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.49745577166
-    y: 4140721.1886899387
-    theta: 1.3343130386610929
-    kappa: -0.0030032048057008283
+    x: 586351.12793512829
+    y: 4140721.0129252234
+    theta: 1.3083782725221949
+    kappa: 0.0033050071381950938
     s: 21.788852483247844
     dkappa: 0
     ddkappa: 0
@@ -1364,10 +1364,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.54476627475
-    y: 4140721.3845134554
-    theta: 1.3336961160370071
-    kappa: -0.003062240972385436
+    x: 586351.18008209229
+    y: 4140721.2075166609
+    theta: 1.3090262471384781
+    kappa: 0.0032017567290901043
     s: 21.990310005705531
     dkappa: 0
     ddkappa: 0
@@ -1378,10 +1378,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.59206382371
-    y: 4140721.5802833522
-    theta: 1.3330793623610797
-    kappa: -0.0031212609740048523
+    x: 586351.23221477761
+    y: 4140721.4020548165
+    theta: 1.3096740442395889
+    kappa: 0.0030985345916309213
     s: 22.191712365808229
     dkappa: 0
     ddkappa: 0
@@ -1392,10 +1392,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.63948946411
-    y: 4140721.7759700343
-    theta: 1.3324507492255364
-    kappa: -0.0031765446436295051
+    x: 586351.28415850364
+    y: 4140721.5965909776
+    theta: 1.3102902762497786
+    kappa: 0.002993710207636942
     s: 22.39306407961271
     dkappa: 0
     ddkappa: 0
@@ -1406,10 +1406,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.68719854346
-    y: 4140721.9715402392
-    theta: 1.3317970163140003
-    kappa: -0.0032239903659630094
+    x: 586351.33585751709
+    y: 4140721.791144501
+    theta: 1.3108648018893883
+    kappa: 0.0028867547678438181
     s: 22.594369472751936
     dkappa: 0
     ddkappa: 0
@@ -1420,10 +1420,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.73489762493
-    y: 4140722.1670694612
-    theta: 1.3311434201700278
-    kappa: -0.003271426145594997
+    x: 586351.38754569646
+    y: 4140721.9856572533
+    theta: 1.3114392072468821
+    kappa: 0.002779821741578428
     s: 22.795632680435041
     dkappa: 0
     ddkappa: 0
@@ -1434,10 +1434,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.78258764348
-    y: 4140722.3625615309
-    theta: 1.330489948541447
-    kappa: -0.0033188529122729774
+    x: 586351.43922405492
+    y: 4140722.180133048
+    theta: 1.3120135032016766
+    kappa: 0.0026729090329394179
     s: 22.996857647447321
     dkappa: 0
     ddkappa: 0
@@ -1448,10 +1448,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.83065498923
-    y: 4140722.5579255866
-    theta: 1.3298119737238518
-    kappa: -0.0033555847245192293
+    x: 586351.49055021629
+    y: 4140722.3746664114
+    theta: 1.3125162159172898
+    kappa: 0.0025642337032953919
     s: 23.198048128150258
     dkappa: 0
     ddkappa: 0
@@ -1462,10 +1462,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.87877797906
-    y: 4140722.7532441611
-    theta: 1.3291300777950532
-    kappa: -0.0033905635183670429
+    x: 586351.541854752
+    y: 4140722.569173513
+    theta: 1.313015991472797
+    kappa: 0.0024555038215481706
     s: 23.399207686481503
     dkappa: 0
     ddkappa: 0
@@ -1476,10 +1476,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.9268943785
-    y: 4140722.9485359872
-    theta: 1.3284482753327294
-    kappa: -0.0034255375218592405
+    x: 586351.59315226157
+    y: 4140722.7636539768
+    theta: 1.3135156985273502
+    kappa: 0.0023467888303887155
     s: 23.600339695954879
     dkappa: 0
     ddkappa: 0
@@ -1490,10 +1490,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.975188173
-    y: 4140723.1437587808
-    theta: 1.3277587702768507
-    kappa: -0.0034551682721657186
+    x: 586351.64428746584
+    y: 4140722.9581519156
+    theta: 1.3139773738260019
+    kappa: 0.0022381221954055138
     s: 23.801447339660392
     dkappa: 0
     ddkappa: 0
@@ -1504,10 +1504,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.02375792945
-    y: 4140723.3388912156
-    theta: 1.3270573934673524
-    kappa: -0.0034766049803930076
+    x: 586351.695255613
+    y: 4140723.1526716733
+    theta: 1.3143996834135194
+    kappa: 0.0021295035423214092
     s: 24.0025336102642
     dkappa: 0
     ddkappa: 0
@@ -1518,10 +1518,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.07232320029
-    y: 4140723.5340056284
-    theta: 1.3263560811983992
-    kappa: -0.0034980397088824845
+    x: 586351.746219053
+    y: 4140723.3471734673
+    theta: 1.3148219540363728
+    kappa: 0.0020208949204628215
     s: 24.203601310008665
     dkappa: 0
     ddkappa: 0
@@ -1532,10 +1532,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.12088461651
-    y: 4140723.7291045552
-    theta: 1.3256548250499687
-    kappa: -0.003519472736065845
+    x: 586351.79717320576
+    y: 4140723.5416611927
+    theta: 1.3152426701213695
+    kappa: 0.0019123356768675276
     s: 24.404653050712295
     dkappa: 0
     ddkappa: 0
@@ -1546,10 +1546,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.16989277641
-    y: 4140723.9240777404
-    theta: 1.3249445140326295
-    kappa: -0.0035271357021148014
+    x: 586351.84787062963
+    y: 4140723.7362020118
+    theta: 1.3155898706968483
+    kappa: 0.0018057530778153898
     s: 24.605691253769788
     dkappa: 0
     ddkappa: 0
@@ -1560,10 +1560,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.218932925
-    y: 4140724.1190312654
-    theta: 1.3242335396231815
-    kappa: -0.0035337351791511237
+    x: 586351.89856520237
+    y: 4140723.9307318889
+    theta: 1.3159370518051396
+    kappa: 0.001699176473120637
     s: 24.806718150152008
     dkappa: 0
     ddkappa: 0
@@ -1574,10 +1574,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.26797081309
-    y: 4140724.313975804
-    theta: 1.3235225981253715
-    kappa: -0.0035403343519913337
+    x: 586351.94925743842
+    y: 4140724.1252528005
+    theta: 1.316284216886648
+    kappa: 0.001592604780964969
     s: 25.007735780405998
     dkappa: 0
     ddkappa: 0
@@ -1588,10 +1588,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.3172287246
-    y: 4140724.5088570584
-    theta: 1.3228120879925298
-    kappa: -0.0035396782416368838
+    x: 586351.99983445264
+    y: 4140724.3197960109
+    theta: 1.3165917814799666
+    kappa: 0.0014881489173189217
     s: 25.208745994654972
     dkappa: 0
     ddkappa: 0
@@ -1602,10 +1602,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.36674442166
-    y: 4140724.7036671913
-    theta: 1.3221020703368129
-    kappa: -0.003530545130647776
+    x: 586352.05031691166
+    y: 4140724.5143578611
+    theta: 1.3168668201244018
+    kappa: 0.0013854307521562663
     s: 25.409750452598313
     dkappa: 0
     ddkappa: 0
@@ -1616,10 +1616,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.41625906271
-    y: 4140724.8984731687
-    theta: 1.3213920676292004
-    kappa: -0.0035214122144499767
+    x: 586352.100798294
+    y: 4140724.7089155605
+    theta: 1.3171418529105638
+    kappa: 0.0012827147777702079
     s: 25.610750623511588
     dkappa: 0
     ddkappa: 0
@@ -1630,10 +1630,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.46577296255
-    y: 4140725.0932762315
-    theta: 1.3206820760681672
-    kappa: -0.0035122794349358452
+    x: 586352.15126608673
+    y: 4140724.9034736739
+    theta: 1.3174113726311703
+    kappa: 0.001180455662767076
     s: 25.811747786246528
     dkappa: 0
     ddkappa: 0
@@ -1644,10 +1644,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.51575533138
-    y: 4140725.2879576348
-    theta: 1.319984004500552
-    kappa: -0.0034865431069460804
+    x: 586352.20158149989
+    y: 4140725.0980692715
+    theta: 1.3176156886496295
+    kappa: 0.0010835865178321937
     s: 26.012743029231046
     dkappa: 0
     ddkappa: 0
@@ -1658,10 +1658,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.56574051979
-    y: 4140725.4826372638
-    theta: 1.3192860140625007
-    kappa: -0.0034606980818103943
+    x: 586352.2518966574
+    y: 4140725.2926638811
+    theta: 1.3178200036406764
+    kappa: 0.00098671786532535653
     s: 26.213737250469222
     dkappa: 0
     ddkappa: 0
@@ -1672,10 +1672,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.61572563008
-    y: 4140725.6773165883
-    theta: 1.3185880248919573
-    kappa: -0.0034348530970720412
+    x: 586352.3022117361
+    y: 4140725.4872581856
+    theta: 1.3180243183065237
+    kappa: 0.00088984936423006566
     s: 26.414731157541308
     dkappa: 0
     ddkappa: 0
@@ -1686,10 +1686,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.66595251043
-    y: 4140725.871933742
-    theta: 1.3179026014319775
-    kappa: -0.0033996552232802633
+    x: 586352.35245664453
+    y: 4140725.6818708219
+    theta: 1.3181891261608207
+    kappa: 0.00079767145382337733
     s: 26.615725267603739
     dkappa: 0
     ddkappa: 0
@@ -1700,10 +1700,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.716397746
-    y: 4140726.0664951708
-    theta: 1.3172285190187394
-    kappa: -0.0033560141544244169
+    x: 586352.40265627
+    y: 4140725.876495766
+    theta: 1.3183283744851662
+    kappa: 0.00070852802641443568
     s: 26.816719971902209
     dkappa: 0
     ddkappa: 0
@@ -1714,10 +1714,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.76684325957
-    y: 4140726.26105767
-    theta: 1.3165544327661576
-    kappa: -0.0033123728452719731
+    x: 586352.45285617188
+    y: 4140726.0711217816
+    theta: 1.3184676235769324
+    kappa: 0.00061938410816365962
     s: 27.017715782918383
     dkappa: 0
     ddkappa: 0
@@ -1728,10 +1728,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.817315239
-    y: 4140726.45561497
-    theta: 1.3158825078558956
-    kappa: -0.003267635870811178
+    x: 586352.50304656534
+    y: 4140726.2657519421
+    theta: 1.3185989187585476
+    kappa: 0.00053154019249507006
     s: 27.218713234643488
     dkappa: 0
     ddkappa: 0
@@ -1742,10 +1742,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.86821474461
-    y: 4140726.6500631082
-    theta: 1.3152460791529581
-    kappa: -0.0032049479742200489
+    x: 586352.55317497766
+    y: 4140726.4604002917
+    theta: 1.3186800722646455
+    kappa: 0.00045189424168119572
     s: 27.419712831467073
     dkappa: 0
     ddkappa: 0
@@ -1756,10 +1756,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.91911491356
-    y: 4140726.8445137804
-    theta: 1.3146096418058113
-    kappa: -0.0031422592605369064
+    x: 586352.6033040433
+    y: 4140726.6550511792
+    theta: 1.3187612268291606
+    kappa: 0.00037224725273916871
     s: 27.620715048177019
     dkappa: 0
     ddkappa: 0
@@ -1770,10 +1770,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.97001585865
-    y: 4140727.0389674176
-    theta: 1.3139731949204319
-    kappa: -0.0030795695909167642
+    x: 586352.65343387343
+    y: 4140726.8497050344
+    theta: 1.3188423826307678
+    kappa: 0.00029259904926426665
     s: 27.821720329959522
     dkappa: 0
     ddkappa: 0
@@ -1784,10 +1784,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.02115904854
-    y: 4140727.23336097
-    theta: 1.3133646761607143
-    kappa: -0.0030122150747161447
+    x: 586352.7035394609
+    y: 4140727.0443687253
+    theta: 1.3188883294114206
+    kappa: 0.00021916266620607125
     s: 28.022729092399107
     dkappa: 0
     ddkappa: 0
@@ -1798,10 +1798,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.07247135486
-    y: 4140727.4277140619
-    theta: 1.3127756073774723
-    kappa: -0.0029416105060552219
+    x: 586352.75363348727
+    y: 4140727.2390393838
+    theta: 1.318916714645072
+    kappa: 0.00014882392108122715
     s: 28.223741721478621
     dkappa: 0
     ddkappa: 0
@@ -1812,10 +1812,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.12378473929
-    y: 4140727.6220712368
-    theta: 1.31218652615951
-    kappa: -0.0028710044540816116
+    x: 586352.80372856616
+    y: 4140727.4337141332
+    theta: 1.3189451004750643
+    kappa: 7.84836982283108e-05
     s: 28.424758573579236
     dkappa: 0
     ddkappa: 0
@@ -1826,10 +1826,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.17514504527
-    y: 4140727.8164206818
-    theta: 1.3116033038284636
-    kappa: -0.0028005724712900624
+    x: 586352.85382413969
+    y: 4140727.6283934526
+    theta: 1.3189643407940432
+    kappa: 9.9526909754970419e-06
     s: 28.625779975480441
     dkappa: 0
     ddkappa: 0
@@ -1840,10 +1840,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.22683787136
-    y: 4140728.0106870048
-    theta: 1.3110625771602242
-    kappa: -0.00273141054208714
+    x: 586352.903918334
+    y: 4140727.8230781322
+    theta: 1.3189465523950961
+    kappa: -5.1248719868604709e-05
     s: 28.826806224360055
     dkappa: 0
     ddkappa: 0
@@ -1854,10 +1854,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.2785320126
-    y: 4140728.2049582712
-    theta: 1.3105218365409685
-    kappa: -0.0026622468532510289
+    x: 586352.95401380281
+    y: 4140728.0177677651
+    theta: 1.3189287635435665
+    kappa: -0.00011245168781261684
     s: 29.027837587794217
     dkappa: 0
     ddkappa: 0
@@ -1868,10 +1868,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.33022753044
-    y: 4140728.3992347103
-    theta: 1.3099810816468078
-    kappa: -0.0025930813229079778
+    x: 586353.00411060546
+    y: 4140728.2124625817
+    theta: 1.3189109742184075
+    kappa: -0.00017365628530664409
     s: 29.228874303757387
     dkappa: 0
     ddkappa: 0
@@ -1882,10 +1882,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.38215300883
-    y: 4140728.5934554921
-    theta: 1.3094715773109213
-    kappa: -0.0025249005509934033
+    x: 586353.05422746215
+    y: 4140728.4071579785
+    theta: 1.318865088242829
+    kappa: -0.00022721038365484367
     s: 29.429916580622351
     dkappa: 0
     ddkappa: 0
@@ -1896,10 +1896,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.43420118734
-    y: 4140728.7876494466
-    theta: 1.3089786420196821
-    kappa: -0.0024572411847683575
+    x: 586353.10435267049
+    y: 4140728.6018571528
+    theta: 1.3188087847504033
+    kappa: -0.000277929081689791
     s: 29.630964597160222
     dkappa: 0
     ddkappa: 0
@@ -1910,10 +1910,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.48625089042
-    y: 4140728.9818490893
-    theta: 1.3084856922707204
-    kappa: -0.0023895798367513646
+    x: 586353.15447934717
+    y: 4140728.79656203
+    theta: 1.3187524796088059
+    kappa: -0.00032864926531221792
     s: 29.832018502540429
     dkappa: 0
     ddkappa: 0
@@ -1924,10 +1924,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.53836145089
-    y: 4140729.1760385893
-    theta: 1.3080014676314808
-    kappa: -0.0023222180038636442
+    x: 586353.20462035364
+    y: 4140728.9912694204
+    theta: 1.3186879502129085
+    kappa: -0.00037614745815509032
     s: 30.033078416330728
     dkappa: 0
     ddkappa: 0
@@ -1938,10 +1938,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.59073379007
-    y: 4140729.3701640107
-    theta: 1.3075555750077019
-    kappa: -0.00225617717733464
+    x: 586353.25479910383
+    y: 4140729.1859733853
+    theta: 1.3186002072783478
+    kappa: -0.00041454742291083718
     s: 30.2341444284972
     dkappa: 0
     ddkappa: 0
@@ -1952,10 +1952,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.64310773346
-    y: 4140729.5642953785
-    theta: 1.3071096686288219
-    kappa: -0.0021901343279460118
+    x: 586353.304979391
+    y: 4140729.3806833159
+    theta: 1.3185124616555446
+    kappa: -0.00045294856387442914
     s: 30.435216599404242
     dkappa: 0
     ddkappa: 0
@@ -1966,10 +1966,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.69548328908
-    y: 4140729.7584327227
-    theta: 1.3066637486039407
-    kappa: -0.0021240894455936354
+    x: 586353.35516122286
+    y: 4140729.5753992386
+    theta: 1.3184247133324249
+    kappa: -0.00049135088692098977
     s: 30.636294959814585
     dkappa: 0
     ddkappa: 0
@@ -1980,10 +1980,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.74806277792
-    y: 4140729.9525213023
-    theta: 1.3062501257101324
-    kappa: -0.0020593311998636276
+    x: 586353.40539833624
+    y: 4140729.7701072982
+    theta: 1.3183193774310571
+    kappa: -0.00051862192922951357
     s: 30.837379510889271
     dkappa: 0
     ddkappa: 0
@@ -1994,10 +1994,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.80072335456
-    y: 4140730.146594326
-    theta: 1.3058491824704319
-    kappa: -0.0019950771902956617
+    x: 586353.45565091516
+    y: 4140729.9648177326
+    theta: 1.3182094810441045
+    kappa: -0.00054300874435476922
     s: 31.038470224187677
     dkappa: 0
     ddkappa: 0
@@ -2008,10 +2008,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.85338552971
-    y: 4140730.3406732413
-    theta: 1.3054482270579171
-    kappa: -0.001930821230273969
+    x: 586353.50590501959
+    y: 4140730.1595340776
+    theta: 1.3180995813213683
+    kappa: -0.00056739629975062825
     s: 31.2395670416675
     dkappa: 0
     ddkappa: 0
@@ -2022,10 +2022,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.906112527
-    y: 4140730.5347407567
-    theta: 1.3050582030803444
-    kappa: -0.0018670292789163343
+    x: 586353.55618617486
+    y: 4140730.3542496487
+    theta: 1.3179858633516963
+    kappa: -0.00058704455884189322
     s: 31.440669875684748
     dkappa: 0
     ddkappa: 0
@@ -2036,10 +2036,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.95903030888
-    y: 4140730.7287624851
-    theta: 1.304700910729542
-    kappa: -0.001804629551799144
+    x: 586353.60652131715
+    y: 4140730.5489573688
+    theta: 1.3178643004690158
+    kappa: -0.00059695036792820641
     s: 31.641778608993771
     dkappa: 0
     ddkappa: 0
@@ -2050,10 +2050,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.01194960426
-    y: 4140730.9227897627
-    theta: 1.304343608114793
-    kappa: -0.0017422280398218075
+    x: 586353.65685789927
+    y: 4140730.7436706577
+    theta: 1.3177427341077341
+    kappa: -0.000606856460356846
     s: 31.842893094747232
     dkappa: 0
     ddkappa: 0
@@ -2064,10 +2064,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.064870367
-    y: 4140731.1168224206
-    theta: 1.303986295640809
-    kappa: -0.0016798247977326685
+    x: 586353.7071958771
+    y: 4140730.9383893451
+    theta: 1.3176211643781011
+    kappa: -0.00061676282743664207
     s: 32.044013156496121
     dkappa: 0
     ddkappa: 0
@@ -2078,10 +2078,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.11797333218
-    y: 4140731.3108108342
-    theta: 1.3036633423354884
-    kappa: -0.0016189067987138709
+    x: 586353.75760734989
+    y: 4140731.1330945631
+    theta: 1.3174959070606673
+    kappa: -0.00062167137506366054
     s: 32.245138588189739
     dkappa: 0
     ddkappa: 0
@@ -2092,10 +2092,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.17112770141
-    y: 4140731.5047905175
-    theta: 1.3033498950855777
-    kappa: -0.0015583988690328582
+    x: 586353.80803177669
+    y: 4140731.3278017319
+    theta: 1.317370050759171
+    kappa: -0.0006257718239852404
     s: 32.446269154175731
     dkappa: 0
     ddkappa: 0
@@ -2106,10 +2106,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.22428335738
-    y: 4140731.6987748975
-    theta: 1.3030364402506811
-    kappa: -0.0014978894745549495
+    x: 586353.85845742445
+    y: 4140731.522513615
+    theta: 1.3172441914114112
+    kappa: -0.00062987237217190643
     s: 32.64740458920005
     dkappa: 0
     ddkappa: 0
@@ -2120,10 +2120,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.27750189079
-    y: 4140731.8927467563
-    theta: 1.3027359909342446
-    kappa: -0.0014380111697056032
+    x: 586353.90891834663
+    y: 4140731.7172210789
+    theta: 1.3171174603737961
+    kappa: -0.00063318360488583689
     s: 32.848544598406981
     dkappa: 0
     ddkappa: 0
@@ -2134,10 +2134,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.33085503278
-    y: 4140732.0866860645
-    theta: 1.3024637015450904
-    kappa: -0.0013795005979596706
+    x: 586353.95943268039
+    y: 4140731.911919089
+    theta: 1.3169893940337247
+    kappa: -0.000635284104381325
     s: 33.049688857339127
     dkappa: 0
     ddkappa: 0
@@ -2148,10 +2148,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.38420920807
-    y: 4140732.2806291282
-    theta: 1.3021914068655045
-    kappa: -0.0013209888930088727
+    x: 586354.00994799251
+    y: 4140732.1066208696
+    theta: 1.3168613252119044
+    kappa: -0.00063738464455828626
     s: 33.250837011937406
     dkappa: 0
     ddkappa: 0
@@ -2162,10 +2162,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.437564315
-    y: 4140732.4745755782
-    theta: 1.3019191074552277
-    kappa: -0.0012624761664557457
+    x: 586354.06046418659
+    y: 4140732.3013260495
+    theta: 1.3167332541568089
+    kappa: -0.00063948522141024677
     s: 33.451988678541078
     dkappa: 0
     ddkappa: 0
@@ -2176,10 +2176,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.49106274673
-    y: 4140732.6684857472
-    theta: 1.301680415579475
-    kappa: -0.0012056846357081982
+    x: 586354.11106099479
+    y: 4140732.4960134961
+    theta: 1.3166045093049423
+    kappa: -0.000639664891285244
     s: 33.653143443887714
     dkappa: 0
     ddkappa: 0
@@ -2190,10 +2190,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.54458725115
-    y: 4140732.8623914858
-    theta: 1.3014477036229062
-    kappa: -0.001149198899073587
+    x: 586354.1616645914
+    y: 4140732.6907019252
+    theta: 1.3164757112471694
+    kappa: -0.0006396973025984392
     s: 33.85430086511321
     dkappa: 0
     ddkappa: 0
@@ -2204,10 +2204,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.59811233659
-    y: 4140733.0562993297
-    theta: 1.3012149891433304
-    kappa: -0.0010927125493286295
+    x: 586354.21226873726
+    y: 4140732.8853924666
+    theta: 1.3163469117922277
+    kappa: -0.000639729714263435
     s: 34.055460469751786
     dkappa: 0
     ddkappa: 0
@@ -2218,10 +2218,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.65169303725
-    y: 4140733.2501935433
-    theta: 1.3009971735003829
-    kappa: -0.0010370168487079483
+    x: 586354.26291344641
+    y: 4140733.080074192
+    theta: 1.316218450598662
+    kappa: -0.00063873481552530827
     s: 34.256621755735992
     dkappa: 0
     ddkappa: 0
@@ -2232,10 +2232,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.7053633756
-    y: 4140733.4440641711
-    theta: 1.3008034848737022
-    kappa: -0.00098260186424943227
+    x: 586354.31360497919
+    y: 4140733.2747449251
+    theta: 1.3160903820540866
+    kappa: -0.00063654896181969834
     s: 34.457784191396684
     dkappa: 0
     ddkappa: 0
@@ -2246,10 +2246,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.75903387088
-    y: 4140733.6379353656
-    theta: 1.3006097956753642
-    kappa: -0.00092818672062558276
+    x: 586354.36429666029
+    y: 4140733.4694162263
+    theta: 1.3159623131337741
+    kappa: -0.000634363101720406
     s: 34.658947215463058
     dkappa: 0
     ddkappa: 0
diff --git a/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt b/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
index aa3a58f412664d3fb5a318ce882d1f1e8e63c736..749c21d9c0de0cb4ce41f1e4837449e9725fcf5a 100755
--- a/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
@@ -14,2248 +14,2248 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4250001907348633
-  a: -0.0767818497207001
+  v: 4.4250001907348571
+  a: -0.076781849720700165
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586361.43752291636
-    y: 4140761.9349190667
-    theta: 1.2881331999677885
-    kappa: 0.0073313932078633327
-    s: 0.22114690321213826
+    x: 586361.43756566069
+    y: 4140761.934906621
+    theta: 1.2876109193745362
+    kappa: 0.0033564346763362181
+    s: 0.22114690823964875
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4207338835268155
-  a: -0.093828356582949968
+  v: 4.4207341847631518
+  a: -0.0938163402784509
   relative_time: 0.05
 }
 trajectory_point {
   path_point {
-    x: 586361.49893662625
-    y: 4140762.1471232539
-    theta: 1.2898986957449672
-    kappa: 0.0084602975550961239
-    s: 0.442059288135342
+    x: 586361.49922638072
+    y: 4140762.147039243
+    theta: 1.2881050850574178
+    kappa: 0.0013900793940770637
+    s: 0.44205932818968874
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4156215632783944
-  a: -0.11062237133563513
+  v: 4.4156227615939176
+  a: -0.11059853762185216
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586361.55988293316
-    y: 4140762.3591744103
-    theta: 1.2918561334320762
-    kappa: 0.0091782788321493839
-    s: 0.66269516973272957
+    x: 586361.56075214571
+    y: 4140762.3589232061
+    theta: 1.2882213359649197
+    kappa: -0.00018020452788121817
+    s: 0.6626953043567605
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4096758545950783
-  a: -0.12716389397875552
+  v: 4.4096785358878723
+  a: -0.127128441750904
   relative_time: 0.15000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586361.62032180815
-    y: 4140762.5710401889
-    theta: 1.2939227717278576
-    kappa: 0.0095251987807698085
-    s: 0.88301319419769286
+    x: 586361.62219919858
+    y: 4140762.5704991305
+    theta: 1.2880401317930179
+    kappa: -0.0013872092884209552
+    s: 0.88301351198054112
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4029093820823446
-  a: -0.14345292451231118
+  v: 4.4029141223057335
+  a: -0.14340605266560635
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 586361.68022944662
-    y: 4140762.7826841427
-    theta: 1.2960244321109304
-    kappa: 0.0095408843689341535
-    s: 1.1029726389538983
+    x: 586361.68360788689
+    y: 4140762.7817129074
+    theta: 1.287635380968581
+    kappa: -0.0022638076996076935
+    s: 1.102973257033744
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3953347703456727
-  a: -0.15948946293630212
+  v: 4.3953421355082174
+  a: -0.15943137036595928
   relative_time: 0.25
 }
 trajectory_point {
   path_point {
-    x: 586361.73959616432
-    y: 4140762.9940664284
-    theta: 1.2980954591817198
-    kappa: 0.0092649885722891068
-    s: 1.3225334126552857
+    x: 586361.74500499817
+    y: 4140762.9925149931
+    theta: 1.2870738931955072
+    kappa: -0.0028427270577756837
+    s: 1.3225344762221183
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3869646439905408
-  a: -0.17527350925072832
+  v: 4.3869751901560434
+  a: -0.17520439485196271
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586361.79842415557
-    y: 4140763.2051445446
-    theta: 1.3000786450699862
-    kappa: 0.008736835700317979
-    s: 1.5416560551860692
+    x: 586361.806405512
+    y: 4140763.2028598916
+    theta: 1.28641509921509
+    kappa: -0.00315642142269534
+    s: 1.5416577369844491
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3778116276224264
-  a: -0.19080506345558979
+  v: 4.3778259009099276
+  a: -0.19072512612361675
   relative_time: 0.35
 }
 trajectory_point {
   path_point {
-    x: 586361.85672248562
-    y: 4140763.4158747969
-    theta: 1.301923038179166
-    kappa: 0.0079933911265053486
-    s: 1.7603017376607368
+    x: 586361.8678115767
+    y: 4140763.4127064724
+    theta: 1.2857123326706406
+    kappa: -0.0032372734598599992
+    s: 1.7603042374925573
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3678883458468079
-  a: -0.20608412555088654
+  v: 4.3679068824305869
+  a: -0.2059935641809213
   relative_time: 0.39999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586361.91446588282
-    y: 4140763.6262236116
-    theta: 1.3035524098597107
-    kappa: 0.0070552478189620143
-    s: 1.9784322624240493
+    x: 586361.92924316623
+    y: 4140763.6220089835
+    theta: 1.2850204003333847
+    kappa: -0.0030884331723206875
+    s: 1.9784358066512993
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3572074232691635
-  a: -0.22111069553661852
+  v: 4.3572307493787408
+  a: -0.2210097090238764
   relative_time: 0.44999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586361.97175478342
-    y: 4140763.8361237347
-    theta: 1.3049621198206593
-    kappa: 0.0059962283942610259
-    s: 2.1960100630510437
+    x: 586361.99065396632
+    y: 4140763.8307417645
+    theta: 1.2843825607280857
+    kappa: -0.0027827270043081164
+    s: 2.1960149040985679
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3457814844949718
-  a: -0.23588477341278577
+  v: 4.3458101164151053
+  a: -0.23577356065248206
   relative_time: 0.49999999999999994
 }
 trajectory_point {
   path_point {
-    x: 586362.0286273628
-    y: 4140764.0455260808
-    theta: 1.3061332952708207
-    kappa: 0.0048538424333133424
-    s: 2.4129982043470286
+    x: 586362.0520195195
+    y: 4140764.0388734564
+    theta: 1.2838266177922453
+    kappa: -0.0023511281941632958
+    s: 2.4130046202052919
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.333623154129711
-  a: -0.2504063591793883
+  v: 4.3336575982003982
+  a: -0.25028511906673828
   relative_time: 0.54999999999999993
 }
 trajectory_point {
   path_point {
-    x: 586362.08512403537
-    y: 4140764.2543817833
-    theta: 1.3070546576438773
-    kappa: 0.0036647915369347828
-    s: 2.6293603823475884
+    x: 586362.11330972821
+    y: 4140764.2463750509
+    theta: 1.2833742744252814
+    kappa: -0.0018242101547946603
+    s: 2.6293686760754356
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3207450567788594
-  a: -0.26467545283642613
+  v: 4.3207858093953373
+  a: -0.264544384266645
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 586362.14128624741
-    y: 4140764.4626424946
-    theta: 1.3077222510208144
-    kappa: 0.0024649160445657896
-    s: 2.8450609243185796
+    x: 586362.17449198873
+    y: 4140764.4532189649
+    theta: 1.2830410104322589
+    kappa: -0.0012317946859723636
+    s: 2.8450714235459991
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3071598170478946
-  a: -0.27869205438389921
+  v: 4.3072073646606395
+  a: -0.27855135625220234
   relative_time: 0.65
 }
 trajectory_point {
   path_point {
-    x: 586362.19715507363
-    y: 4140764.6702607386
-    theta: 1.3081390158134367
-    kappa: 0.001289089498439006
-    s: 3.0600647887561347
+    x: 586362.23553228087
+    y: 4140764.6593787181
+    theta: 1.2828363348135126
+    kappa: -0.000602899393202399
+    s: 3.0600778451870183
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2928800595422958
-  a: -0.2924561638218075
+  v: 4.2929348786570234
+  a: -0.29230603502341018
   relative_time: 0.70000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586362.25276974228
-    y: 4140764.8771902709
-    theta: 1.3083142466401285
-    kappa: 0.0001710788736698143
-    s: 3.2743375653866571
+    x: 586362.29639510065
+    y: 4140764.8648289475
+    theta: 1.2827642621417157
+    kappa: 3.4138611338787675e-05
+    s: 3.2743535543015647
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.27791840886754
-  a: -0.30596778115015111
+  v: 4.2779809660452042
+  a: -0.30580842058026858
   relative_time: 0.75000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586362.3081763899
-    y: 4140765.0833837017
-    theta: 1.3082233445839269
-    kappa: -0.00082710809408848593
-    s: 3.4878454751668273
+    x: 586362.35703998746
+    y: 4140765.06954643
+    theta: 1.2828458023107967
+    kappa: 0.00064112187165128547
+    s: 3.4878647949257475
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2622874896291068
-  a: -0.31922690636893003
+  v: 4.2623582414859023
+  a: -0.31905851292277754
   relative_time: 0.80000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586362.3634177997
-    y: 4140765.2887952048
-    theta: 1.3079424595727616
-    kappa: -0.0016833397044474058
-    s: 3.7005553702835985
+    x: 586362.41742687463
+    y: 4140765.2735084835
+    theta: 1.2830488995824705
+    kappa: 0.0011863712610251222
+    s: 3.7005784418287098
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2459999264324733
-  a: -0.33223353947814416
+  v: 4.2460793196398328
+  a: -0.33205631205093705
   relative_time: 0.8500000000000002
 }
 trajectory_point {
   path_point {
-    x: 586362.41851703718
-    y: 4140765.4933848828
-    theta: 1.3075068179867926
-    kappa: -0.0023673176959806423
-    s: 3.9124347341541967
+    x: 586362.47752634343
+    y: 4140765.476689917
+    theta: 1.2833532337163822
+    kappa: 0.0016445528749458485
+    s: 3.91246200051263
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2290683438831183
-  a: -0.34498768047779349
+  v: 4.2291568151677144
+  a: -0.34480181796474707
   relative_time: 0.90000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586362.47348979523
-    y: 4140765.6971154478
-    theta: 1.3069544951117174
-    kappa: -0.0028495920300681465
-    s: 4.1234516814261228
+    x: 586362.53731359867
+    y: 4140765.6790648173
+    theta: 1.2837380771008933
+    kappa: 0.0019908172125837125
+    s: 4.1234836072127257
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.21150536658652
-  a: -0.35748932936787814
+  v: 4.2116033427302639
+  a: -0.3572950306642077
   relative_time: 0.95000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586362.52834448265
-    y: 4140765.8999522035
-    theta: 1.3063278137841292
-    kappa: -0.0031020677815915922
-    s: 4.3335749579771523
+    x: 586362.59676796873
+    y: 4140765.880606696
+    theta: 1.2841788529490292
+    kappa: 0.0022013098114777056
+    s: 4.3336120288972459
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1933236191481562
-  a: -0.3697384861483981
+  v: 4.1934315169881993
+  a: -0.36953595014931878
   relative_time: 1.0000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586362.583082311
-    y: 4140766.1018630341
-    theta: 1.305672895975442
-    kappa: -0.0030980113622711691
-    s: 4.5427739409153336
+    x: 586362.6558730636
+    y: 4140766.0812884485
+    theta: 1.2846476440277768
+    kappa: 0.0022532230419926255
+    s: 4.5428166632674811
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1745357261735059
-  a: -0.38173515081935327
+  v: 4.1746539526022373
+  a: -0.38152457642008047
   relative_time: 1.0500000000000003
 }
 trajectory_point {
   path_point {
-    x: 586362.63771830115
-    y: 4140766.3028126862
-    theta: 1.3050598052079208
-    kappa: -0.002746861404499837
-    s: 4.75101863857899
+    x: 586362.71460538206
+    y: 4140766.2810857007
+    theta: 1.2851044199462185
+    kappa: 0.0020897084991382971
+    s: 4.7510675387577512
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1551543122680474
-  a: -0.39347932338074376
+  v: 4.1552832642330966
+  a: -0.39326090947649273
   relative_time: 1.1000000000000003
 }
 trajectory_point {
   path_point {
-    x: 586362.69220839685
-    y: 4140766.5027826102
-    theta: 1.3045670101269615
-    kappa: -0.0020502391606567324
-    s: 4.9582796905367168
+    x: 586362.77297105081
+    y: 4140766.4799660118
+    theta: 1.2854939874039808
+    kappa: 0.0016902978833551211
+    s: 4.9583353145354163
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1351920020372575
-  a: -0.40497100383256945
+  v: 4.1353320665414941
+  a: -0.40474494931855548
   relative_time: 1.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586362.746508629
-    y: 4140766.7017549695
-    theta: 1.3042318262078385
-    kappa: -0.0012436618762333097
-    s: 5.1645283675873852
+    x: 586362.83098884858
+    y: 4140766.6778939213
+    theta: 1.2857909856049659
+    kappa: 0.0012217963256004483
+    s: 5.164591280500872
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1146614200866161
-  a: -0.41621019217483046
+  v: 4.1148129741881467
+  a: -0.41597669594626879
   relative_time: 1.2000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586362.800583117
-    y: 4140766.89971041
-    theta: 1.3040528295101341
-    kappa: -0.00048389494434102287
-    s: 5.3697365717601393
+    x: 586362.88866728719
+    y: 4140766.8748376574
+    theta: 1.2859973058405052
+    kappa: 0.00078653383370611148
+    s: 5.3698073572875469
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0935751910216007
-  a: -0.42719688840752668
+  v: 4.0937386018337731
+  a: -0.42695614935963278
   relative_time: 1.2500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586362.85439845221
-    y: 4140767.096629574
-    theta: 1.304010295594205
-    kappa: 0.00013823001064569535
-    s: 5.5738768363143967
+    x: 586362.94601363782
+    y: 4140767.0707664806
+    theta: 1.2861257001300894
+    kappa: 0.00043958261687592382
+    s: 5.5739560962619112
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.07194593944769
-  a: -0.43793109253065821
+  v: 4.0721215641390893
+  a: -0.4376833095586472
   relative_time: 1.3000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586362.90792382916
-    y: 4140767.2924930593
-    theta: 1.304085709460338
-    kappa: 0.00062398156804605932
-    s: 5.776922325739851
+    x: 586363.0030333295
+    y: 4140767.2656508526
+    theta: 1.2861911787284455
+    kappa: 0.00017755304009624468
+    s: 5.7770106795234639
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0497862899703607
-  a: -0.44841280454422494
+  v: 4.049974475764814
+  a: -0.44815817654331214
   relative_time: 1.3500000000000005
 }
 trajectory_point {
   path_point {
-    x: 586362.96112409758
-    y: 4140767.4872833034
-    theta: 1.3042570693892972
-    kappa: 0.00097408081734879129
-    s: 5.9788468357564675
+    x: 586363.059729787
+    y: 4140767.4594624857
+    theta: 1.2862038133433586
+    kappa: 6.4898799724610808e-06
+    s: 5.9789449199047446
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0271088671950928
-  a: -0.458642024448227
+  v: 4.0273099513716639
+  a: -0.45838075031362768
   relative_time: 1.4000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586363.0139847548
-    y: 4140767.6809777496
-    theta: 1.3044814815245585
-    kappa: 0.0012264532789577705
-    s: 6.1796247933144866
+    x: 586363.11610534007
+    y: 4140767.6521740705
+    theta: 1.2861924732796495
+    kappa: -0.00010087157010759621
+    s: 6.1797332609713269
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0039262957273642
-  a: -0.46861875224266436
+  v: 4.004140605620357
+  a: -0.46835103086959373
   relative_time: 1.4500000000000006
 }
 trajectory_point {
   path_point {
-    x: 586363.06649297627
-    y: 4140767.8735540351
-    theta: 1.3047429917058624
-    kappa: 0.0013912747163007237
-    s: 6.379231256594422
+    x: 586363.172155732
+    y: 4140767.8437608816
+    theta: 1.2861668777642832
+    kappa: -0.00015117872019373616
+    s: 6.3793507770218243
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9802512001726527
-  a: -0.47834298792753693
+  v: 3.9804790531716105
+  a: -0.47806901821121034
   relative_time: 1.5000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586363.11863826879
-    y: 4140768.0649898127
-    theta: 1.3050275253602877
-    kappa: 0.0014781976494672208
-    s: 6.57764191500706
+    x: 586363.22787557635
+    y: 4140768.0341991824
+    theta: 1.2861359276808191
+    kappa: -0.00015057303387373336
+    s: 6.5777731730878779
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9560962051364363
-  a: -0.48781473150284482
+  v: 3.9563379086861419
+  a: -0.4875347123384775
   relative_time: 1.5500000000000007
 }
 trajectory_point {
   path_point {
-    x: 586363.17040405469
-    y: 4140768.2552650417
-    theta: 1.3053215968955749
-    kappa: 0.0014873699812229405
-    s: 6.7748330891934643
+    x: 586363.28325886431
+    y: 4140768.2234660727
+    theta: 1.2861096217915158
+    kappa: -0.00010064569538150473
+    s: 6.7749767849341715
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9314739352241945
-  a: -0.49703398296858781
+  v: 3.9317297868246688
+  a: -0.49674811325139523
   relative_time: 1.6000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586363.22178145056
-    y: 4140768.4443582278
-    theta: 1.305607237791683
-    kappa: 0.001430169655426681
-    s: 6.9707817310249709
+    x: 586363.33829763345
+    y: 4140768.4115398847
+    theta: 1.2861007233691188
+    kappa: -3.9362773912860284e-06
+    s: 6.9709385790584228
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9063970150414038
-  a: -0.50600074232476622
+  v: 3.9066673022479081
+  a: -0.5057092209499634
   relative_time: 1.6500000000000008
 }
 trajectory_point {
   path_point {
-    x: 586363.27277959744
-    y: 4140768.6322436393
-    theta: 1.3058757381881805
-    kappa: 0.0013294470947460978
-    s: 7.1654654236031892
+    x: 586363.3929804205
+    y: 4140768.5984006296
+    theta: 1.2861129537598355
+    kappa: 0.00012226914154934903
+    s: 7.1656361526913859
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8808780691935434
-  a: -0.51471500957137994
+  v: 3.8811630696165782
+  a: -0.5144180354341823
   relative_time: 1.7000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586363.32339684991
-    y: 4140768.8188991407
-    theta: 1.3061217126947726
-    kappa: 0.001192881895618001
-    s: 7.35886238126
+    x: 586363.44729788508
+    y: 4140768.7840283555
+    theta: 1.2861495998073205
+    kappa: 0.00027305327781371522
+    s: 7.3590477337968494
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8549297222860917
-  a: -0.52317678470842877
+  v: 3.8552297035913958
+  a: -0.52287455670405159
   relative_time: 1.7500000000000009
 }
 trajectory_point {
   path_point {
-    x: 586363.37363126478
-    y: 4140769.004303331
-    theta: 1.3063401378619839
-    kappa: 0.0010272046925583376
-    s: 7.5509514495575623
+    x: 586363.50124098
+    y: 4140768.9684036826
+    theta: 1.2862132455514164
+    kappa: 0.00044384863392859657
+    s: 7.55115218107164
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8285645989245269
-  a: -0.531386067735913
+  v: 3.8288798188330788
+  a: -0.53107878475957149
   relative_time: 1.8000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586363.42347111413
-    y: 4140769.188438111
-    theta: 1.3065126774350215
-    kappa: 0.00083614500403056337
-    s: 7.7417121052883076
+    x: 586363.55479183351
+    y: 4140769.1515104673
+    theta: 1.2863200381191331
+    kappa: 0.00063376172116331423
+    s: 7.7419289839456189
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8017953237143267
-  a: -0.53934285865383236
+  v: 3.8021260300023445
+  a: -0.53903071960074189
   relative_time: 1.850000000000001
 }
 trajectory_point {
   path_point {
-    x: 586363.47293491173
-    y: 4140769.3712778529
-    theta: 1.3066502025623874
-    kappa: 0.0006378038146698968
-    s: 7.9311244564749428
+    x: 586363.60794166243
+    y: 4140769.3333305623
+    theta: 1.2864614378028252
+    kappa: 0.00083059731794611678
+    s: 7.93135826258168
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7746345212609693
-  a: -0.547047157462187
+  v: 3.77498095175991
+  a: -0.54673036122756291
   relative_time: 1.900000000000001
 }
 trajectory_point {
   path_point {
-    x: 586363.52202209993
-    y: 4140769.5528027532
-    theta: 1.3067530665929521
-    kappa: 0.00043408487391817768
-    s: 8.1191692423704449
+    x: 586363.6606826873
+    y: 4140769.5138461841
+    theta: 1.2866348761104023
+    kappa: 0.0010299241902148196
+    s: 8.1194207678757575
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.747094816169934
-  a: -0.5544989641609771
+  v: 3.7474571987664937
+  a: -0.55417770964003443
   relative_time: 1.9500000000000011
 }
 trajectory_point {
   path_point {
-    x: 586363.570731605
-    y: 4140769.7329938007
-    theta: 1.3068209490795941
-    kappa: 0.000226522520824682
-    s: 8.3058278334580677
+    x: 586363.71300691238
+    y: 4140769.6930402694
+    theta: 1.2868395103329038
+    kappa: 0.0012285450096941311
+    s: 8.3060978814568269
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7191888330466978
-  a: -0.56169827875020117
+  v: 3.7195673856828129
+  a: -0.56137276483815624
   relative_time: 2.0000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586363.619065893
-    y: 4140769.911831703
-    theta: 1.3068384939375841
-    kappa: 3.5909246447128294e-05
-    s: 8.4910822497257783
+    x: 586363.7648843748
+    y: 4140769.8709028349
+    theta: 1.2870902829315221
+    kappa: 0.0014187541063420787
+    s: 8.4913716339661462
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.690930243501616
-  a: -0.56860717796687865
+  v: 3.6913251745746982
+  a: -0.56827757867433859
   relative_time: 2.0500000000000007
 }
 trajectory_point {
   path_point {
-    x: 586363.66702913051
-    y: 4140770.0892974176
-    theta: 1.3068279849265698
-    kappa: -0.00013313814864644011
-    s: 8.67491519570493
+    x: 586363.8163205802
+    y: 4140770.0474142525
+    theta: 1.2873694134572162
+    kappa: 0.0015959335395739432
+    s: 8.6752247401547944
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.66233192775863
-  a: -0.57528732666637716
+  v: 3.6627434378965877
+  a: -0.57495378763800442
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586363.71462024807
-    y: 4140770.2653739341
-    theta: 1.3067928378337892
-    kappa: -0.00027742444290597434
-    s: 8.8573099710237724
+    x: 586363.86731050326
+    y: 4140770.2225585906
+    theta: 1.2876731335407938
+    kappa: 0.0017585318043743968
+    s: 8.8576405095003636
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.6334053233435979
-  a: -0.58173872484869682
+  v: 3.6338336058921068
+  a: -0.58140139172915384
   relative_time: 2.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586363.76184138178
-    y: 4140770.4400439672
-    theta: 1.306732594902756
-    kappa: -0.00038581408528009972
-    s: 9.03825044718685
+    x: 586363.91784082609
+    y: 4140770.3963229288
+    theta: 1.2880027263633518
+    kappa: 0.0019024453278331675
+    s: 9.0386028229926261
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.6041618677823797
-  a: -0.58796137251383751
+  v: 3.6046071088048812
+  a: -0.587620390947787
   relative_time: 2.2
 }
 trajectory_point {
   path_point {
-    x: 586363.80869183992
-    y: 4140770.6132915877
-    theta: 1.306656162355099
-    kappa: -0.00044177616085542366
-    s: 9.2177210675750043
+    x: 586363.96788778785
+    y: 4140770.5686979555
+    theta: 1.2883572075804741
+    kappa: 0.0020130500167068128
+    s: 9.2180961331335318
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5746129986008341
-  a: -0.59395526966179923
+  v: 3.5750753768785368
+  a: -0.59361078529390354
   relative_time: 2.25
 }
 trajectory_point {
   path_point {
-    x: 586363.85516666551
-    y: 4140770.7851026123
-    theta: 1.3065762122160585
-    kappa: -0.00045538938198517954
-    s: 9.3957068474453589
+    x: 586364.01746682893
+    y: 4140770.73966356
+    theta: 1.2887234252026594
+    kappa: 0.002097969031648713
+    s: 9.39610546393722
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5447701533248197
-  a: -0.5997204162925821
+  v: 3.5452498403567
+  a: -0.59937257476750383
   relative_time: 2.3
 }
 trajectory_point {
   path_point {
-    x: 586363.90126240125
-    y: 4140770.9554630457
-    theta: 1.3064957268485213
-    kappa: -0.00042604773002034374
-    s: 9.5721933739313414
+    x: 586364.0665757301
+    y: 4140770.9092053827
+    theta: 1.2890986769674253
+    kappa: 0.0021566464358119958
+    s: 9.5726164109300029
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5146447694801957
-  a: -0.605256812406186
+  v: 3.5151419294829953
+  a: -0.60490575936858759
   relative_time: 2.3499999999999996
 }
 trajectory_point {
   path_point {
-    x: 586363.94697837834
-    y: 4140771.1243587276
-    theta: 1.306433137161306
-    kappa: -0.00031218637406521294
-    s: 9.7471668060426655
+    x: 586364.11518684425
+    y: 4140771.077316998
+    theta: 1.2894770222853735
+    kappa: 0.0021679284907204486
+    s: 9.74761514115038
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.4842482845928213
-  a: -0.61056445800261094
+  v: 3.48476307450105
+  a: -0.61021033909715494
   relative_time: 2.3999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586363.99230277573
-    y: 4140771.2917791107
-    theta: 1.3063947469027619
-    kappa: -0.00014195511124535796
-    s: 9.92061387466534
+    x: 586364.16331009311
+    y: 4140771.24398169
+    theta: 1.2898511233783314
+    kappa: 0.0021412009472830656
+    s: 9.92108839314903
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.4535921361885547
-  a: -0.615643353081857
+  v: 3.4541247056544888
+  a: -0.615286313953206
   relative_time: 2.4499999999999993
 }
 trajectory_point {
   path_point {
-    x: 586364.0372297148
-    y: 4140771.4577126317
-    theta: 1.3063804403928727
-    kappa: 6.7897997639673228e-05
-    s: 10.09252188256167
+    x: 586364.210957332
+    y: 4140771.4091828181
+    theta: 1.2902183538276415
+    kappa: 0.0020977112988990452
+    s: 10.093023476988812
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.4226877617932554
-  a: -0.62049349764392414
+  v: 3.4232382531869381
+  a: -0.62013368393674051
   relative_time: 2.4999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586364.08174859046
-    y: 4140771.6221495997
-    theta: 1.3064152822301871
-    kappa: 0.0002798530384366556
-    s: 10.262878704370245
+    x: 586364.25811106793
+    y: 4140771.5729127917
+    theta: 1.2905703017273373
+    kappa: 0.0020642752558103768
+    s: 10.263408274244767
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.391546598932782
-  a: -0.6251148916888124
+  v: 3.3921151473420235
+  a: -0.62475244904775873
   relative_time: 2.5499999999999989
 }
 trajectory_point {
   path_point {
-    x: 586364.12585058622
-    y: 4140771.7850804315
-    theta: 1.306479903978224
-    kappa: 0.00045946910021927651
-    s: 10.431672786605958
+    x: 586364.30476669234
+    y: 4140771.7351608858
+    theta: 1.2909195812358947
+    kappa: 0.0020677343401555555
+    s: 10.432231238004118
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3601800851329937
-  a: -0.6295075352165217
+  v: 3.3607668183633708
+  a: -0.62914260928626042
   relative_time: 2.5999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586364.16953316377
-    y: 4140771.9464944382
-    theta: 1.306565493188971
-    kappa: 0.00061465952838559768
-    s: 10.598893147659986
+    x: 586364.350945104
+    y: 4140771.8959096572
+    theta: 1.2912675380255245
+    kappa: 0.0020948566934078571
+    s: 10.599481392866267
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.32859965791975
-  a: -0.633671428227052
+  v: 3.3292046964946063
+  a: -0.63330416465224582
   relative_time: 2.6499999999999986
 }
 trajectory_point {
   path_point {
-    x: 586364.21278086281
-    y: 4140772.1063850173
-    theta: 1.3066801549393607
-    kappa: 0.00073252119777518824
-    s: 10.764529377799803
+    x: 586364.39662688831
+    y: 4140772.0551538416
+    theta: 1.2916178999657446
+    kappa: 0.0021549674213399176
+    s: 10.765148334942802
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2968167548189085
-  a: -0.6376065707204035
+  v: 3.2974402119793553
+  a: -0.63723711514571468
   relative_time: 2.6999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586364.25559352059
-    y: 4140772.2647420177
-    theta: 1.30680900997105
-    kappa: 0.00082614737403036327
-    s: 10.92857163916918
+    x: 586364.44180381834
+    y: 4140772.2128855344
+    theta: 1.2919805492380454
+    kappa: 0.0022417882091283169
+    s: 10.929222231857487
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2648428133563292
-  a: -0.64131296269657589
+  v: 3.2654847950612433
+  a: -0.64094146076666725
   relative_time: 2.7499999999999982
 }
 trajectory_point {
   path_point {
-    x: 586364.2979736448
-    y: 4140772.4215551629
-    theta: 1.3069470301278283
-    kappa: 0.00090242807074392768
-    s: 11.091010665788172
+    x: 586364.48649847764
+    y: 4140772.36908862
+    theta: 1.2923505221788683
+    kappa: 0.0023441859029388651
+    s: 11.091693822746272
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.232689271057871
-  a: -0.64479060415556955
+  v: 3.2333498759838974
+  a: -0.64441720151510329
   relative_time: 2.799999999999998
 }
 trajectory_point {
   path_point {
-    x: 586364.339900892
-    y: 4140772.5768209388
-    theta: 1.307097986568357
-    kappa: 0.00094835087907418143
-    s: 11.251837763553132
+    x: 586364.53067927773
+    y: 4140772.5237630783
+    theta: 1.2927387146106379
+    kappa: 0.0024718032969795016
+    s: 11.252554418257283
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2003675654493922
-  a: -0.64803949509738412
+  v: 3.2010468849909421
+  a: -0.64766433739102292
   relative_time: 2.8499999999999979
 }
 trajectory_point {
   path_point {
-    x: 586364.38138664258
-    y: 4140772.7305278545
-    theta: 1.3072514453003787
-    kappa: 0.00097979196893223418
-    s: 11.411044810236707
+    x: 586364.57435382239
+    y: 4140772.6768982522
+    theta: 1.2931452295039145
+    kappa: 0.0026143439316726009
+    s: 11.411795900550832
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1678891340567521
-  a: -0.65105963552202
+  v: 3.1685872523260041
+  a: -0.65068286839442624
   relative_time: 2.8999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586364.42242997175
-    y: 4140772.8826683373
-    theta: 1.3074071800350555
-    kappa: 0.00099751306799638435
-    s: 11.568624255487833
+    x: 586364.61754093168
+    y: 4140772.8284809212
+    theta: 1.293562560966675
+    kappa: 0.0027661812803834009
+    s: 11.569410723299409
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.13526541440581
-  a: -0.65385102542947671
+  v: 3.1359824082327084
+  a: -0.653472794525313
   relative_time: 2.9499999999999975
 }
 trajectory_point {
   path_point {
-    x: 586364.46301355632
-    y: 4140773.0332398275
-    theta: 1.3075625878566
-    kappa: 0.00099415848988896813
-    s: 11.724569120831745
+    x: 586364.66018108092
+    y: 4140772.9785207449
+    theta: 1.2940142219853976
+    kappa: 0.0029367007745308548
+    s: 11.725391911687689
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1025078440224245
-  a: -0.65641366481975461
+  v: 3.1032437829546815
+  a: -0.65603411578368354
   relative_time: 2.9999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586364.50315582042
-    y: 4140773.182230718
-    theta: 1.3077154037261765
-    kappa: 0.000983501656151256
-    s: 11.878872999669966
+    x: 586364.70232665981
+    y: 4140773.1269961223
+    theta: 1.2944792004535359
+    kappa: 0.0031116371461155441
+    s: 11.879733062412525
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.0696278604324547
-  a: -0.65874755369285354
+  v: 3.070382806735549
+  a: -0.65836683216953751
   relative_time: 3.0499999999999972
 }
 trajectory_point {
   path_point {
-    x: 586364.5428430856
-    y: 4140773.3296386488
-    theta: 1.3078640282306986
-    kappa: 0.00096002998773811259
-    s: 12.03153005728031
+    x: 586364.74395487038
+    y: 4140773.2739074593
+    theta: 1.2949657645037644
+    kappa: 0.0032938317312884236
+    s: 12.032428343682952
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.0366369011617591
-  a: -0.6608526920487735
+  v: 3.0374109098189361
+  a: -0.66047094368287507
   relative_time: 3.099999999999997
 }
 trajectory_point {
   path_point {
-    x: 586364.58207928634
-    y: 4140773.4754571058
-    theta: 1.3080060332190957
-    kappa: 0.00092777630140814026
-    s: 12.182535030816894
+    x: 586364.78504038812
+    y: 4140773.4192563947
+    theta: 1.2954823033504042
+    kappa: 0.0034790447795599562
+    s: 12.183472495220187
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.003546403736197
-  a: -0.66272907988751462
+  v: 3.00433952244847
+  a: -0.66234645032369632
   relative_time: 3.1499999999999968
 }
 trajectory_point {
   path_point {
-    x: 586364.6208731815
-    y: 4140773.6196788731
-    theta: 1.3081437331284527
-    kappa: 0.00089092297542352531
-    s: 12.331883229310113
+    x: 586364.8256353949
+    y: 4140773.5630232827
+    theta: 1.2960081003268331
+    kappa: 0.0036632487994297446
+    s: 12.332860828257626
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9703678056816276
-  a: -0.66437671720907687
+  v: 2.9711800748677755
+  a: -0.66399335209200117
   relative_time: 3.1999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586364.65920325974
-    y: 4140773.7623054655
-    theta: 1.3082698819220397
-    kappa: 0.00084499209282655583
-    s: 12.479570533666669
+    x: 586364.865662092
+    y: 4140773.7052257541
+    theta: 1.296568932480759
+    kappa: 0.0038394847128955896
+    s: 12.480589225540854
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9371125445239095
-  a: -0.66579560401346
+  v: 2.9379439973204788
+  a: -0.66541164898778948
   relative_time: 3.2499999999999964
 }
 trajectory_point {
   path_point {
-    x: 586364.69709348457
-    y: 4140773.9033267656
-    theta: 1.308391115284341
-    kappa: 0.00079777755286239568
-    s: 12.625593396669547
+    x: 586364.90518380131
+    y: 4140773.8458422143
+    theta: 1.2971398097005555
+    kappa: 0.0040089992371139475
+    s: 12.626654141327624
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9037920577889014
-  a: -0.66698574030066438
+  v: 2.9046427200502056
+  a: -0.66660134101106139
   relative_time: 3.2999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586364.73453132715
-    y: 4140774.0427430593
-    theta: 1.3085017423874294
-    kappa: 0.00074681425226669966
-    s: 12.769948842978035
+    x: 586364.94416601048
+    y: 4140773.9848792688
+    theta: 1.2977308818078277
+    kappa: 0.004168846843187704
+    s: 12.771052601387883
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8704177830024626
-  a: -0.66794712607068973
+  v: 2.8712876733005812
+  a: -0.667562428161817
   relative_time: 3.3499999999999961
 }
 trajectory_point {
   path_point {
-    x: 586364.77152225235
-    y: 4140774.1805503857
-    theta: 1.30860460289167
-    kappa: 0.00069547800893650651
-    s: 12.9126344691277
+    x: 586364.98260943161
+    y: 4140774.1223341264
+    theta: 1.2983382610899243
+    kappa: 0.004316827336963102
+    s: 12.913782203003752
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8370011576904521
-  a: -0.66867976132353624
+  v: 2.8378902873152327
+  a: -0.66829491044005607
   relative_time: 3.3999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586364.80806633073
-    y: 4140774.3167468295
-    theta: 1.3086999505832855
-    kappa: 0.000643807331561477
-    s: 13.053648443530417
+    x: 586365.0205531551
+    y: 4140774.2581939455
+    theta: 1.29895156233415
+    kappa: 0.0044574326355691484
+    s: 13.054841114969538
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8035536193787292
-  a: -0.66918364605920377
+  v: 2.8044619923377847
+  a: -0.66879878784577884
   relative_time: 3.4499999999999957
 }
 trajectory_point {
   path_point {
-    x: 586364.84416303271
-    y: 4140774.4513312262
-    theta: 1.3087849322356844
-    kappa: 0.000594119227121478
-    s: 13.192989506474341
+    x: 586365.057908583
+    y: 4140774.392482046
+    theta: 1.2995870334985455
+    kappa: 0.0045785960739071693
+    s: 13.194228077591722
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7700866055931517
-  a: -0.66945878027769234
+  v: 2.7710142186118638
+  a: -0.66907406037898509
   relative_time: 3.4999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586364.87982090015
-    y: 4140774.5843005721
-    theta: 1.308865366885082
-    kappa: 0.00054555088605870471
-    s: 13.330656970123931
+    x: 586365.09478697414
+    y: 4140774.5251667048
+    theta: 1.3002207162769157
+    kappa: 0.0046943924310378074
+    s: 13.331942402688973
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7366115538595794
-  a: -0.66950516397900206
+  v: 2.737558396381095
+  a: -0.66912072803967493
   relative_time: 3.5499999999999954
 }
 trajectory_point {
   path_point {
-    x: 586364.91502750665
-    y: 4140774.7156580784
-    theta: 1.3089345770708323
-    kappa: 0.00050246400418131468
-    s: 13.466650718519929
+    x: 586365.13108262932
+    y: 4140774.656277095
+    theta: 1.3008698194274666
+    kappa: 0.0047870583722096575
+    s: 13.467983973592144
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7031399017038718
-  a: -0.66932279716313281
+  v: 2.704105955889105
+  a: -0.66893879082784846
   relative_time: 3.5999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586364.94979848142
-    y: 4140774.8454000261
-    theta: 1.3090014585254302
-    kappa: 0.00046077385879857039
-    s: 13.600971207579379
+    x: 586365.16689904814
+    y: 4140774.7857849337
+    theta: 1.3015150658033545
+    kappa: 0.0048732378246877789
+    s: 13.602353245144258
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6696830866518861
-  a: -0.6689116798300847
+  v: 2.6706683273795191
+  a: -0.66852824874350558
   relative_time: 3.649999999999995
 }
 trajectory_point {
   path_point {
-    x: 586364.98412353988
-    y: 4140774.9735302362
-    theta: 1.3090583861208471
-    kappa: 0.00042814570535472629
-    s: 13.733619465095611
+    x: 586365.20213126892
+    y: 4140774.9137202669
+    theta: 1.3021695788697094
+    kappa: 0.0049359149067716478
+    s: 13.735051243700529
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6362525462294828
-  a: -0.66827181197985763
+  v: 2.6372569410959636
+  a: -0.66788910178664618
   relative_time: 3.6999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586365.01801452565
-    y: 4140775.1000471893
-    theta: 1.309113750743363
-    kappa: 0.00039749390864407
-    s: 13.864597090738251
+    x: 586365.236877898
+    y: 4140775.0400574692
+    theta: 1.3028188860840413
+    kappa: 0.004989166836786749
+    s: 13.866079567128347
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.60285971796252
-  a: -0.6674031936124516
+  v: 2.6038832272820631
+  a: -0.66702134995727036
   relative_time: 3.7499999999999947
 }
 trajectory_point {
   path_point {
-    x: 586365.05146191036
-    y: 4140775.2249556864
-    theta: 1.3091627239505599
-    kappa: 0.00037775410970581664
-    s: 13.993906256053217
+    x: 586365.27108030277
+    y: 4140775.1648148997
+    theta: 1.3034672409278611
+    kappa: 0.0050209004961349231
+    s: 13.995440384807289
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5695160393768575
-  a: -0.66630582472786659
+  v: 2.5705586161814451
+  a: -0.66592499325537824
   relative_time: 3.7999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586365.08447659889
-    y: 4140775.3482556469
-    theta: 1.3092105381891639
-    kappa: 0.00036090439196833409
-    s: 14.121549704462721
+    x: 586365.30478618317
+    y: 4140775.2879822217
+    theta: 1.3041088127699767
+    kappa: 0.0050406535733722108
+    s: 14.123136437629107
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5362329479983536
-  a: -0.66497970532610273
+  v: 2.5372945380377345
+  a: -0.66460003168096959
   relative_time: 3.8499999999999943
 }
 trajectory_point {
   path_point {
-    x: 586365.11705257534
-    y: 4140775.469952113
-    theta: 1.3092552199096532
-    kappa: 0.00035680046972894465
-    s: 14.247530751265268
+    x: 586365.3379469926
+    y: 4140775.4095761403
+    theta: 1.3047449869833521
+    kappa: 0.0050382806934767274
+    s: 14.249171037997737
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5030218813528675
-  a: -0.66342483540716
+  v: 2.5041024230945572
+  a: -0.66304646523404465
   relative_time: 3.8999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586365.14919789077
-    y: 4140775.5900469543
-    theta: 1.3092995130357485
-    kappa: 0.00035575384983567361
-    s: 14.371853283635655
+    x: 586365.37062097783
+    y: 4140775.5295846881
+    theta: 1.3053709181316875
+    kappa: 0.0050235540733328437
+    s: 14.373548069829297
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4698942769662575
-  a: -0.66164121497103823
+  v: 2.470993701595539
+  a: -0.66126429391460317
   relative_time: 3.949999999999994
 }
 trajectory_point {
   path_point {
-    x: 586365.18090649438
-    y: 4140775.7085464047
-    theta: 1.309344215800859
-    kappa: 0.00036974930801916551
-    s: 14.494521760624972
+    x: 586365.402765787
+    y: 4140775.648024004
+    theta: 1.3059850183983175
+    kappa: 0.0049858422684235879
+    s: 14.496271988552085
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4368615723643834
-  a: -0.65962884401773758
+  v: 2.4379798037843052
+  a: -0.65925351772264529
   relative_time: 3.9999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586365.21218767064
-    y: 4140775.8254532292
-    theta: 1.3093889375346817
-    kappa: 0.00038570352871648736
-    s: 14.615541253657003
+    x: 586365.43443632522
+    y: 4140775.7648843443
+    theta: 1.3065867306860581
+    kappa: 0.0049373211805968809
+    s: 14.617347861604665
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4039375231186115
-  a: -0.65730393980696322
+  v: 2.4050744781659033
+  a: -0.65693033607984974
   relative_time: 4.0499999999999936
 }
 trajectory_point {
   path_point {
-    x: 586365.24303388968
-    y: 4140775.9407754061
-    theta: 1.3094384142694384
-    kappa: 0.00042001244025051691
-    s: 14.734917523322023
+    x: 586365.46557575534
+    y: 4140775.8801870635
+    theta: 1.3071704124439247
+    kappa: 0.00486249164758723
+    s: 14.736781445280712
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3711340969843198
-  a: -0.654803919558408
+  v: 2.3722896872696939
+  a: -0.65443212883481361
   relative_time: 4.0999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586365.2734571232
-    y: 4140776.0545162023
-    theta: 1.3094872125435928
-    kappa: 0.00045385088329987123
-    s: 14.852656819670656
+    x: 586365.49626305606
+    y: 4140775.9939172119
+    theta: 1.3077418645489307
+    kappa: 0.0047817733928262971
+    s: 14.854578985098339
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3384600497634005
-  a: -0.652128783272073
+  v: 2.3396341823757925
+  a: -0.65175889598753711
   relative_time: 4.1499999999999932
 }
 trajectory_point {
   path_point {
-    x: 586365.30344698229
-    y: 4140776.1666853195
-    theta: 1.3095463330034549
-    kappa: 0.00050882849624912622
-    s: 14.968765830543617
+    x: 586365.52641221171
+    y: 4140776.1061048862
+    theta: 1.308286588353085
+    kappa: 0.0046714341354892
+    s: 14.970747164139663
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3059241372577421
-  a: -0.64927853094795829
+  v: 2.307116714764311
+  a: -0.64891063753802025
   relative_time: 4.1999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586365.33301735588
-    y: 4140776.2772864182
-    theta: 1.3096048435923031
-    kappa: 0.00056346312150780452
-    s: 15.083251681571717
+    x: 586365.55613951606
+    y: 4140776.2167261294
+    theta: 1.3088235237345012
+    kappa: 0.0045628528611753837
+    s: 15.085293103050811
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2735351152692345
-  a: -0.6462531625860638
+  v: 2.2747460357153617
+  a: -0.645887353486263
   relative_time: 4.2499999999999929
 }
 trajectory_point {
   path_point {
-    x: 586365.36215723818
-    y: 4140776.38633027
-    theta: 1.3096734803039298
-    kappa: 0.00061449252085402594
-    s: 15.196121936175858
+    x: 586365.5853324635
+    y: 4140776.3258189363
+    theta: 1.3093276956966311
+    kappa: 0.0044859357322278564
+    s: 15.198224360041911
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2413017395997659
-  a: -0.64305267818638967
+  v: 2.2425308965090558
+  a: -0.64268904383226533
   relative_time: 4.2999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586365.3908786712
-    y: 4140776.4938219376
-    theta: 1.3097439714952614
-    kappa: 0.00066406220188147853
-    s: 15.30738459556704
+    x: 586365.61411007959
+    y: 4140776.4333596677
+    theta: 1.3098246947731793
+    kappa: 0.0044101129122321335
+    s: 15.309548930887095
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2092327660512252
-  a: -0.63967707774893579
+  v: 2.2104800484255058
+  a: -0.63931570857602726
   relative_time: 4.3499999999999925
 }
 trajectory_point {
   path_point {
-    x: 586365.419178257
-    y: 4140776.5997710638
-    theta: 1.3098190905517915
-    kappa: 0.00070582872234058278
-    s: 15.41704809874636
+    x: 586365.64237534953
+    y: 4140776.5393829485
+    theta: 1.3103056913526197
+    kappa: 0.0043738952932069943
+    s: 15.419275248924507
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1773369504255022
-  a: -0.63612636127370215
+  v: 2.1786022427448239
+  a: -0.63576734771754873
   relative_time: 4.3999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586365.4470596842
-    y: 4140776.7041858505
-    theta: 1.3098979710185235
-    kappa: 0.000740892231958601
-    s: 15.525121322505003
+    x: 586365.67020048609
+    y: 4140776.6438786844
+    theta: 1.3107769753926091
+    kappa: 0.0043501275264931282
+    s: 15.527412185056292
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1456230485244849
-  a: -0.63240052876068875
+  v: 2.1469062307471218
+  a: -0.63204396125682982
   relative_time: 4.4499999999999922
 }
 trajectory_point {
   path_point {
-    x: 586365.474528936
-    y: 4140776.80707434
-    theta: 1.3099771537760505
-    kappa: 0.00077313732868126257
-    s: 15.631613581424251
+    x: 586365.69757435354
+    y: 4140776.746859428
+    theta: 1.3112410088359698
+    kappa: 0.0043453005069968662
+    s: 15.633969047748604
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.114099816150063
-  a: -0.62849958020989571
+  v: 2.1154007637125116
+  a: -0.62814554919387056
   relative_time: 4.499999999999992
 }
 trajectory_point {
   path_point {
-    x: 586365.50157597533
-    y: 4140776.9084493062
-    theta: 1.310060889145789
-    kappa: 0.00079584928920622229
-    s: 15.736534627875482
+    x: 586365.72448878107
+    y: 4140776.8483374366
+    theta: 1.3116977472864029
+    kappa: 0.00436385466134036
+    s: 15.738955583031597
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0827760091041254
-  a: -0.62442351562132292
+  v: 2.0840945929211059
+  a: -0.62407211152867093
   relative_time: 4.5499999999999918
 }
 trajectory_point {
   path_point {
-    x: 586365.528220607
-    y: 4140777.0083160084
-    theta: 1.3101433786951295
-    kappa: 0.00081822333967210433
-    s: 15.839894652020169
+    x: 586365.75099871086
+    y: 4140776.94830864
+    theta: 1.3121480505871521
+    kappa: 0.0043834123636175238
+    s: 15.842381974499435
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0516603831885609
-  a: -0.62017233499497038
+  v: 2.0529964696530154
+  a: -0.61982364826123093
   relative_time: 4.5999999999999917
 }
 trajectory_point {
   path_point {
-    x: 586365.55445099273
-    y: 4140777.1066886033
-    theta: 1.3102279727640085
-    kappa: 0.0008327379187525241
-    s: 15.941704281809875
+    x: 586365.7770004312
+    y: 4140777.046811467
+    theta: 1.3126002260622427
+    kappa: 0.0044339798271690462
+    s: 15.944258843310289
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.020761694205258
-  a: -0.61574603833083819
+  v: 2.0221151451883537
+  a: -0.61540015939155046
   relative_time: 4.6499999999999915
 }
 trajectory_point {
   path_point {
-    x: 586365.58028079267
-    y: 4140777.2035749014
-    theta: 1.3103122026549612
-    kappa: 0.00084497333355557872
-    s: 16.041974582986263
+    x: 586365.80260949407
+    y: 4140777.1438267827
+    theta: 1.3130455731767625
+    kappa: 0.0044837836608617289
+    s: 16.044597248186328
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9900886979561068
-  a: -0.61114462562892613
+  v: 1.9914593708072315
+  a: -0.61080164491962963
   relative_time: 4.6999999999999913
 }
 trajectory_point {
   path_point {
-    x: 586365.60571097559
-    y: 4140777.2989865481
-    theta: 1.3103957024600874
-    kappa: 0.00085483331155595166
-    s: 16.140717059081087
+    x: 586365.82777118194
+    y: 4140777.2393808854
+    theta: 1.3134925887153921
+    kappa: 0.0045462717643641832
+    s: 16.143408685413739
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9596501502429955
-  a: -0.60636809688923443
+  v: 1.9610378977897616
+  a: -0.60602810484546832
   relative_time: 4.7499999999999911
 }
 trajectory_point {
   path_point {
-    x: 586365.6307348574
-    y: 4140777.3929376821
-    theta: 1.3104793753696347
-    kappa: 0.00085878593128542436
-    s: 16.237943651416195
+    x: 586365.85249943309
+    y: 4140777.3334824461
+    theta: 1.3139397300843076
+    kappa: 0.0046189103576915911
+    s: 16.240705088842702
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9294548068678132
-  a: -0.601416452111763
+  v: 1.9308594774160555
+  a: -0.60107953916906665
   relative_time: 4.7999999999999909
 }
 trajectory_point {
   path_point {
-    x: 586365.65537177154
-    y: 4140777.4854359627
-    theta: 1.3105617543676835
-    kappa: 0.00086267742800326592
-    s: 16.333666739103531
+    x: 586365.8768457768
+    y: 4140777.426130686
+    theta: 1.3143799656803994
+    kappa: 0.0046904271080937541
+    s: 16.336498829887407
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8995114236324491
-  a: -0.59628969129651188
+  v: 1.9009328609662255
+  a: -0.59595594789042461
   relative_time: 4.8499999999999908
 }
 trajectory_point {
   path_point {
-    x: 586365.67961282446
-    y: 4140777.5764970221
-    theta: 1.3106429214348863
-    kappa: 0.00086228016508376935
-    s: 16.427899139045135
+    x: 586365.90071502235
+    y: 4140777.5173638039
+    theta: 1.3148322862334558
+    kappa: 0.0047768790612259865
+    s: 16.430802717526056
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8698287563387921
-  a: -0.59098781444348092
+  v: 1.871266799720384
+  a: -0.59065733100954221
   relative_time: 4.8999999999999906
 }
 trajectory_point {
   path_point {
-    x: 586365.70346766175
-    y: 4140777.6661320073
-    theta: 1.3107228517005225
-    kappa: 0.00085975662506970773
-    s: 16.520654105933136
+    x: 586365.924205672
+    y: 4140777.6071696691
+    theta: 1.3152784575423149
+    kappa: 0.004862768295620332
+    s: 16.523629998300848
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8404155607887307
-  a: -0.58551082155267031
+  v: 1.8418700449586423
+  a: -0.58518368852641944
   relative_time: 4.94999999999999
 }
 trajectory_point {
   path_point {
-    x: 586365.72694605251
-    y: 4140777.7543524881
-    theta: 1.3108015206083552
-    kappa: 0.00085727290834518017
-    s: 16.611945332249764
+    x: 586365.9472967831
+    y: 4140777.6955678803
+    theta: 1.3157232252783888
+    kappa: 0.0049496518663754213
+    s: 16.61499435631799
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8112805927841547
-  a: -0.57985871262408
+  v: 1.8127513479611128
+  a: -0.57953502044105609
   relative_time: 4.99999999999999
 }
 trajectory_point {
   path_point {
-    x: 586365.75003319269
-    y: 4140777.841177023
-    theta: 1.31087777203169
-    kappa: 0.000850210258743214
-    s: 16.70178694826734
+    x: 586365.96993431984
+    y: 4140777.7825871268
+    theta: 1.3161777101707401
+    kappa: 0.0050421538499934009
+    s: 16.704909913247697
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7824326081269528
-  a: -0.57403148765771
+  v: 1.7839194600079076
+  a: -0.57371132675345249
   relative_time: 5.04999999999999
 }
 trajectory_point {
   path_point {
-    x: 586365.77275039558
-    y: 4140777.9266150179
-    theta: 1.31095273162623
-    kappa: 0.000842968434586947
-    s: 16.790193522048281
+    x: 586365.99221076537
+    y: 4140777.8682183311
+    theta: 1.3166249456082124
+    kappa: 0.0051331803357004433
+    s: 16.793391228324186
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7538803626190136
-  a: -0.56802914665356008
+  v: 1.7553831323791385
+  a: -0.56771260746360852
   relative_time: 5.09999999999999
 }
 trajectory_point {
   path_point {
-    x: 586365.79510112212
-    y: 4140778.0106810792
-    theta: 1.3110262766112915
-    kappa: 0.00083551107844286629
-    s: 16.877180059445095
+    x: 586366.0140870132
+    y: 4140777.9524871334
+    theta: 1.3170747283759419
+    kappa: 0.0052240534120682426
+    s: 16.880453298345685
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7256326120622267
-  a: -0.56185168961163057
+  v: 1.7271511163549178
+  a: -0.56153886257152408
   relative_time: 5.14999999999999
 }
 trajectory_point {
   path_point {
-    x: 586365.817075671
-    y: 4140778.0933937654
-    theta: 1.3110965982118072
-    kappa: 0.00082496563371921164
-    s: 16.962762004100384
+    x: 586366.03555296222
+    y: 4140778.0354121029
+    theta: 1.3175303075115019
+    kappa: 0.0053152151610135947
+    s: 16.966111557674424
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6976981122584804
-  a: -0.55549911653192141
+  v: 1.6992321632153571
+  a: -0.55519009207719927
   relative_time: 5.1999999999999895
 }
 trajectory_point {
   path_point {
-    x: 586365.83869364578
-    y: 4140778.1747642988
-    theta: 1.31116577872608
-    kappa: 0.00081459130661158262
-    s: 17.046955237446856
+    x: 586366.05667109368
+    y: 4140778.1169934208
+    theta: 1.3179785048104922
+    kappa: 0.0054048997969942739
+    s: 17.050381878236635
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6700856190096642
-  a: -0.54897142741443239
+  v: 1.6716350242405686
+  a: -0.5486662959806341
   relative_time: 5.2499999999999893
 }
 trajectory_point {
   path_point {
-    x: 586365.85995748092
-    y: 4140778.2548089181
-    theta: 1.3112335234406765
-    kappa: 0.000804137708308196
-    s: 17.129776078707298
+    x: 586366.07739194762
+    y: 4140778.1972606969
+    theta: 1.318429523087761
+    kappa: 0.0054915939512435487
+    s: 17.133280569522562
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.642803888117667
-  a: -0.54226862225916372
+  v: 1.6443684507106644
+  a: -0.54196747428182845
   relative_time: 5.2999999999999892
 }
 trajectory_point {
   path_point {
-    x: 586365.8808588353
-    y: 4140778.3335471759
-    theta: 1.3112976271625638
-    kappa: 0.00079181406300154154
-    s: 17.2112412848946
+    x: 586366.09771288873
+    y: 4140778.2762319078
+    theta: 1.318884742340487
+    kappa: 0.0055751201463759108
+    s: 17.214824378586449
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.615861675384378
-  a: -0.5353907010661153
+  v: 1.6174411939057567
+  a: -0.53509362698078244
   relative_time: 5.349999999999989
 }
 trajectory_point {
   path_point {
-    x: 586365.901416789
-    y: 4140778.4109917954
-    theta: 1.3113606776865481
-    kappa: 0.00077969289018499149
-    s: 17.291368050811744
+    x: 586366.117700472
+    y: 4140778.3539076238
+    theta: 1.3193324938950244
+    kappa: 0.0056572761235559287
+    s: 17.295030490046553
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5892677366116854
-  a: -0.52833766383528713
+  v: 1.590862005105957
+  a: -0.52804475407749607
   relative_time: 5.3999999999999888
 }
 trajectory_point {
   path_point {
-    x: 586365.92163586582
-    y: 4140778.4871598203
-    theta: 1.3114226888869462
-    kappa: 0.00076777152248884092
-    s: 17.370174009051809
+    x: 586366.13732110255
+    y: 4140778.4303146568
+    theta: 1.319779650435229
+    kappa: 0.0057350239535144979
+    s: 17.373916526085132
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5630308276014788
-  a: -0.5211095105666792
+  v: 1.5646396355913779
+  a: -0.52082085557196933
   relative_time: 5.4499999999999886
 }
 trajectory_point {
   path_point {
-    x: 586365.94150662026
-    y: 4140778.5620724517
-    theta: 1.3114806605585267
-    kappa: 0.00075494163932368174
-    s: 17.447677229997964
+    x: 586366.15654860809
+    y: 4140778.5054783663
+    theta: 1.3202317832144848
+    kappa: 0.0058059145102879722
+    s: 17.451500546448447
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5371597041556473
-  a: -0.51370624126029163
+  v: 1.538782836642131
+  a: -0.51342193146420212
   relative_time: 5.4999999999999885
 }
 trajectory_point {
   path_point {
-    x: 586365.96104764962
-    y: 4140778.6357439039
-    theta: 1.3115375717899482
-    kappa: 0.00074228772618664945
-    s: 17.523896221823478
+    x: 586366.17545802146
+    y: 4140778.5793985985
+    theta: 1.3206764361421002
+    kappa: 0.0058756322826259751
+    s: 17.527801048446769
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5116631220760797
-  a: -0.5061278559161243
+  v: 1.5133003595383285
+  a: -0.50584798175419454
   relative_time: 5.5499999999999883
 }
 trajectory_point {
   path_point {
-    x: 586365.98026428558
-    y: 4140778.7081923643
-    theta: 1.31159353825905
-    kappa: 0.00072984387595890674
-    s: 17.59884993049171
+    x: 586366.19404689549
+    y: 4140778.6520955176
+    theta: 1.3211148107896504
+    kappa: 0.0059432440353398147
+    s: 17.602836966954371
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4865498371646648
-  a: -0.49837435453417722
+  v: 1.4882009555600826
+  a: -0.49809900644194649
   relative_time: 5.5999999999999881
 }
 trajectory_point {
   path_point {
-    x: 586365.99915327574
-    y: 4140778.7794387471
-    theta: 1.3116465393466179
-    kappa: 0.00071753913303192977
-    s: 17.672557739756112
+    x: 586366.212233436
+    y: 4140778.72360998
+    theta: 1.3215602568469655
+    kappa: 0.0059972392288509604
+    s: 17.676627674409538
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4618286052232916
-  a: -0.49044573711445039
+  v: 1.4634933759875048
+  a: -0.49017500552745813
   relative_time: 5.6499999999999879
 }
 trajectory_point {
   path_point {
-    x: 586366.0177237516
-    y: 4140778.8495011395
-    theta: 1.311697592193616
-    kappa: 0.00070540356924557576
-    s: 17.745039471160236
+    x: 586366.23011796258
+    y: 4140778.7939368407
+    theta: 1.321998305740439
+    kappa: 0.0060503377543518148
+    s: 17.749192980814552
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4375081820538496
-  a: -0.48234200365694391
+  v: 1.4391863721007074
+  a: -0.48207597901072941
   relative_time: 5.6999999999999877
 }
 trajectory_point {
   path_point {
-    x: 586366.03598528588
-    y: 4140778.9183979621
-    theta: 1.3117477957167079
-    kappa: 0.00069346989478169345
-    s: 17.816315384037726
+    x: 586366.24770546565
+    y: 4140778.8630957245
+    theta: 1.3224290795274143
+    kappa: 0.00610255442768163
+    s: 17.820553133735704
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4135973234582271
-  a: -0.47406315416165767
+  v: 1.4152886951798025
+  a: -0.47380192689176015
   relative_time: 5.7499999999999876
 }
 trajectory_point {
   path_point {
-    x: 586366.05394139758
-    y: 4140778.9861496929
-    theta: 1.311796749787286
-    kappa: 0.0006818546552453664
-    s: 17.88640617551232
+    x: 586366.26493304607
+    y: 4140778.9311239086
+    theta: 1.3228590311921176
+    kappa: 0.00614314882700124
+    s: 17.890728818303298
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3901047852383137
-  a: -0.46560918862859169
+  v: 1.3918090965049021
+  a: -0.46535284917055064
   relative_time: 5.7999999999999874
 }
 trajectory_point {
   path_point {
-    x: 586366.07158896048
-    y: 4140779.0527790259
-    theta: 1.311842483462877
-    kappa: 0.00067112898636251359
-    s: 17.95533298049785
+    x: 586366.28182958125
+    y: 4140778.9980358654
+    theta: 1.3232860855004605
+    kappa: 0.0061758807034081884
+    s: 17.959741157211631
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3670393231959979
-  a: -0.45698010705774594
+  v: 1.3687563273561183
+  a: -0.45672874584710066
   relative_time: 5.8499999999999872
 }
 trajectory_point {
   path_point {
-    x: 586366.08894402743
-    y: 4140779.1183040235
-    theta: 1.3118874591347762
-    kappa: 0.000660581087970969
-    s: 18.023117371698245
+    x: 586366.29844656913
+    y: 4140779.0638407874
+    theta: 1.3237060743758093
+    kappa: 0.0062080710421826158
+    s: 18.027611710719015
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3444096931331688
-  a: -0.44817590944912056
+  v: 1.3461391390135631
+  a: -0.44792961692141031
   relative_time: 5.899999999999987
 }
 trajectory_point {
   path_point {
-    x: 586366.10601223365
-    y: 4140779.1827459633
-    theta: 1.3119316914069994
-    kappa: 0.00065020753502773379
-    s: 18.089781359607525
+    x: 586366.31478939531
+    y: 4140779.1285600024
+    theta: 1.3241191337963991
+    kappa: 0.0062397302766761878
+    s: 18.09436247664776
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3222246508517155
-  a: -0.43919659580271542
+  v: 1.3239662827573482
+  a: -0.43895546239347949
   relative_time: 5.9499999999999869
 }
 trajectory_point {
   path_point {
-    x: 586366.12279541907
-    y: 4140779.2461275803
-    theta: 1.3119742004144508
-    kappa: 0.00064069403290548457
-    s: 18.155347392509803
+    x: 586366.33077805885
+    y: 4140779.1922367848
+    theta: 1.3245302377997288
+    kappa: 0.006251350189628524
+    s: 18.160015890384187
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3004929521535271
-  a: -0.43004216611853052
+  v: 1.3022465098675859
+  a: -0.42980628226330841
   relative_time: 5.9999999999999867
 }
 trajectory_point {
   path_point {
-    x: 586366.13929805055
-    y: 4140779.3084714329
-    theta: 1.3120146425295673
-    kappa: 0.00063228563623980054
-    s: 18.21983842160078
+    x: 586366.346496722
+    y: 4140779.2548736017
+    theta: 1.3249350863040457
+    kappa: 0.0062608742978454562
+    s: 18.224594889996137
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2792270333842009
-  a: -0.42058353827578937
+  v: 1.2809922520584238
+  a: -0.42035298853622577
   relative_time: 6.0499999999999865
 }
 trajectory_point {
   path_point {
-    x: 586366.15553162736
-    y: 4140779.3697988484
-    theta: 1.3120544252869808
-    kappa: 0.00062401432789591744
-    s: 18.28327800565404
+    x: 586366.36195960129
+    y: 4140779.316491154
+    theta: 1.3253333468712756
+    kappa: 0.0062702434239940838
+    s: 18.288123020938713
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.25843570296382
-  a: -0.4110586321607253
+  v: 1.2602123151706546
+  a: -0.4108334454076259
   relative_time: 6.0999999999999863
 }
 trajectory_point {
   path_point {
-    x: 586366.1715022428
-    y: 4140779.4301328445
-    theta: 1.3120935636191593
-    kappa: 0.00061587700320990188
-    s: 18.345689956934869
+    x: 586366.37717248977
+    y: 4140779.3771125255
+    theta: 1.3257251685740019
+    kappa: 0.006279461077928981
+    s: 18.350624082069729
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2381222748060003
-  a: -0.40146744777334237
+  v: 1.2399100116743533
+  a: -0.40124765287751346
   relative_time: 6.1499999999999861
 }
 trajectory_point {
   path_point {
-    x: 586366.18721148255
-    y: 4140779.4894978055
-    theta: 1.3121313049849974
-    kappa: 0.00060891830390106269
-    s: 18.407098253404243
+    x: 586366.39204904507
+    y: 4140779.4367840141
+    theta: 1.3261099966609295
+    kappa: 0.0062643069214085335
+    s: 18.412122037870507
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2182900628243578
-  a: -0.39180998511364051
+  v: 1.2200886540395954
+  a: -0.39159561094588841
   relative_time: 6.199999999999986
 }
 trajectory_point {
   path_point {
-    x: 586366.20266455051
-    y: 4140779.5479173297
-    theta: 1.3121675001669704
-    kappa: 0.00060335943625637221
-    s: 18.467527038718803
+    x: 586366.40668726771
+    y: 4140779.4955059811
+    theta: 1.3264886872646404
+    kappa: 0.0062489958060150275
+    s: 18.472641018445881
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1989423809325084
-  a: -0.38208624418161985
+  v: 1.2007515547364565
+  a: -0.38187731961275073
   relative_time: 6.2499999999999858
 }
 trajectory_point {
   path_point {
-    x: 586366.21787335083
-    y: 4140779.6054134127
-    theta: 1.3122031232093694
-    kappa: 0.0005978884379998601
-    s: 18.527000622230883
+    x: 586366.42109457427
+    y: 4140779.5533016166
+    theta: 1.3268614040792537
+    kappa: 0.0062339262215892373
+    s: 18.532205319524181
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.180082543044068
-  a: -0.37229622497728021
+  v: 1.1819020262350124
+  a: -0.37209277887810049
   relative_time: 6.2999999999999856
 }
 trajectory_point {
   path_point {
-    x: 586366.23284414236
-    y: 4140779.6620097151
-    theta: 1.3122381887719841
-    kappa: 0.000592503057662268
-    s: 18.585543478988491
+    x: 586366.43527688133
+    y: 4140779.6101946556
+    theta: 1.32722830006732
+    kappa: 0.0062190919794846066
+    s: 18.590839402457242
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1617138630726527
-  a: -0.36243992750062171
+  v: 1.1635433810053384
+  a: -0.36224198874193764
   relative_time: 6.3499999999999854
 }
 trajectory_point {
   path_point {
-    x: 586366.24758176086
-    y: 4140779.7177304467
-    theta: 1.3122725139958085
-    kappa: 0.0005878022324727547
-    s: 18.643180249735323
+    x: 586366.449167978
+    y: 4140779.66622692
+    theta: 1.3275839198599098
+    kappa: 0.0061828866985212758
+    s: 18.64856789422041
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1438396549318779
-  a: -0.35251735175164434
+  v: 1.1456789315175104
+  a: -0.35232494920426222
   relative_time: 6.3999999999999853
 }
 trajectory_point {
   path_point {
-    x: 586366.26208830927
-    y: 4140779.77260071
-    theta: 1.3123055419116374
-    kappa: 0.00058552332454252438
-    s: 18.69993574091075
+    x: 586366.4628282
+    y: 4140779.7214089697
+    theta: 1.3279326424637812
+    kappa: 0.0061415680194649956
+    s: 18.705415587412524
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1264632325353603
-  a: -0.3425284977303481
+  v: 1.1283119902416043
+  a: -0.34234166026507418
   relative_time: 6.4499999999999851
 }
 trajectory_point {
   path_point {
-    x: 586366.276375988
-    y: 4140779.8266431103
-    theta: 1.3123380715136421
-    kappa: 0.00058327879999559322
-    s: 18.755834924649825
+    x: 586366.47628276784
+    y: 4140779.7757602548
+    theta: 1.3282761150934224
+    kappa: 0.0061008713919064256
+    s: 18.761407440255937
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1095879097967152
-  a: -0.33247336543673295
+  v: 1.1114458696476952
+  a: -0.33229212192437352
   relative_time: 6.4999999999999849
 }
 trajectory_point {
   path_point {
-    x: 586366.2904512221
-    y: 4140779.8798819492
-    theta: 1.3123701174302953
-    kappa: 0.00058106764947056522
-    s: 18.810902938783279
+    x: 586366.48953771847
+    y: 4140779.8293051631
+    theta: 1.328614491786055
+    kappa: 0.0060607785550496548
+    s: 18.816568576596495
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.093217000629559
-  a: -0.32235195487079893
+  v: 1.0950838822058588
+  a: -0.3221763341821603
   relative_time: 6.5499999999999847
 }
 trajectory_point {
   path_point {
-    x: 586366.30432047893
-    y: 4140779.93234169
-    theta: 1.3124016943864996
-    kappa: 0.00057888885695285173
-    s: 18.865165086837532
+    x: 586366.50257611286
+    y: 4140779.8820739244
+    theta: 1.3289438173248329
+    kappa: 0.00601222779839368
+    s: 18.870924285903559
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0773538189475076
-  a: -0.312164266032546
+  v: 1.079229340386171
+  a: -0.3119942970384344
   relative_time: 6.5999999999999845
 }
 trajectory_point {
   path_point {
-    x: 586366.3179847392
-    y: 4140779.9840484182
-    theta: 1.3124327428108988
-    kappa: 0.00057903540743615067
-    s: 18.918646838034675
+    x: 586366.51537577773
+    y: 4140779.9340982279
+    theta: 1.3292591958985667
+    kappa: 0.005944085110963726
+    s: 18.924500023269985
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0620016786641766
-  a: -0.30191029892197419
+  v: 1.0638855566587073
+  a: -0.301746010493196
   relative_time: 6.6499999999999844
 }
 trajectory_point {
   path_point {
-    x: 586366.33145335177
-    y: 4140780.0350261764
-    theta: 1.312463315250217
-    kappa: 0.00058034593439660524
-    s: 18.971373827292489
+    x: 586366.52799522225
+    y: 4140779.9853900257
+    theta: 1.3295701339774031
+    kappa: 0.0058769018787314976
+    s: 18.977321409412134
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0471638936931822
-  a: -0.29159005353908346
+  v: 1.0490558434935435
+  a: -0.29143147454644497
   relative_time: 6.6999999999999842
 }
 trajectory_point {
   path_point {
-    x: 586366.34473575815
-    y: 4140780.0852991571
-    theta: 1.3124934650193985
-    kappa: 0.00058163834305589878
-    s: 19.023371855224426
+    x: 586366.54044060677
+    y: 4140780.0359743554
+    theta: 1.3298767832934983
+    kappa: 0.0058106453041923021
+    s: 19.029414230669879
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0328437779481403
-  a: -0.28120352988387387
+  v: 1.034743513360755
+  a: -0.28105068919818132
   relative_time: 6.749999999999984
 }
 trajectory_point {
   path_point {
-    x: 586366.357838591
-    y: 4140780.1348924646
-    theta: 1.3125232071743091
-    kappa: 0.00058291327880548392
-    s: 19.074666888139628
+    x: 586366.55271813169
+    y: 4140780.0858764183
+    theta: 1.3301792965588854
+    kappa: 0.0057452823791858251
+    s: 19.080804439006585
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0190446453426669
-  a: -0.27075072795634536
+  v: 1.0209518787304173
+  a: -0.27060365444840506
   relative_time: 6.7999999999999838
 }
 trajectory_point {
   path_point {
-    x: 586366.37076852552
-    y: 4140780.1838313644
-    theta: 1.3125525568668943
-    kappa: 0.000584171391155172
-    s: 19.125285058042913
+    x: 586366.564797022
+    y: 4140780.1351306536
+    theta: 1.3304675718262351
+    kappa: 0.0056647933878897641
+    s: 19.131518152009129
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0057698097903778
-  a: -0.26023164775649804
+  v: 1.0076842520726064
+  a: -0.26009037029711629
   relative_time: 6.8499999999999837
 }
 trajectory_point {
   path_point {
-    x: 586366.383527026
-    y: 4140780.2321426691
-    theta: 1.3125821627226215
-    kappa: 0.00058855896315148466
-    s: 19.175252662634779
+    x: 586366.57668584608
+    y: 4140780.1837620223
+    theta: 1.3307423981142328
+    kappa: 0.0055701334405801215
+    s: 19.181581652887886
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.99302258520488906
-  a: -0.2496462892843318
+  v: 0.99494394585739765
+  a: -0.24951083674431487
   relative_time: 6.8999999999999835
 }
 trajectory_point {
   path_point {
-    x: 586366.39612489752
-    y: 4140780.2798508955
-    theta: 1.3126115523688531
-    kappa: 0.00059365443678782725
-    s: 19.224596165311407
+    x: 586366.588426542
+    y: 4140780.2317874711
+    theta: 1.3310138002606964
+    kappa: 0.0054766529034019906
+    s: 19.231021390476744
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.98080628549981674
-  a: -0.23899465253984664
+  v: 0.98273427255486689
+  a: -0.23886505379000084
   relative_time: 6.9499999999999833
 }
 trajectory_point {
   path_point {
-    x: 586366.40857022838
-    y: 4140780.3269814504
-    theta: 1.3126405861523645
-    kappa: 0.00059868821218924946
-    s: 19.273342195164659
+    x: 586366.60002543009
+    y: 4140780.2792328522
+    theta: 1.3312819243263805
+    kappa: 0.0053843014538032356
+    s: 19.279863979233085
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.96912422458877656
-  a: -0.22827673752304262
+  v: 0.97105854463508956
+  a: -0.22815302143417424
   relative_time: 6.9999999999999831
 }
 trajectory_point {
   path_point {
-    x: 586366.42086985963
-    y: 4140780.3735602396
-    theta: 1.312669280032438
-    kappa: 0.00060366305632862725
-    s: 19.321517546982079
+    x: 586366.61148886988
+    y: 4140780.326124182
+    theta: 1.3315469172841055
+    kappa: 0.005293028456071173
+    s: 19.3281361992378
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.95797971638538448
-  a: -0.2174925442339197
+  v: 0.95992007456814132
+  a: -0.21737473967683502
   relative_time: 7.0499999999999829
 }
 trajectory_point {
   path_point {
-    x: 586366.43303067458
-    y: 4140780.41961333
-    theta: 1.31269765006705
-    kappa: 0.00060858175328945736
-    s: 19.369149181246886
+    x: 586366.62279874156
+    y: 4140780.3724936103
+    theta: 1.3317983620318954
+    kappa: 0.0051897692219928342
+    s: 19.375864996195283
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.9473760748032567
-  a: -0.20664207267247792
+  v: 0.949322174824098
+  a: -0.20653020851798318
   relative_time: 7.0999999999999828
 }
 trajectory_point {
   path_point {
-    x: 586366.44505502167
-    y: 4140780.4651681581
-    theta: 1.3127268068612044
-    kappa: 0.00061613712273445335
-    s: 19.416264224137986
+    x: 586366.6339490083
+    y: 4140780.4183705142
+    theta: 1.3320310319117994
+    kappa: 0.0050678509700983459
+    s: 19.423077481433427
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.93731661375600883
-  a: -0.19572532283871719
+  v: 0.939268157873035
+  a: -0.19561942795761877
   relative_time: 7.1499999999999826
 }
 trajectory_point {
   path_point {
-    x: 586366.45695175242
-    y: 4140780.5102506131
-    theta: 1.3127563162640488
-    kappa: 0.00062522493977242358
-    s: 19.462889967529961
+    x: 586366.64498377871
+    y: 4140780.4637722177
+    theta: 1.3322612917898422
+    kappa: 0.0049471955677175376
+    s: 19.469800931903634
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.927804647157257
-  a: -0.18474229473263762
+  v: 0.9297613361850281
+  a: -0.18464239799574178
   relative_time: 7.1999999999999824
 }
 trajectory_point {
   path_point {
-    x: 586366.46873064258
-    y: 4140780.5548865129
-    theta: 1.3127855333674892
-    kappa: 0.00063422273926548025
-    s: 19.509053868993078
+    x: 586366.655909534
+    y: 4140780.5087253866
+    theta: 1.3324892768819623
+    kappa: 0.0048277321490456607
+    s: 19.516062790180815
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.91884348892061707
-  a: -0.17369298835423919
+  v: 0.9208050222301527
+  a: -0.17359911863235217
   relative_time: 7.2499999999999822
 }
 trajectory_point {
   path_point {
-    x: 586366.48039874015
-    y: 4140780.5991025665
-    theta: 1.3128144756542224
-    kappa: 0.000643135905260475
-    s: 19.554783551793285
+    x: 586366.66673279426
+    y: 4140780.5532568484
+    theta: 1.3327151232217651
+    kappa: 0.0047093894205833325
+    s: 19.56189066446337
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.9104364529597051
-  a: -0.16257740370352181
+  v: 0.9124025284784848
+  a: -0.16248958986745
   relative_time: 7.2999999999999821
 }
 trajectory_point {
   path_point {
-    x: 586366.49196313566
-    y: 4140780.6429256424
-    theta: 1.3128431607118161
-    kappa: 0.00065196985409998048
-    s: 19.600106804892206
+    x: 586366.67746011913
+    y: 4140780.5973935914
+    theta: 1.3329389676603511
+    kappa: 0.0045920956611364815
+    s: 19.607312328573215
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.90258685318813692
-  a: -0.15139554078048562
+  v: 0.90455716740009962
+  a: -0.15131381170103519
   relative_time: 7.3499999999999819
 }
 trajectory_point {
   path_point {
-    x: 586366.5034299551
-    y: 4140780.6863830369
-    theta: 1.312872000798498
-    kappa: 0.00066139808145768923
-    s: 19.645051582947147
+    x: 586366.68805235077
+    y: 4140780.6411738591
+    theta: 1.3331283699363781
+    kappa: 0.0044422156122321905
+    s: 19.652355721955768
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.8952980035195286
-  a: -0.14014739958513045
+  v: 0.89727225146507328
+  a: -0.14007178413310778
   relative_time: 7.3999999999999817
 }
 trajectory_point {
   path_point {
-    x: 586366.5148013311
-    y: 4140780.7295032684
-    theta: 1.3129029894919484
-    kappa: 0.00067477130028441908
-    s: 19.6896460063111
+    x: 586366.69856110588
+    y: 4140780.6846140558
+    theta: 1.3333154932399012
+    kappa: 0.0042926697442471247
+    s: 19.697048949679949
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.88857321786749588
-  a: -0.12883298011745642
+  v: 0.8905510931434808
+  a: -0.12876350716366775
   relative_time: 7.4499999999999815
 }
 trajectory_point {
   path_point {
-    x: 586366.52609058132
-    y: 4140780.7723120786
-    theta: 1.3129337543797914
-    kappa: 0.00068804793541063069
-    s: 19.733918361032735
+    x: 586366.70899417368
+    y: 4140780.7277413821
+    theta: 1.3335012688321293
+    kappa: 0.004144200953585286
+    s: 19.741420282438177
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.882415810145655
-  a: -0.11745228237746352
+  v: 0.88439700490539841
+  a: -0.1173889807927152
   relative_time: 7.4999999999999813
 }
 trajectory_point {
   path_point {
-    x: 586366.53730496042
-    y: 4140780.8148369784
-    theta: 1.3129643152331028
-    kappa: 0.00070123651910054992
-    s: 19.777897098856396
+    x: 586366.71935824014
+    y: 4140780.7705834769
+    theta: 1.333685815758394
+    kappa: 0.0039967140908523571
+    s: 19.785498156546382
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.87682909426762157
-  a: -0.10600530636515176
+  v: 0.87881329922090123
+  a: -0.10594820502024999
   relative_time: 7.5499999999999812
 }
 trajectory_point {
   path_point {
-    x: 586366.548451766
-    y: 4140780.8571056388
-    theta: 1.3129946919381026
-    kappa: 0.00071434563330813
-    s: 19.821610837222121
+    x: 586366.72966003069
+    y: 4140780.8131681411
+    theta: 1.3338692537579671
+    kappa: 0.0038501134524691625
+    s: 19.829311173943992
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.87181638414701168
-  a: -0.094492052080521027
+  v: 0.87380328856006528
+  a: -0.0944411798462722
   relative_time: 7.599999999999981
 }
 trajectory_point {
   path_point {
-    x: 586366.5595383374
-    y: 4140780.8991458919
-    theta: 1.3130249044961553
-    kappa: 0.00072738390967704415
-    s: 19.865088359265613
+    x: 586366.73989356449
+    y: 4140780.8555264114
+    theta: 1.3340393446031384
+    kappa: 0.0036953576496048905
+    s: 19.872888102193951
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.86738099369744148
-  a: -0.082912519523571482
+  v: 0.8693702853929659
+  a: -0.0828679052707818
   relative_time: 7.6499999999999808
 }
 trajectory_point {
   path_point {
-    x: 586366.570568882
-    y: 4140780.9409865658
-    theta: 1.3130565778851859
-    kappa: 0.000742193269301246
-    s: 19.908358613818272
+    x: 586366.75004897825
+    y: 4140780.8976904331
+    theta: 1.3341800479888601
+    kappa: 0.0035206522887558179
+    s: 19.916257874482685
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.86352623683252672
-  a: -0.071266708694303071
+  v: 0.86551760218967888
+  a: -0.071228381293778786
   relative_time: 7.6999999999999806
 }
 trajectory_point {
   path_point {
-    x: 586366.58154952829
-    y: 4140780.9826561543
-    theta: 1.3130903872844044
-    kappa: 0.00075953059218090079
-    s: 19.951450715407173
+    x: 586366.76016269834
+    y: 4140780.9396813475
+    theta: 1.3343201737173975
+    kappa: 0.0033466641910424003
+    s: 19.959449589620149
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.86025542746588324
-  a: -0.059554619592715741
+  v: 0.86224855142028
+  a: -0.059522607915263259
   relative_time: 7.7499999999999805
 }
 trajectory_point {
   path_point {
-    x: 586366.5924922392
-    y: 4140781.0241817851
-    theta: 1.3131240798804791
-    kappa: 0.00077680801882241014
-    s: 19.994393944255059
+    x: 586366.77024157718
+    y: 4140780.9815276065
+    theta: 1.3344598167249957
+    kappa: 0.00317327547128485
+    s: 20.00249251203978
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.85757187951112712
-  a: -0.04777625221880949
+  v: 0.85956644555484452
+  a: -0.047750585135235057
   relative_time: 7.79999999999998
 }
 trajectory_point {
   path_point {
-    x: 586366.603404518
-    y: 4140781.0655919309
-    theta: 1.3131576787761468
-    kappa: 0.00079403739625520122
-    s: 20.037217746280376
+    x: 586366.78029250633
+    y: 4140781.0232578209
+    theta: 1.3345990724853491
+    kappa: 0.0030003675771263206
+    s: 20.045416071798535
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.85547890688187422
-  a: -0.035931606572584374
+  v: 0.85747459706344831
+  a: -0.035912312953694236
   relative_time: 7.84999999999998
 }
 trajectory_point {
   path_point {
-    x: 586366.61429391021
-    y: 4140781.1069152267
-    theta: 1.3131912072041481
-    kappa: 0.00081123063817333441
-    s: 20.079951733097232
+    x: 586366.79032241576
+    y: 4140781.0649007643
+    theta: 1.3347380370095654
+    kappa: 0.0028278212890330478
+    s: 20.088249864576859
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.85397982349174062
-  a: -0.024020682654040393
+  v: 0.85597631841616706
+  a: -0.024007791370640907
   relative_time: 7.89999999999998
 }
 trajectory_point {
   path_point {
-    x: 586366.62516800372
-    y: 4140781.1481804661
-    theta: 1.3132246885272267
-    kappa: 0.00082839972493551793
-    s: 20.122625682015428
+    x: 586366.80032457469
+    y: 4140781.1064886646
+    theta: 1.3348592535714452
+    kappa: 0.0026830720721652086
+    s: 20.131023651678717
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.85307794325434227
-  a: -0.012043480463177492
+  v: 0.85507492208307612
+  a: -0.012037020386074959
   relative_time: 7.94999999999998
 }
 trajectory_point {
   path_point {
-    x: 586366.63602961332
-    y: 4140781.189417873
-    theta: 1.3132602762689825
-    kappa: 0.00084309988912481472
-    s: 20.165269536040441
+    x: 586366.81030392693
+    y: 4140781.1480511129
+    theta: 1.3349601748002775
+    kappa: 0.0025701507012397887
+    s: 20.173767360031572
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.85277658008329482
-  a: 4.2743586448068527e-15
+  v: 0.85477372053425138
+  a: 3.6082248300317588e-15
   relative_time: 7.99999999999998
 }
 decision {
@@ -2264,10 +2264,10 @@ decision {
       reason_code: STOP_REASON_DESTINATION
       reason: "stop by DEST"
       stop_point {
-        x: 586367.77189878887
-        y: 4140785.5300498847
+        x: 586367.82279939589
+        y: 4140785.5980816446
       }
-      stop_heading: 1.3156678974259997
+      stop_heading: 1.3683406050571048
     }
   }
   object_decision {
@@ -2287,10 +2287,10 @@ decision {
           reason_code: STOP_REASON_DESTINATION
           distance_s: 0
           stop_point {
-            x: 586367.77189878887
-            y: 4140785.5300498847
+            x: 586367.82279939589
+            y: 4140785.5980816446
           }
-          stop_heading: 1.3156678974259997
+          stop_heading: 1.3683406050571048
         }
       }
     }
diff --git a/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt b/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
index fca95dc7916d930154ec50fd44549b511004d320..b4e4551c820f85b7b6c197bf8b02af23361d8b08 100755
--- a/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
@@ -8,9 +8,9 @@ trajectory_point {
   path_point {
     x: 586343.905893
     y: 4140690.05199
+    z: 0
     theta: 1.6602776953500005
-    kappa: -3.1954284775870091e-18
-    s: 4.3368086899420177e-19
+    kappa: 0
     dkappa: 0
     ddkappa: 0
   }
@@ -20,2242 +20,2242 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586343.88725004031
-    y: 4140690.2657220792
-    theta: 1.6580131378062777
-    kappa: -0.007409085866456981
-    s: 0.21454361247520423
+    x: 586343.88663168577
+    y: 4140690.2657193872
+    theta: 1.660183991858776
+    kappa: -0.0004961824967182135
+    s: 0.21459554764257416
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3181066620668931
-  a: 0.98935995278079258
+  v: 4.3212079698121268
+  a: 1.1122280788333607
   relative_time: 0.05
 }
 trajectory_point {
   path_point {
-    x: 586343.868391925
-    y: 4140690.4819208095
-    theta: 1.6557224353388855
-    kappa: -0.014903678902452219
-    s: 0.43156324120611428
+    x: 586343.86712069577
+    y: 4140690.4822192527
+    theta: 1.660089073730135
+    kappa: -0.00099879678576691457
+    s: 0.43197280162628948
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3602565646388323
-  a: 0.69872924909981016
+  v: 4.37242495886841
+  a: 0.93736039866418519
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586343.84985840332
-    y: 4140690.6999009168
-    theta: 1.6526552362959843
-    kappa: -0.022236860426854334
-    s: 0.65033230943352771
+    x: 586343.84739926865
+    y: 4140690.7010541921
+    theta: 1.6599931318564205
+    kappa: -0.001506832055376069
+    s: 0.65169459275072317
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3881888968771783
-  a: 0.42065713943705285
+  v: 4.41503490120317
+  a: 0.76794620997247387
   relative_time: 0.15000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586343.83351092238
-    y: 4140690.91911555
-    theta: 1.6459540148096869
-    kappa: -0.028546376278489188
-    s: 0.87015563688328745
+    x: 586343.82791566814
+    y: 4140690.9218389657
+    theta: 1.6591478706369231
+    kappa: -0.0032674975020776124
+    s: 0.87333738554414575
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4025315884828409
-  a: 0.15514362379252078
+  v: 4.4493104713902811
+  a: 0.60398551275822643
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 586343.81713440316
-    y: 4140691.1387195764
-    theta: 1.6392407497873298
-    kappa: -0.034867099816606871
-    s: 1.0903694397662822
+    x: 586343.80830088723
+    y: 4140691.144129137
+    theta: 1.6582938295366652
+    kappa: -0.005045197188624396
+    s: 1.0964912782635217
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4039125691567333
-  a: -0.09781129783378617
+  v: 4.4755243440036168
+  a: 0.44547830702144309
   relative_time: 0.25
 }
 trajectory_point {
   path_point {
-    x: 586343.80268677929
-    y: 4140691.3582018958
-    theta: 1.6313484806450271
-    kappa: -0.040394704132963574
-    s: 1.3103413307784464
+    x: 586343.7886879188
+    y: 4140691.3675384107
+    theta: 1.657296511367335
+    kappa: -0.0068884831434351526
+    s: 1.3207600028945088
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3929597685997663
-  a: -0.338207625441868
+  v: 4.49394919361705
+  a: 0.29242459276212363
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586343.79162325966
-    y: 4140691.5770514151
-    theta: 1.6214181125345906
-    kappa: -0.0445316315664919
-    s: 1.5294703191007595
+    x: 586343.76989583182
+    y: 4140691.5917532016
+    theta: 1.6550633809194824
+    kappa: -0.0092405649035140116
+    s: 1.5457609251514595
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3703011165128505
-  a: -0.56604535903172493
+  v: 4.5048576948044534
+  a: 0.14482436998026826
   relative_time: 0.35
 }
 trajectory_point {
   path_point {
-    x: 586343.78063105524
-    y: 4140691.7944902391
-    theta: 1.6115516170483302
-    kappa: -0.048641892525025848
-    s: 1.7471868103992472
+    x: 586343.75107341062
+    y: 4140691.8163299207
+    theta: 1.652826638329514
+    kappa: -0.011596443400036254
+    s: 1.7711250444774183
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3365645425968973
-  a: -0.78132449860335651
+  v: 4.5085225221397014
+  a: 0.0026776386758768922
   relative_time: 0.39999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586343.77305875451
-    y: 4140692.0100956215
-    theta: 1.6008242659320149
-    kappa: -0.051829629865960188
-    s: 1.9629526068249805
+    x: 586343.7326924284
+    y: 4140692.0409496403
+    theta: 1.6502209405215775
+    kappa: -0.013954455734221435
+    s: 1.9964969940441248
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2923779765528192
-  a: -0.98404504415676319
+  v: 4.5052163501966662
+  a: -0.13401560115105049
   relative_time: 0.44999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586343.76909801271
-    y: 4140692.2233671467
-    theta: 1.5892084275905138
-    kappa: -0.054040991781273071
-    s: 2.1762609070140755
+    x: 586343.71576729033
+    y: 4140692.2653503139
+    theta: 1.6464269837403376
+    kappa: -0.016315605450544668
+    s: 2.2215350407520109
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.238369348081525
-  a: -1.1742069956919443
+  v: 4.4952118535492209
+  a: -0.26525534950051388
   relative_time: 0.49999999999999994
 }
 trajectory_point {
   path_point {
-    x: 586343.76519172953
-    y: 4140692.4337062766
-    theta: 1.5777522496235921
-    kappa: -0.056221948382669938
-    s: 2.3866363060876945
+    x: 586343.69889194146
+    y: 4140692.4890908604
+    theta: 1.6426441651494159
+    kappa: -0.01866980929082733
+    s: 2.4459110852302044
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1751665868839281
-  a: -1.3518103532089008
+  v: 4.4787817067712385
+  a: -0.39104160637251351
   relative_time: 0.54999999999999993
 }
 trajectory_point {
   path_point {
-    x: 586343.764827152
-    y: 4140692.6406697449
-    theta: 1.5660482338035726
-    kappa: -0.057781533430636453
-    s: 2.5936347956520454
+    x: 586343.68308731506
+    y: 4140692.7119263588
+    theta: 1.6383801788241914
+    kappa: -0.020924381276257495
+    s: 2.6693106618365241
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.103397622660939
-  a: -1.5168551167076321
+  v: 4.4561985844365939
+  a: -0.51137437176704892
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 586343.76852257713
-    y: 4140692.8438451095
-    theta: 1.5540508603124341
-    kappa: -0.05862940514184134
-    s: 2.796843763798381
+    x: 586343.66928328073
+    y: 4140692.9336192873
+    theta: 1.6331871062235097
+    kappa: -0.022994848614392993
+    s: 2.8914329386574842
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0236903851134684
-  a: -1.6693412861881383
+  v: 4.4277351611191582
+  a: -0.62625364568412056
   relative_time: 0.65
 }
 trajectory_point {
   path_point {
-    x: 586343.77214215591
-    y: 4140693.0428504259
-    theta: 1.5422992507572932
-    kappa: -0.059459874817027678
-    s: 2.995881995103
+    x: 586343.655576474
+    y: 4140693.1537507419
+    theta: 1.6280305728692526
+    kappa: -0.025050732804930063
+    s: 3.1119907175082915
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9366728039424284
-  a: -1.8092688616504193
+  v: 4.3936641113928063
+  a: -0.73567942812372822
   relative_time: 0.70000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586343.77727307391
-    y: 4140693.2372758258
-    theta: 1.5307656236876792
-    kappa: -0.060092067781043465
-    s: 3.1903996706272464
+    x: 586343.64339878724
+    y: 4140693.3721234389
+    theta: 1.6224572439775697
+    kappa: -0.026966577699579778
+    s: 3.3307104339328477
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8429728088487303
-  a: -1.9366378430944757
+  v: 4.3542581098314095
+  a: -0.83965171908587166
   relative_time: 0.75000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586343.78812031809
-    y: 4140693.4266441055
-    theta: 1.5193291183454043
-    kappa: -0.060050570270662976
-    s: 3.3800783679175104
+    x: 586343.63369951374
+    y: 4140693.5885279104
+    theta: 1.6161693768339549
+    kappa: -0.028658902194031111
+    s: 3.5473321572037468
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7432183295332848
-  a: -2.0514482305203066
+  v: 4.3097898310088434
+  a: -0.93817051857055134
   relative_time: 0.80000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586343.79867441894
-    y: 4140693.6108947704
-    theta: 1.5082001536960292
-    kappa: -0.060010194216806825
-    s: 3.5646310610052279
+    x: 586343.62410520611
+    y: 4140693.8025904419
+    theta: 1.6099494793560771
+    kappa: -0.030332912275749271
+    s: 3.7616095903222777
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.6380372956970031
-  a: -2.1537000239279114
+  v: 4.2605319494989793
+  a: -1.0312358265777672
   relative_time: 0.8500000000000002
 }
 trajectory_point {
   path_point {
-    x: 586343.80892075785
-    y: 4140693.7897726088
-    theta: 1.4973969090682675
-    kappa: -0.059970995545561075
-    s: 3.7438021204068788
+    x: 586343.61589989974
+    y: 4140694.0141222444
+    theta: 1.6035203398463593
+    kappa: -0.031899657125647018
+    s: 3.9733100700184214
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.528057637040797
-  a: -2.2433932233172924
+  v: 4.2067571398756911
+  a: -1.1188476431075189
   relative_time: 0.90000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586343.82314857084
-    y: 4140693.962721365
-    theta: 1.487073859081417
-    kappa: -0.059584495928600476
-    s: 3.9173673131239908
+    x: 586343.6109469732
+    y: 4140694.2229680177
+    theta: 1.5964736979212251
+    kappa: -0.03323061552818387
+    s: 4.1822145667508543
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.4139072832655772
-  a: -2.3205278286884479
+  v: 4.1487380767128519
+  a: -1.2010059681598064
   relative_time: 0.95000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586343.83939653856
-    y: 4140694.1296992037
-    theta: 1.4771750004861441
-    kappa: -0.059008738628336485
-    s: 4.085133802643135
+    x: 586343.60606520658
+    y: 4140694.4288132573
+    theta: 1.5895280772701736
+    kappa: -0.034542451746353939
+    s: 4.3881176847069456
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2962141640722562
-  a: -2.3851038400413782
+  v: 4.0867474345843346
+  a: -1.2777108017346297
   relative_time: 1.0000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586343.85506727407
-    y: 4140694.290744917
-    theta: 1.4676271668462315
-    kappa: -0.058453435925310743
-    s: 4.24694014893593
+    x: 586343.60143708787
+    y: 4140694.6314683235
+    theta: 1.5826623055255393
+    kappa: -0.035824632789468909
+    s: 4.5908276618027575
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1756062091617432
-  a: -2.4371212573760834
+  v: 4.0210578880640133
+  a: -1.3489621438319896
   relative_time: 1.0500000000000003
 }
 trajectory_point {
   path_point {
-    x: 586343.87014818285
-    y: 4140694.4457290722
-    theta: 1.4584397456941389
-    kappa: -0.057919034115560347
-    s: 4.4026563084590382
+    x: 586343.60152386525
+    y: 4140694.8308070125
+    theta: 1.5751727873574863
+    kappa: -0.036842798694075765
+    s: 4.7901663696830479
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.0527113482349515
-  a: -2.4765800806925635
+  v: 3.95194211172576
+  a: -1.4147599944518854
   relative_time: 1.1000000000000003
 }
 trajectory_point {
   path_point {
-    x: 586343.88759996858
-    y: 4140694.594205996
-    theta: 1.4498703086797438
-    kappa: -0.057215709650851675
-    s: 4.5521836341541695
+    x: 586343.60160910327
+    y: 4140695.0266099377
+    theta: 1.5678155690214024
+    kappa: -0.037842904914093409
+    s: 4.9859693137212666
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9281575109927904
-  a: -2.5034803099908181
+  v: 3.8796727801434487
+  a: -1.4751043535943165
   relative_time: 1.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586343.90701366751
-    y: 4140694.736155835
-    theta: 1.4418857917612233
-    kappa: -0.056369459061480477
-    s: 4.6954548754480765
+    x: 586343.6016927365
+    y: 4140695.2187262392
+    theta: 1.5605971228106068
+    kappa: -0.038824180894599672
+    s: 5.1780856330195562
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8025726271361733
-  a: -2.5178219452708475
+  v: 3.8045225678909524
+  a: -1.5299952212592847
   relative_time: 1.2000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586343.92557478917
-    y: 4140694.8718717671
-    theta: 1.43425114071141
-    kappa: -0.05556037264132338
-    s: 4.8324341782525613
+    x: 586343.604603535
+    y: 4140695.4069797564
+    theta: 1.5531962904769492
+    kappa: -0.039667940002321843
+    s: 5.366378100408757
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.67658462636601
-  a: -2.5196049865326522
+  v: 3.7267641495421446
+  a: -1.5794325974467878
   relative_time: 1.2500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586343.9432827296
-    y: 4140695.0013493751
-    theta: 1.4269675391676626
-    kappa: -0.054788476719804663
-    s: 4.963117084964467
+    x: 586343.60959651368
+    y: 4140695.5912571494
+    theta: 1.5457023727728172
+    kappa: -0.0404046207855919
+    s: 5.5507231224483977
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5508214383832111
-  a: -2.5088294337762318
+  v: 3.6466701996708979
+  a: -1.623416482156828
   relative_time: 1.3000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586343.960141139
-    y: 4140695.1246153493
-    theta: 1.4200341875579272
-    kappa: -0.054053612178845405
-    s: 5.0875305344656869
+    x: 586343.61447959743
+    y: 4140695.771478625
+    theta: 1.5383731663320264
+    kappa: -0.0411250873402077
+    s: 5.7310107394267042
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4259109928886904
-  a: -2.4854952870015863
+  v: 3.5645133928510857
+  a: -1.6619468753894031
   relative_time: 1.3500000000000005
 }
 trajectory_point {
   path_point {
-    x: 586343.97821632132
-    y: 4140695.241408431
-    theta: 1.413715600103256
-    kappa: -0.05323778384613273
-    s: 5.2057328621231562
+    x: 586343.61925017776
+    y: 4140695.9475478935
+    theta: 1.531213362743163
+    kappa: -0.041828954727645362
+    s: 5.9071446253605959
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3024812195833562
-  a: -2.4496025462087156
+  v: 3.4805664036565815
+  a: -1.6950237771445154
   relative_time: 1.4000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586343.99735908757
-    y: 4140695.3518425347
-    theta: 1.4079841160882862
-    kappa: -0.052349683762812904
-    s: 5.3178137997888575
+    x: 586343.62782613817
+    y: 4140696.1192198615
+    theta: 1.5239004224211938
+    kappa: -0.042374826760441811
+    s: 6.079042087995683
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1811600481681217
-  a: -2.4011512113976194
+  v: 3.3951019066612589
+  a: -1.7226471874221625
   relative_time: 1.4500000000000006
 }
 trajectory_point {
   path_point {
-    x: 586344.01547704428
-    y: 4140695.4563645395
-    theta: 1.402558941877561
-    kappa: -0.051509128186047887
-    s: 5.4238944757998189
+    x: 586343.63716199691
+    y: 4140696.2865516087
+    theta: 1.5166888010179034
+    kappa: -0.042871951890964176
+    s: 6.246634068806272
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0625754083438976
-  a: -2.3401412825682986
+  v: 3.3083925764389894
+  a: -1.7448171062223463
   relative_time: 1.5000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586344.03259624192
-    y: 4140695.5551247294
-    theta: 1.3974326562600612
-    kappa: -0.05071490854897507
-    s: 5.5241274149781132
+    x: 586343.64625492762
+    y: 4140696.4495292222
+    theta: 1.5096646618232485
+    kappa: -0.043356141341253381
+    s: 6.409865142995363
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9473552298115959
-  a: -2.2665727597207526
+  v: 3.2207110875636489
+  a: -1.7615335335450659
   relative_time: 1.5500000000000007
 }
 trajectory_point {
   path_point {
-    x: 586344.04874809308
-    y: 4140695.6483043237
-    theta: 1.3925961294820119
-    kappa: -0.049965567507182666
-    s: 5.6186965386308581
+    x: 586343.655102602
+    y: 4140696.6081109731
+    theta: 1.5028304820177214
+    kappa: -0.043827271147202147
+    s: 6.5686935194946505
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8361274422721263
-  a: -2.1804456428549805
+  v: 3.1323301146091076
+  a: -1.7727964693903213
   relative_time: 1.6000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586344.06396937289
-    y: 4140695.736115477
-    theta: 1.3880385413759473
-    kappa: -0.049259398938709655
-    s: 5.7078171645502209
+    x: 586343.6665672349
+    y: 4140696.7620656267
+    theta: 1.4960260602906392
+    kappa: -0.044187983778871689
+    s: 6.72309104096452
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7295199754264008
-  a: -2.0817599319709839
+  v: 3.0435223321492408
+  a: -1.7786059137581134
   relative_time: 1.6500000000000008
 }
 trajectory_point {
   path_point {
-    x: 586344.07830221858
-    y: 4140695.8188012778
-    theta: 1.3837473982450395
-    kappa: -0.048594447944045827
-    s: 5.7917360070134105
+    x: 586343.6794300084
+    y: 4140696.9114650716
+    theta: 1.4893192881768555
+    kappa: -0.044479608213019432
+    s: 6.8730431837940511
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6281607589753317
-  a: -1.9705156270687625
+  v: 2.9545604147579207
+  a: -1.7789618666484408
   relative_time: 1.7000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586344.09329739271
-    y: 4140695.89634983
-    theta: 1.3799850246229721
-    kappa: -0.047894678960831862
-    s: 5.87073117678268
+    x: 586343.69191138446
+    y: 4140697.056434636
+    theta: 1.482810993061241
+    kappa: -0.044762585618071796
+    s: 7.0185490581010184
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5326777226198285
-  a: -1.846712728148316
+  v: 2.8657170370090208
+  a: -1.7738643280613045
   relative_time: 1.7500000000000009
 }
 trajectory_point {
   path_point {
-    x: 586344.10838603938
-    y: 4140695.9691843456
-    theta: 1.3766205672019129
-    kappa: -0.047188174856737965
-    s: 5.9451121811053333
+    x: 586343.70401245647
+    y: 4140697.1969870175
+    theta: 1.4765011662343908
+    kappa: -0.0450369407780495
+    s: 7.1596214077318905
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4436987960608034
-  a: -1.7103512352096439
+  v: 2.7772648734764145
+  a: -1.7633132979967039
   relative_time: 1.8000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586344.12260782847
-    y: 4140696.0378344459
-    theta: 1.3734491947293668
-    kappa: -0.046522260106962296
-    s: 6.0152199237137181
+    x: 586343.71573548694
+    y: 4140697.3331484953
+    theta: 1.4703890919781106
+    kappa: -0.045302724991622148
+    s: 7.29628661026183
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3618519089991672
-  a: -1.5614311482527472
+  v: 2.6894765987339744
+  a: -1.74730877645464
   relative_time: 1.850000000000001
 }
 trajectory_point {
   path_point {
-    x: 586344.13603828335
-    y: 4140696.1026646905
-    theta: 1.3704541597015092
-    kappa: -0.045893398437237852
-    s: 6.0814267048252226
+    x: 586343.73055647034
+    y: 4140697.4646053808
+    theta: 1.464361946922522
+    kappa: -0.045439795833844408
+    s: 7.4285846769946895
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2877649911358322
-  a: -1.3999524672776249
+  v: 2.6026248873555744
+  a: -1.725850763435111
   relative_time: 1.900000000000001
 }
 trajectory_point {
   path_point {
-    x: 586344.14875929663
-    y: 4140696.1640703855
-    theta: 1.3676172699355349
-    kappa: -0.045297755355416
-    s: 6.1441362211422881
+    x: 586343.74550199357
+    y: 4140697.5917143226
+    theta: 1.4585112320527727
+    kappa: -0.045551356623139763
+    s: 7.5565692529630191
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2220659721717086
-  a: -1.2259151922842775
+  v: 2.5169824139150871
+  a: -1.6989392589381187
   relative_time: 1.9500000000000011
 }
 trajectory_point {
   path_point {
-    x: 586344.16085912928
-    y: 4140696.2224775748
-    theta: 1.3649188944716477
-    kappa: -0.044731198151466547
-    s: 6.2037835658523957
+    x: 586343.75995166111
+    y: 4140697.7146061035
+    theta: 1.4528543117429813
+    kappa: -0.045659216101093358
+    s: 7.6803076169280624
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1653827818077067
-  a: -1.03931932327271
+  v: 2.4328218529863861
+  a: -1.6665742629636644
   relative_time: 2.0000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586344.17241856351
-    y: 4140696.2782762027
-    theta: 1.362341056925243
-    kappa: -0.0441899443248814
-    s: 6.260766961959507
+    x: 586343.77391402412
+    y: 4140697.8333534477
+    theta: 1.4473882142232597
+    kappa: -0.045763438096661668
+    s: 7.7998729946349972
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1142200721326809
-  a: -1.0072781645590363
+  v: 2.3499447677755465
+  a: -1.6483927648198358
   relative_time: 2.0500000000000007
 }
 trajectory_point {
   path_point {
-    x: 586344.18346714368
-    y: 4140696.331608884
-    theta: 1.3598771975375563
-    kappa: -0.043672610504444244
-    s: 6.3152320512791817
+    x: 586343.78739517252
+    y: 4140697.9480081508
+    theta: 1.4421108605223234
+    kappa: -0.045864068067473049
+    s: 7.9153175359056949
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0646460552687325
-  a: -0.97577161082962038
+  v: 2.267994214569411
+  a: -1.6295129827755206
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586344.19402084791
-    y: 4140696.3825527458
-    theta: 1.357523773857392
-    kappa: -0.043178448531746932
-    s: 6.3672576001957442
+    x: 586343.80100975977
+    y: 4140698.0585356732
+    theta: 1.4370160283297655
+    kappa: -0.045938489358692776
+    s: 8.0266884401952669
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0166340009666484
-  a: -0.944799662084462
+  v: 2.1870051075630035
+  a: -1.6099349168307187
   relative_time: 2.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586344.20409538341
-    y: 4140696.4311836082
-    theta: 1.3552772971496183
-    kappa: -0.042706722943175505
-    s: 6.4169210385810569
+    x: 586343.81691183336
+    y: 4140698.1646975
+    theta: 1.4320855737114238
+    kappa: -0.0459068347327135
+    s: 8.1340346526685732
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.97015717897721565
-  a: -0.9143623183235613
+  v: 2.107012360951348
+  a: -1.5896585669854302
   relative_time: 2.2
 }
 trajectory_point {
   path_point {
-    x: 586344.21370618627
-    y: 4140696.4775759843
-    theta: 1.3531343335470156
-    kappa: -0.042256710969910107
-    s: 6.4642984597945228
+    x: 586343.83222520561
+    y: 4140698.266929172
+    theta: 1.4273371808834621
+    kappa: -0.045876351974008828
+    s: 8.23740686420023
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.9251888590512215
-  a: -0.88445957954691834
+  v: 2.0280508889294691
+  a: -1.5686839332396547
   relative_time: 2.25
 }
 trajectory_point {
   path_point {
-    x: 586344.22302666411
-    y: 4140696.5217679041
-    theta: 1.3511307749052852
-    kappa: -0.041821300701576185
-    s: 6.5094646206830822
+    x: 586343.84695764445
+    y: 4140698.3652825477
+    theta: 1.422768695616923
+    kappa: -0.045847025619895641
+    s: 8.3368575113746
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.88170231093945306
-  a: -0.85509144575453311
+  v: 1.950155605692391
+  a: -1.5470110155933925
   relative_time: 2.3
 }
 trajectory_point {
   path_point {
-    x: 586344.232967305
-    y: 4140696.5636322089
-    theta: 1.3494854453073997
-    kappa: -0.041363562778300417
-    s: 6.5524929415812174
+    x: 586343.86111717625
+    y: 4140698.4598112111
+    theta: 1.4183778350718446
+    kappa: -0.045818839692909862
+    s: 8.4324407764858
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.83967080439269748
-  a: -0.8262579169464056
+  v: 1.8733614254351383
+  a: -1.5246398140466435
   relative_time: 2.3499999999999996
 }
 trajectory_point {
   path_point {
-    x: 586344.24243070348
-    y: 4140696.6034866422
-    theta: 1.3479190502237157
-    kappa: -0.040927800496155478
-    s: 6.5934555063109483
+    x: 586343.87471208628
+    y: 4140698.5505704735
+    theta: 1.4141621931785842
+    kappa: -0.045791777700806412
+    s: 8.5242125875376935
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.79906760916174191
-  a: -0.79795899312253571
+  v: 1.7977032623527354
+  a: -1.5015703285994078
   relative_time: 2.3999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586344.25143320393
-    y: 4140696.641400035
-    theta: 1.346428899632615
-    kappa: -0.040513261240912424
-    s: 6.632423062181835
+    x: 586343.887750918
+    y: 4140698.6376173724
+    theta: 1.4101192456848521
+    kappa: -0.04576582263655922
+    s: 8.6122306182439026
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.7598659949973734
-  a: -0.77019467428292354
+  v: 1.7232160306402062
+  a: -1.4778025592516852
   relative_time: 2.4499999999999993
 }
 trajectory_point {
   path_point {
-    x: 586344.25999084208
-    y: 4140696.6774399225
-    theta: 1.3450123518247479
-    kappa: -0.04011920661624406
-    s: 6.6694650199909749
+    x: 586343.90024247381
+    y: 4140698.721010671
+    theta: 1.4062463548689221
+    kappa: -0.045740956978361245
+    s: 8.6965542880277962
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.72203923165037887
-  a: -0.7429649604275691
+  v: 1.6499346444925755
+  a: -1.4533365060034762
   relative_time: 2.4999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586344.26811934484
-    y: 4140696.7116725361
-    theta: 1.3436668138145336
-    kappa: -0.039744912443724904
-    s: 6.7046494540230084
+    x: 586343.91462987638
+    y: 4140698.8004067671
+    theta: 1.4025905927875637
+    kappa: -0.045617388626303927
+    s: 8.7772447620224927
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.6855605888715457
-  a: -0.71626985155647238
+  v: 1.5778940181048673
+  a: -1.4281721688547802
   relative_time: 2.5499999999999989
 }
 trajectory_point {
   path_point {
-    x: 586344.27583413024
-    y: 4140696.7441628082
-    theta: 1.3423897417007911
-    kappa: -0.039389668762831218
-    s: 6.7380431020501117
+    x: 586343.92846291512
+    y: 4140698.8762761983
+    theta: 1.3990979781599033
+    kappa: -0.045495917052288191
+    s: 8.8543649510708633
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.65040333641166093
-  a: -0.69010934766963339
+  v: 1.5071290656721059
+  a: -1.4023095478055974
   relative_time: 2.5999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586344.28315030783
-    y: 4140696.7749743727
-    theta: 1.3411786409818105
-    kappa: -0.039052779830940978
-    s: 6.7697113653320029
+    x: 586343.94166714989
+    y: 4140698.9486968564
+    theta: 1.3957639277311154
+    kappa: -0.045379967173747413
+    s: 8.9279795117255354
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.6165407440215116
-  a: -0.664483448767052
+  v: 1.4376747013893161
+  a: -1.375748642855928
   relative_time: 2.6499999999999986
 }
 trajectory_point {
   path_point {
-    x: 586344.290082678
-    y: 4140696.804169558
-    theta: 1.3400310668297142
-    kappa: -0.038733564123333904
-    s: 6.7997183086159385
+    x: 586343.954254491
+    y: 4140699.017734067
+    theta: 1.3925855305622452
+    kappa: -0.045269434400853334
+    s: 8.9981548462488785
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.583946081451885
-  a: -0.63939215484872836
+  v: 1.3695658394515218
+  a: -1.3484894540057717
   relative_time: 2.6999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586344.2966457326
-    y: 4140696.8318093987
-    theta: 1.3389446243285343
-    kappa: -0.038431354333191448
-    s: 6.8281266601367134
+    x: 586343.96623716224
+    y: 4140699.0834548725
+    theta: 1.3895597756062181
+    kappa: -0.045164211394120121
+    s: 9.06495910261302
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.55259261845356811
-  a: -0.61483546591466243
+  v: 1.3028373940537477
+  a: -1.3205319812551288
   relative_time: 2.7499999999999982
 }
 trajectory_point {
   path_point {
-    x: 586344.30285365484
-    y: 4140696.8579536229
-    theta: 1.3379169686800274
-    kappa: -0.038145497371596795
-    s: 6.8549978116166628
+    x: 586343.97762770043
+    y: 4140699.1459280327
+    theta: 1.3866835546155196
+    kappa: -0.045064188064404362
+    s: 9.1284621744998375
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.522453624777348
-  a: -0.590813381964854
+  v: 1.2375242793910177
+  a: -1.2918762246039992
   relative_time: 2.799999999999998
 }
 trajectory_point {
   path_point {
-    x: 586344.30872031883
-    y: 4140696.8826606628
-    theta: 1.3369458053808825
-    kappa: -0.037875354367534836
-    s: 6.8803918182656627
+    x: 586343.98843895551
+    y: 4140699.205224026
+    theta: 1.3839536647945287
+    kappa: -0.0449692515729051
+    s: 9.1887357013009563
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.49350237017401188
-  a: -0.56732590299930352
+  v: 1.1736614096583564
+  a: -1.2625221840523826
   relative_time: 2.8499999999999979
 }
 trajectory_point {
   path_point {
-    x: 586344.31425929011
-    y: 4140696.9059876488
-    theta: 1.3360288903746347
-    kappa: -0.037620300667892223
-    s: 6.9043673987811252
+    x: 586343.99868409056
+    y: 4140699.2614150466
+    theta: 1.3813668112064392
+    kappa: -0.044879286331163809
+    s: 9.2458530681177571
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.46571212439434695
-  a: -0.54437302901801066
+  v: 1.1112836990507879
+  a: -1.2324698596002794
   relative_time: 2.8999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586344.31948382547
-    y: 4140696.9279904095
-    theta: 1.3351640301812751
-    kappa: -0.037379725837457321
-    s: 6.9269819353480049
+    x: 586344.00837658183
+    y: 4140699.3145750072
+    theta: 1.3789196089454492
+    kappa: -0.04479417400106437
+    s: 9.29988940576137
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.43905615718914015
-  a: -0.52195476002097552
+  v: 1.050426061763337
+  a: -1.2017192512476895
   relative_time: 2.9499999999999975
 }
 trajectory_point {
   path_point {
-    x: 586344.324406873
-    y: 4140696.9487234759
-    theta: 1.3343490820072568
-    kappa: -0.03715303365892024
-    s: 6.9482914736387933
+    x: 586344.01753021847
+    y: 4140699.3647795371
+    theta: 1.3766085850855061
+    kappa: -0.044713793494833134
+    s: 9.3509215907526766
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.41350773830917853
-  a: -0.5000710960081981
+  v: 0.99112341199102749
+  a: -1.1702703589946126
   relative_time: 2.9999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586344.32904107182
-    y: 4140696.9682400757
-    theta: 1.3335819538383291
-    kappa: -0.036939642132872805
-    s: 6.9683507228135229
+    x: 586344.02636767854
+    y: 4140699.4120645928
+    theta: 1.3744411842393776
+    kappa: -0.044628465950772904
+    s: 9.3990282453223077
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.38904013750524946
-  a: -0.4787220369796783
+  v: 0.93341066392888417
+  a: -1.1381231828410492
   relative_time: 3.0499999999999972
 }
 trajectory_point {
   path_point {
-    x: 586344.33339875238
-    y: 4140696.9865921391
-    theta: 1.3328606045173776
-    kappa: -0.036738983477808566
-    s: 6.9872130555197653
+    x: 586344.03586743341
+    y: 4140699.456317924
+    theta: 1.3724644788654778
+    kappa: -0.044493900451225
+    s: 9.4442897374106511
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.36562662452814
-  a: -0.45790758293541634
+  v: 0.87732273177193121
+  a: -1.105277722786999
   relative_time: 3.099999999999997
 }
 trajectory_point {
   path_point {
-    x: 586344.33749193628
-    y: 4140697.0038302932
-    theta: 1.3321830438092228
-    kappa: -0.036550504130122848
-    s: 7.00493050789263
+    x: 586344.044787263
+    y: 4140699.497869751
+    theta: 1.3706083523006281
+    kappa: -0.044367549684092768
+    s: 9.4867881806678387
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.34324046912863693
-  a: -0.437627733875412
+  v: 0.82289452971519284
+  a: -1.0717339788324618
   relative_time: 3.1499999999999968
 }
 trajectory_point {
   path_point {
-    x: 586344.34133233654
-    y: 4140697.0200038664
-    theta: 1.3315473324541232
-    kappa: -0.036373664744112646
-    s: 7.0215537795547682
+    x: 586344.05314476835
+    y: 4140699.5368020646
+    theta: 1.3688691706777247
+    kappa: -0.044249164329786095
+    s: 9.5266074344537568
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.321854941057528
-  a: -0.41788248979966525
+  v: 0.77016097195369349
+  a: -1.0374919509774383
   relative_time: 3.1999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586344.34493135731
-    y: 4140697.0351608875
-    theta: 1.3309515822115312
-    kappa: -0.03620794019197672
-    s: 7.0371322336163695
+    x: 586344.06095791631
+    y: 4140699.5731985644
+    theta: 1.3672432178404113
+    kappa: -0.044138489878600869
+    s: 9.5638331038380446
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.30144331006559977
-  a: -0.39867185070817623
+  v: 0.71915697268245715
+  a: -1.0025516392219278
   relative_time: 3.2499999999999964
 }
 trajectory_point {
   path_point {
-    x: 586344.34830009378
-    y: 4140697.0493480829
-    theta: 1.330393955895484
-    kappa: -0.03605281956381557
-    s: 7.0517138966751611
+    x: 586344.068245041
+    y: 4140699.6071446547
+    theta: 1.3657266963349264
+    kappa: -0.044035266630718939
+    s: 9.59855253960009
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.28197884590363964
-  a: -0.37999581660094506
+  v: 0.66991744609650916
+  a: -0.96691304356593044
   relative_time: 3.2999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586344.35144933278
-    y: 4140697.06261088
-    theta: 1.3298726674028485
-    kappa: -0.0359078061676314
-    s: 7.0653454588164122
+    x: 586344.07502484228
+    y: 4140699.638727447
+    theta: 1.3643157282789622
+    kappa: -0.043939229696208136
+    s: 9.630854838229034
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.26343481832243432
-  a: -0.3618543874779715
+  v: 0.62247730639087306
+  a: -0.93057616400944643
   relative_time: 3.3499999999999961
 }
 trajectory_point {
   path_point {
-    x: 586344.35438955191
-    y: 4140697.0749934046
-    theta: 1.3293859817354936
-    kappa: -0.035772417529328158
-    s: 7.07807227361293
+    x: 586344.08131638682
+    y: 4140699.668035761
+    theta: 1.3630063561174457
+    kappa: -0.043850108995022272
+    s: 9.660830841923767
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.24578449707277161
-  a: -0.34424756333925566
+  v: 0.57687146776057285
+  a: -0.89354100055247576
   relative_time: 3.3999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586344.35713092051
-    y: 4140697.0865384843
-    theta: 1.3289322150173417
-    kappa: -0.03564618539271152
-    s: 7.0899383581250621
+    x: 586344.08713910729
+    y: 4140699.6951601217
+    theta: 1.3617945432749534
+    kappa: -0.043767629257001145
+    s: 9.68857313859293
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.22900115190543802
-  a: -0.32717534418479743
+  v: 0.53313484440063363
+  a: -0.85580755319501811
   relative_time: 3.4499999999999957
 }
 trajectory_point {
   path_point {
-    x: 586344.35968329862
-    y: 4140697.0972876442
-    theta: 1.3285097345071233
-    kappa: -0.035528655719488905
-    s: 7.1009863929006922
+    x: 586344.09251280339
+    y: 4140699.7201927621
+    theta: 1.3606761747142144
+    kappa: -0.043691510021870508
+    s: 9.71417606185492
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.21305805257122079
-  a: -0.31063773001459694
+  v: 0.4913023505060794
+  a: -0.81737582193707392
   relative_time: 3.4999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586344.362056238
-    y: 4140697.1072811102
-    theta: 1.3281169586075616
-    kappa: -0.035419388689269443
-    s: 7.1112577219752486
+    x: 586344.09745764046
+    y: 4140699.7432276206
+    theta: 1.3596470574098403
+    kappa: -0.0436214656392421
+    s: 9.7377356910378783
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.19792846882090742
-  a: -0.29463472082865416
+  v: 0.45140890027193481
+  a: -0.778245806778643
   relative_time: 3.5499999999999954
 }
 trajectory_point {
   path_point {
-    x: 586344.36425898108
-    y: 4140697.1165578072
-    theta: 1.327752356871613
-    kappa: -0.035317958699564007
-    s: 7.1207923528716943
+    x: 586344.10199415078
+    y: 4140699.764360344
+    theta: 1.3587029207460719
+    kappa: -0.043557205268613661
+    s: 9.759349851179703
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.18358567040528451
-  a: -0.27916631662696911
+  v: 0.41348940789322386
+  a: -0.73841750771972514
   relative_time: 3.5999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586344.36630046228
-    y: 4140697.12515536
-    theta: 1.3274144500063076
-    kappa: -0.035223954365785189
-    s: 7.1296289566005351
+    x: 586344.10614323278
+    y: 4140699.7836882845
+    theta: 1.3578394168469308
+    kappa: -0.043498432879368887
+    s: 9.77911811302804
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.17000292707513975
-  a: -0.26423251740954179
+  v: 0.37757878756497076
+  a: -0.69789092476032066
   relative_time: 3.649999999999995
 }
 trajectory_point {
   path_point {
-    x: 586344.36818930658
-    y: 4140697.1331100939
-    theta: 1.3271018098746596
-    kappa: -0.035136978521247322
-    s: 7.1378048676598134
+    x: 586344.10992615146
+    y: 4140699.8013105025
+    theta: 1.3570521208467343
+    kappa: -0.043444847250777449
+    s: 9.7971417930402929
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.15715350858125965
-  a: -0.24983332317637208
+  v: 0.34371195348219974
+  a: -0.65666605790042953
   relative_time: 3.6999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586344.36993383034
-    y: 4140697.1404570327
-    theta: 1.3268130594960419
-    kappa: -0.035056648217166482
-    s: 7.1453560840351118
+    x: 586344.11336453806
+    y: 4140699.8173277648
+    theta: 1.3563365311084778
+    kappa: -0.043396141971995025
+    s: 9.8135239533836032
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.14501068467443146
-  a: -0.23596873392746021
+  v: 0.31192381983993611
+  a: -0.6147429071400512
   relative_time: 3.7499999999999947
 }
 trajectory_point {
   path_point {
-    x: 586344.37154204142
-    y: 4140697.1472298997
-    theta: 1.3265468730453731
-    kappa: -0.034982594722660451
-    s: 7.1523172671995532
+    x: 586344.11648039031
+    y: 4140699.8318425436
+    theta: 1.3556880693970976
+    kappa: -0.043352005442063225
+    s: 9.8283694019348768
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.13354772510544266
-  a: -0.22263874966280583
+  v: 0.282249300833203
+  a: -0.57212147247918632
   relative_time: 3.7999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586344.3730216386
-    y: 4140697.1534611187
-    theta: 1.3263019758513961
-    kappa: -0.034914463524748758
-    s: 7.1587217421138
+    x: 586344.1192960724
+    y: 4140699.8449590197
+    theta: 1.3551020810141376
+    kappa: -0.043312120869909675
+    s: 9.8417846922807666
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.12273789962507986
-  a: -0.2098433703824093
+  v: 0.25472331065702525
+  a: -0.52880175391783513
   relative_time: 3.8499999999999943
 }
 trajectory_point {
   path_point {
-    x: 586344.37438001216
-    y: 4140697.1591818128
-    theta: 1.3260771443942967
-    kappa: -0.034851914328352648
-    s: 7.1646014972260517
+    x: 586344.12183431478
+    y: 4140699.8567830804
+    theta: 1.3545738348998204
+    kappa: -0.043276166274347973
+    s: 9.8538781237176725
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.11255447798413099
-  a: -0.19758259608627027
+  v: 0.22938076350642822
+  a: -0.48478375145599673
   relative_time: 3.8999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586344.3756242434
-    y: 4140697.1644218047
-    theta: 1.3258712063028559
-    kappa: -0.034794621056295112
-    s: 7.1699871844720491
+    x: 586344.12411821436
+    y: 4140699.867422319
+    theta: 1.3540985237080159
+    kappa: -0.043243814484077672
+    s: 9.8647597412517527
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.10297072993338219
-  a: -0.18585642677438918
+  v: 0.20625657357643457
+  a: -0.44006746509367156
   relative_time: 3.949999999999994
 }
 trajectory_point {
   path_point {
-    x: 586344.376761105
-    y: 4140697.1692096172
-    theta: 1.3256830403513074
-    kappa: -0.034742271849300864
-    s: 7.1749081192750728
+    x: 586344.1261712343
+    y: 4140699.8769860365
+    theta: 1.3536712638590751
+    kappa: -0.043214733137684348
+    s: 9.87454133559891
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.093959925223621177
-  a: -0.17466486244676571
+  v: 0.18538565506206917
+  a: -0.39465289483086008
   relative_time: 3.9999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586344.37779701722
-    y: 4140697.1735722879
-    theta: 1.3255115836643077
-    kappa: -0.034694571071408845
-    s: 7.1793920920329777
+    x: 586344.128015533
+    y: 4140699.8855774538
+    theta: 1.3532874434656179
+    kappa: -0.043188608362907173
+    s: 9.883328478595141
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.085484526088754745
-  a: -0.16439994290393989
+  v: 0.16632175448208844
+  a: -0.36805013144716414
   relative_time: 4.0499999999999936
 }
 trajectory_point {
   path_point {
-    x: 586344.378737964
-    y: 4140697.177535018
-    theta: 1.3253558456408232
-    kappa: -0.034651243184721424
-    s: 7.1834650038836774
+    x: 586344.12966667139
+    y: 4140699.8932690611
+    theta: 1.3529438235357281
+    kappa: -0.043165219729730878
+    s: 9.8911953125089056
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.077515046937622623
-  a: -0.15442806309739229
+  v: 0.1485659416094724
+  a: -0.34232938653628253
   relative_time: 4.0999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586344.3795960967
-    y: 4140697.181120466
-    theta: 1.3252169800584925
-    kappa: -0.034611812023605346
-    s: 7.187151784526689
+    x: 586344.131138146
+    y: 4140699.9001237275
+    theta: 1.3526375949553067
+    kappa: -0.043144376064338151
+    s: 9.8982061392024825
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.070036835783410939
-  a: -0.14474922302712284
+  v: 0.13207411552058013
+  a: -0.31749066009821281
   relative_time: 4.1499999999999932
 }
 trajectory_point {
   path_point {
-    x: 586344.38043933536
-    y: 4140697.184336605
-    theta: 1.3251146076925366
-    kappa: -0.034573963170594205
-    s: 7.1904766310621868
+    x: 586344.13244299
+    y: 4140699.9062021673
+    theta: 1.3523660449810384
+    kappa: -0.04312589274866422
+    s: 9.9044230554919661
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.063035240639305767
-  a: -0.13536342269313148
+  v: 0.11680217529177103
+  a: -0.2935339521329548
   relative_time: 4.1999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586344.38119673228
-    y: 4140697.1872253413
-    theta: 1.3250226566538257
-    kappa: -0.034539967333430587
-    s: 7.1934630079910074
+    x: 586344.1335937737
+    y: 4140699.9115629382
+    theta: 1.3521265572423138
+    kappa: -0.043109591720396842
+    s: 9.909905953147268
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0564956095184932
-  a: -0.12627066209541823
+  v: 0.10270601999940454
+  a: -0.27045926264050857
   relative_time: 4.2499999999999929
 }
 trajectory_point {
   path_point {
-    x: 586344.38187405269
-    y: 4140697.189808663
-    theta: 1.3249404270828764
-    kappa: -0.034509565740351925
-    s: 7.1961336472146442
+    x: 586344.13460260467
+    y: 4140699.9162624413
+    theta: 1.3519166117400909
+    kappa: -0.043095301472976286
+    s: 9.91471251889212
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.050403290434159351
-  a: -0.11747094123398312
+  v: 0.089741548719840081
+  a: -0.24826659162087411
   relative_time: 4.2999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586344.38247687591
-    y: 4140697.1921078507
-    theta: 1.3248672416653273
-    kappa: -0.034482507959242112
-    s: 7.1985105480352507
+    x: 586344.13548112777
+    y: 4140699.9203549237
+    theta: 1.35173378484364
+    kappa: -0.043082857055595357
+    s: 9.9188982344040717
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.044743631399490294
-  a: -0.10896426010882609
+  v: 0.077864660529437038
+  a: -0.22695593907405143
   relative_time: 4.3499999999999925
 }
 trajectory_point {
   path_point {
-    x: 586344.38301059557
-    y: 4140697.1941434746
-    theta: 1.3248024456337542
-    kappa: -0.03445855189763157
-    s: 7.2006149771556407
+    x: 586344.13624052529
+    y: 4140699.9238924747
+    theta: 1.3515757492859117
+    kappa: -0.043072100073199365
+    s: 9.9225163763144888
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.03950198042767214
-  a: -0.10075061871994719
+  v: 0.067031254504554827
+  a: -0.20652730500004055
   relative_time: 4.3999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586344.3834804194
-    y: 4140697.1959353983
-    theta: 1.3247454067692488
-    kappa: -0.034437463802697188
-    s: 7.2024674686792851
+    x: 586344.13689151616
+    y: 4140699.9269250282
+    theta: 1.3514402741581191
+    kappa: -0.043062878686486165
+    s: 9.9256180162085581
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.034663685531890975
-  a: -0.0928300170673464
+  v: 0.057197229721552861
+  a: -0.18698068939884144
   relative_time: 4.4499999999999922
 }
 trajectory_point {
   path_point {
-    x: 586344.38389136968
-    y: 4140697.1975027756
-    theta: 1.3246955154027895
-    kappa: -0.034419018261262369
-    s: 7.2040878241103172
+    x: 586344.13744435692
+    y: 4140699.9295003624
+    theta: 1.3513252249039758
+    kappa: -0.043055047611906122
+    s: 9.9282520206252816
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.030214094725332888
-  a: -0.0852024551510237
+  v: 0.048318485256790555
+  a: -0.16831609227045408
   relative_time: 4.499999999999992
 }
 trajectory_point {
   path_point {
-    x: 586344.38424828229
-    y: 4140697.1988640521
-    theta: 1.3246521844164278
-    kappa: -0.03440299819979703
-    s: 7.2054951123535256
+    x: 586344.13790884113
+    y: 4140699.9316641
+    theta: 1.3512285633139298
+    kappa: -0.043048468121662138
+    s: 9.93046505105748
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.026138556021184006
-  a: -0.077867932970979145
+  v: 0.040350920186627309
+  a: -0.15053351361487854
   relative_time: 4.5499999999999918
 }
 trajectory_point {
   path_point {
-    x: 586344.38455580792
-    y: 4140697.2000369644
-    theta: 1.3246148492443119
-    kappa: -0.034389194884417537
-    s: 7.2067076697143619
+    x: 586344.13829429948
+    y: 4140699.9334597057
+    theta: 1.3511483475196284
+    kappa: -0.043043008043709627
+    s: 9.9323015639517926
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.022422417432630376
-  a: -0.070826450527212675
+  v: 0.033250433587422551
+  a: -0.13363295343211479
   relative_time: 4.5999999999999917
 }
 trajectory_point {
   path_point {
-    x: 586344.38481841085
-    y: 4140697.2010385408
-    theta: 1.3245829678735694
-    kappa: -0.03437740792088681
-    s: 7.2077430998989351
+    x: 586344.13860960014
+    y: 4140699.9349284912
+    theta: 1.3510827319887824
+    kappa: -0.043038541761756526
+    s: 9.9338038107086781
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.019051026972858126
-  a: -0.064078007819724309
+  v: 0.026972924535535681
+  a: -0.11761441172216275
   relative_time: 4.6499999999999915
 }
 trajectory_point {
   path_point {
-    x: 586344.38504037017
-    y: 4140697.2018851009
-    theta: 1.3245560208450637
-    kappa: -0.034367445254614208
-    s: 7.2086182740140137
+    x: 586344.13886314817
+    y: 4140699.9361096104
+    theta: 1.3510299675205302
+    kappa: -0.043034950215263316
+    s: 9.9350118376824081
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.01600973265505333
-  a: -0.057622604848514086
+  v: 0.021474292107326126
+  a: -0.10247788848502254
   relative_time: 4.6999999999999913
 }
 trajectory_point {
   path_point {
-    x: 586344.38522577879
-    y: 4140697.2025922551
-    theta: 1.3245335112540393
-    kappa: -0.03435912317065562
-    s: 7.2093493305670266
+    x: 586344.13906288589
+    y: 4140699.9370400617
+    theta: 1.3509884012413591
+    kappa: -0.043032120899442985
+    s: 9.9359634861810768
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.013283882492402102
-  a: -0.051460241613581967
+  v: 0.01671043537915326
+  a: -0.0882233837206941
   relative_time: 4.7499999999999911
 }
 trajectory_point {
   path_point {
-    x: 586344.38537854387
-    y: 4140697.2031749059
-    theta: 1.3245149647506698
-    kappa: -0.034352266293713456
-    s: 7.2099516754660593
+    x: 586344.13921629288
+    y: 4140699.9377546874
+    theta: 1.3509564766015973
+    kappa: -0.043029947865261042
+    s: 9.9366943924665971
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.010858824498090516
-  a: -0.045590918114927936
+  v: 0.012637253427376538
+  a: -0.074850897429177443
   relative_time: 4.7999999999999909
 }
 trajectory_point {
   path_point {
-    x: 586344.385502387
-    y: 4140697.2036472475
-    theta: 1.32449992954052
-    kappa: -0.034346707588136551
-    s: 7.210439982019861
+    x: 586344.139330386
+    y: 4140699.938286175
+    theta: 1.3509327333724745
+    kappa: -0.043028331719435542
+    s: 9.9372379877546937
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0087199066853046875
-  a: -0.040014634352552036
+  v: 0.00921064532835536
+  a: -0.062360429610472556
   relative_time: 4.8499999999999908
 }
 trajectory_point {
   path_point {
-    x: 586344.38560084347
-    y: 4140697.2040227638
-    theta: 1.324487976384932
-    kappa: -0.034342288357920317
-    s: 7.2108281909378356
+    x: 586344.13941171893
+    y: 4140699.9386650538
+    theta: 1.3509158076437107
+    kappa: -0.043027179624437029
+    s: 9.937625498214917
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0068524770672307028
-  a: -0.034731390326454253
+  v: 0.0063865101584491268
+  a: -0.050751980264579388
   relative_time: 4.8999999999999906
 }
 trajectory_point {
   path_point {
-    x: 586344.38567726326
-    y: 4140697.2043142314
-    theta: 1.3244786986013448
-    kappa: -0.034338858246706593
-    s: 7.211129510330049
+    x: 586344.139466383
+    y: 4140699.9389196998
+    theta: 1.350904431821599
+    kappa: -0.043026405298488613
+    s: 9.9378859449706312
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0052418836570546357
-  a: -0.029741186036634559
+  v: 0.0041207469940172381
+  a: -0.040025549391498105
   relative_time: 4.94999999999999
 }
 trajectory_point {
   path_point {
-    x: 586344.38573481026
-    y: 4140697.2045337181
-    theta: 1.3244717120635567
-    kappa: -0.034336275237783778
-    s: 7.2113564157072254
+    x: 586344.13950000668
+    y: 4140699.9390763305
+    theta: 1.350897434627524
+    kappa: -0.043025929015565893
+    s: 9.9380461440990171
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.003873474467962601
-  a: -0.02504402148309301
+  v: 0.0023692549114191219
+  a: -0.030181136991228541
   relative_time: 4.99999999999999
 }
 trajectory_point {
   path_point {
-    x: 586344.385776463
-    y: 4140697.2046925826
-    theta: 1.3244666552019353
-    kappa: -0.034334405654086721
-    s: 7.2115206499807494
+    x: 586344.13951775513
+    y: 4140699.9391590091
+    theta: 1.3508937410968584
+    kappa: -0.043025677605397016
+    s: 9.9381307066310764
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0027325975131407138
-  a: -0.020639896665829521
+  v: 0.0010879329870142063
+  a: -0.021218743063770751
   relative_time: 5.04999999999999
 }
 trajectory_point {
   path_point {
-    x: 586344.38580501359
-    y: 4140697.2048014752
-    theta: 1.3244631890035858
-    kappa: -0.034333124158196775
-    s: 7.2116332234626634
+    x: 586344.13952433132
+    y: 4140699.9391896431
+    theta: 1.3508923725781841
+    kappa: -0.043025584453462634
+    s: 9.9381620385516278
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0018046008057750335
-  a: -0.016528811584844177
+  v: 0.00023268029716186356
+  a: -0.01313836760912479
   relative_time: 5.09999999999999
 }
 trajectory_point {
   path_point {
-    x: 586344.38582306867
-    y: 4140697.2048703381
-    theta: 1.3244609970124803
-    kappa: -0.034332313752341817
-    s: 7.2117044138656707
+    x: 586344.13952397485
+    y: 4140699.9391879831
+    theta: 1.3508924467327743
+    kappa: -0.043025589500995956
+    s: 9.9381603407993069
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0010748323590516889
-  a: -0.01271076624013695
+  v: -0.0002406040817784505
+  a: -0.0059400106272904929
   relative_time: 5.14999999999999
 }
 trajectory_point {
   path_point {
-    x: 586344.38583304919
-    y: 4140697.204908404
-    theta: 1.324459785329555
-    kappa: -0.034331865778396177
-    s: 7.2117437663031332
+    x: 586344.13952046318
+    y: 4140699.9391716244
+    theta: 1.3508931775342861
+    kappa: -0.043025639244982664
+    s: 9.93814360926657
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00052864018615671193
-  a: -0.00918576063170784
+  v: -0.00037602107344736346
+  a: 0.00037632788173186382
   relative_time: 5.1999999999999895
 }
 trajectory_point {
   path_point {
-    x: 586344.38583719
-    y: 4140697.2049241969
-    theta: 1.3244592826127808
-    kappa: -0.034331679917880734
-    s: 7.2117600932890706
+    x: 586344.13951711042
+    y: 4140699.9391560061
+    theta: 1.3508938752686104
+    kappa: -0.043025686738161
+    s: 9.93812763479969
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00015137230027624515
-  a: -0.0059537947595568186
+  v: -0.0002176716014854474
+  a: 0.0058106479179425574
   relative_time: 5.2499999999999893
 }
 trajectory_point {
   path_point {
-    x: 586344.38583754026
-    y: 4140697.2049255334
-    theta: 1.3244592400772119
-    kappa: -0.034331664191962825
-    s: 7.2117614747381635
+    x: 586344.13951676793
+    y: 4140699.9391544107
+    theta: 1.3508939465338359
+    kappa: -0.043025691589021739
+    s: 9.9381260031987555
     dkappa: 0
     ddkappa: 0
   }
-  v: -7.1623285403624171e-05
-  a: -0.0030148686236838862
+  v: 0.00019034341046672565
+  a: 0.010362949481341477
   relative_time: 5.2999999999999892
 }
 trajectory_point {
   path_point {
-    x: 586344.38583596365
-    y: 4140697.2049195203
-    theta: 1.3244594314950153
-    kappa: -0.0343317349614563
-    s: 7.2117552579657529
+    x: 586344.13952182455
+    y: 4140699.9391779662
+    theta: 1.3508928942402827
+    kappa: -0.043025619961808148
+    s: 9.9381500952176758
     dkappa: 0
     ddkappa: 0
   }
-  v: -0.00015499855769676718
-  a: -0.00036898222408909831
+  v: 0.00080392303876844484
+  a: 0.014033232571928511
   relative_time: 5.349999999999989
 }
 trajectory_point {
   path_point {
-    x: 586344.38583413756
-    y: 4140697.2049125554
-    theta: 1.3244596531954871
-    kappa: -0.034331816926821482
-    s: 7.2117480576878368
+    x: 586344.139534206
+    y: 4140699.9392356435
+    theta: 1.3508903176105758
+    kappa: -0.043025444576516053
+    s: 9.9382090865641768
     dkappa: 0
     ddkappa: 0
   }
-  v: -0.00011340550341716604
-  a: 0.0019838644392275728
+  v: 0.0015789663597793602
+  a: 0.016821497189703882
   relative_time: 5.3999999999999888
 }
 trajectory_point {
   path_point {
-    x: 586344.38583355374
-    y: 4140697.2049103286
-    theta: 1.324459724065058
-    kappa: -0.034331843128165236
-    s: 7.2117457560210738
+    x: 586344.13955537532
+    y: 4140699.9393342584
+    theta: 1.3508859121797239
+    kappa: -0.043025144708893751
+    s: 9.9383099478998052
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8503890621363612e-05
-  a: 0.0040436713662661272
+  v: 0.00247137244985865
+  a: 0.018727743334667479
   relative_time: 5.4499999999999886
 }
 trajectory_point {
   path_point {
-    x: 586344.38583551848
-    y: 4140697.204917822
-    theta: 1.3244594855472904
-    kappa: -0.034331754945240885
-    s: 7.2117535024827815
+    x: 586344.139586333
+    y: 4140699.93947847
+    theta: 1.350879469795186
+    kappa: -0.043024706190442144
+    s: 9.93845744483992
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00028607763760482574
-  a: 0.0058104385570265926
+  v: 0.0034370403853658527
+  a: 0.019751971006819247
   relative_time: 5.4999999999999885
 }
 trajectory_point {
   path_point {
-    x: 586344.3858411517
-    y: 4140697.2049393067
-    theta: 1.3244588016428727
-    kappa: -0.034331502097448258
-    s: 7.2117757139909369
+    x: 586344.139627616
+    y: 4140699.9396707816
+    theta: 1.3508708786169019
+    kappa: -0.043024121408414578
+    s: 9.9386541379537032
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00061466375071936308
-  a: 0.0072841660115089413
+  v: 0.0044318692426603412
+  a: 0.019894180206159184
   relative_time: 5.5499999999999883
 }
 trajectory_point {
   path_point {
-    x: 586344.38585138787
-    y: 4140697.2049783482
-    theta: 1.3244575589096081
-    kappa: -0.034331042643833687
-    s: 7.2118160748641751
+    x: 586344.13967929943
+    y: 4140699.9399115415
+    theta: 1.3508601231172799
+    kappa: -0.043023389305816978
+    s: 9.9389003827641549
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0010096102431510906
-  a: 0.0084648537297131732
+  v: 0.0054117580981015989
+  a: 0.019154370932687403
   relative_time: 5.5999999999999881
 }
 trajectory_point {
   path_point {
-    x: 586344.38586697564
-    y: 4140697.2050378006
-    theta: 1.3244556664624074
-    kappa: -0.034330342983090005
-    s: 7.2118775368217936
+    x: 586344.13974099478
+    y: 4140699.9401989412
+    theta: 1.350847284081131
+    kappa: -0.04302251538140775
+    s: 9.9391943297480889
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0014562651280860539
-  a: 0.0093525017116392883
+  v: 0.0063326060280489427
+  a: 0.017532543186403904
   relative_time: 5.6499999999999879
 }
 trajectory_point {
   path_point {
-    x: 586344.38588847779
-    y: 4140697.205119811
-    theta: 1.3244530559732801
-    kappa: -0.034329377853556525
-    s: 7.2119623189837458
+    x: 586344.1398118512
+    y: 4140699.9405290158
+    theta: 1.3508325386055504
+    kappa: -0.043021511689697867
+    s: 9.93953192433614
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0019399764187103957
-  a: 0.0099471099572873145
+  v: 0.0071503121088618005
+  a: 0.015028696967308575
   relative_time: 5.6999999999999877
 }
 trajectory_point {
   path_point {
-    x: 586344.3859162716
-    y: 4140697.2052258165
-    theta: 1.3244496816713289
-    kappa: -0.034328130333219069
-    s: 7.2120719078706461
+    x: 586344.13989055483
+    y: 4140699.9408956459
+    theta: 1.3508161600997508
+    kappa: -0.043020396840950811
+    s: 9.9399069069127624
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0024460921282101478
-  a: 0.010248678466657224
+  v: 0.0078207754168995447
+  a: 0.011642832275401471
   relative_time: 5.7499999999999876
 }
 trajectory_point {
   path_point {
-    x: 586344.38595054776
-    y: 4140697.2053565476
-    theta: 1.3244455203427508
-    kappa: -0.034326591839709965
-    s: 7.2122070574037673
+    x: 586344.13997532893
+    y: 4140699.9412905555
+    theta: 1.3507985182848541
+    kappa: -0.043019196001182564
+    s: 9.9403108128162234
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0029599602697714389
-  a: 0.010257207239748989
+  v: 0.0082998950285217143
+  a: 0.0073749491106825382
   relative_time: 5.7999999999999874
 }
 trajectory_point {
   path_point {
-    x: 586344.3859913121
-    y: 4140697.205512024
-    theta: 1.3244405713308394
-    kappa: -0.034324762130308
-    s: 7.2123677889050439
+    x: 586344.14006393438
+    y: 4140699.9417033121
+    theta: 1.3507800791936628
+    kappa: -0.043017940892161668
+    s: 9.9407329723386137
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0034669288565803841
-  a: 0.0099726962765626648
+  v: 0.00854357002008771
+  a: 0.0022250474731518866
   relative_time: 5.8499999999999872
 }
 trajectory_point {
   path_point {
-    x: 586344.38603838393
-    y: 4140697.2056915574
-    theta: 1.3244348565359951
-    kappa: -0.034322649301938513
-    s: 7.2125533910970665
+    x: 586344.1401536687
+    y: 4140699.9421213269
+    theta: 1.3507614051704271
+    kappa: -0.043016669791409175
+    s: 9.9411605107258385
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0039523459018230567
-  a: 0.009395145577098224
+  v: 0.0085076994679567641
+  a: -0.0038068726371905393
   relative_time: 5.899999999999987
 }
 trajectory_point {
   path_point {
-    x: 586344.38609139749
-    y: 4140697.2058937526
-    theta: 1.3244284204157395
-    kappa: -0.0343202697911733
-    s: 7.2127624201030862
+    x: 586344.14024136693
+    y: 4140699.9425298572
+    theta: 1.3507431548706388
+    kappa: -0.043015427532198648
+    s: 9.94157834817762
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.004401559418685641
-  a: 0.0085245551413556941
+  v: 0.0081481824484884446
+  a: -0.010720811220344739
   relative_time: 5.9499999999999869
 }
 trajectory_point {
   path_point {
-    x: 586344.38614980027
-    y: 4140697.2061165026
-    theta: 1.3244213299847327
-    kappa: -0.034317648374230653
-    s: 7.2129926994470148
+    x: 586344.14032340131
+    y: 4140699.9429120035
+    theta: 1.3507260832608841
+    kappa: -0.043014265503556193
+    s: 9.9419691998475042
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.004799917420354044
-  a: 0.00736092496933502
+  v: 0.0074209180380420126
+  a: -0.018516768276310824
   relative_time: 5.9999999999999867
 }
 trajectory_point {
   path_point {
-    x: 586344.38621302391
-    y: 4140697.2063576397
-    theta: 1.3244136542828429
-    kappa: -0.034314810576067861
-    s: 7.2132419868795088
+    x: 586344.14039654448
+    y: 4140699.9432527311
+    theta: 1.3507108619060388
+    kappa: -0.043013229417584925
+    s: 9.94231769033837
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0051733456856362546
-  a: 0.0075728217114707248
+  v: 0.006530411907912051
+  a: -0.01711153093007629
   relative_time: 6.0499999999999865
 }
 trajectory_point {
   path_point {
-    x: 586344.38628104783
-    y: 4140697.2066170853
-    theta: 1.324405395781189
-    kappa: -0.034311757310383592
-    s: 7.21351020213637
+    x: 586344.140460707
+    y: 4140699.9435516237
+    theta: 1.350697509486358
+    kappa: -0.043012320545913085
+    s: 9.9426233919344131
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0055568611984529673
-  a: 0.0077644148707160668
+  v: 0.0057089595449153095
+  a: -0.015754617590984319
   relative_time: 6.0999999999999863
 }
 trajectory_point {
   path_point {
-    x: 586344.38635399379
-    y: 4140697.2068953039
-    theta: 1.3243965397313289
-    kappa: -0.034308483124609435
-    s: 7.2137978242004941
+    x: 586344.14051660069
+    y: 4140699.9438119973
+    theta: 1.3506858778325739
+    kappa: -0.043011528803051195
+    s: 9.9428896969189751
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0059494487796596792
-  a: 0.007935704447070667
+  v: 0.0049541447486945393
+  a: -0.01444602825903744
   relative_time: 6.1499999999999861
 }
 trajectory_point {
   path_point {
-    x: 586344.38643197028
-    y: 4140697.2071927083
-    theta: 1.3243870729476883
-    kappa: -0.034304983143998574
-    s: 7.2141052812958231
+    x: 586344.14056491223
+    y: 4140699.94403705
+    theta: 1.3506758240524366
+    kappa: -0.043010844462686146
+    s: 9.9431198767653886
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0063500932501118525
-  a: 0.0080866904405345278
+  v: 0.0042635513188924862
+  a: -0.013185762934235647
   relative_time: 6.199999999999986
 }
 trajectory_point {
   path_point {
-    x: 586344.3865150729
-    y: 4140697.2075096648
-    theta: 1.3243769838075612
-    kappa: -0.034301253071625869
-    s: 7.21443295088734
+    x: 586344.14060630288
+    y: 4140699.9442298627
+    theta: 1.350667210530669
+    kappa: -0.043010258157681266
+    s: 9.9433170821369661
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00675777943066495
-  a: 0.0082173728511076467
+  v: 0.0036347630551518944
+  a: -0.011973821616578943
   relative_time: 6.2499999999999858
 }
 trajectory_point {
   path_point {
-    x: 586344.38660338474
-    y: 4140697.2078464888
-    theta: 1.3243662622511092
-    kappa: -0.034297289188387814
-    s: 7.2147811596810723
+    x: 586344.14064140862
+    y: 4140699.9443933978
+    theta: 1.3506599049289247
+    kappa: -0.0430097608800763
+    s: 9.9434843428869986
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0071714921421744352
-  a: 0.0083277516787900255
+  v: 0.0030653637571155107
+  a: -0.010810204306067328
   relative_time: 6.2999999999999856
 }
 trajectory_point {
   path_point {
-    x: 586344.38669697556
-    y: 4140697.2082034475
-    theta: 1.3243548997813626
-    kappa: -0.034293088353002578
-    s: 7.2151501836240879
+    x: 586344.14067083993
+    y: 4140699.9445304996
+    theta: 1.3506537801857543
+    kappa: -0.04300934398108739
+    s: 9.943624568058766
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0075902162054957709
-  a: 0.0084178269235816641
+  v: 0.0025529372244260809
+  a: -0.0096949110027007984
   relative_time: 6.3499999999999854
 }
 trajectory_point {
   path_point {
-    x: 586344.38679590262
-    y: 4140697.208580758
-    theta: 1.3243428894642209
-    kappa: -0.03428864800200996
-    s: 7.2155402479044994
+    x: 586344.140695182
+    y: 4140699.9446438942
+    theta: 1.3506487145165713
+    kappa: -0.043008999171107114
+    s: 9.9437405458855253
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00801293644148442
-  a: 0.0084875985854825624
+  v: 0.00209506725672635
+  a: -0.00862794170647936
   relative_time: 6.3999999999999853
 }
 trajectory_point {
   path_point {
-    x: 586344.38690021
-    y: 4140697.20897859
-    theta: 1.3243302259284535
-    kappa: -0.034283966149771423
-    s: 7.2159515269514616
+    x: 586344.14071499486
+    y: 4140699.9447361897
+    theta: 1.3506445914136274
+    kappa: -0.043008718519704441
+    s: 9.9438349437905167
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0084386376709958445
-  a: 0.0085370666644927189
+  v: 0.0016893376536590635
+  a: -0.007609296417403006
   relative_time: 6.4499999999999851
 }
 trajectory_point {
   path_point {
-    x: 586344.3870099294
-    y: 4140697.2093970631
-    theta: 1.3243169053657018
-    kappa: -0.034279041388470051
-    s: 7.216384144435172
+    x: 586344.1407308128
+    y: 4140699.9448098755
+    theta: 1.3506412996459882
+    kappa: -0.043008494455624782
+    s: 9.9439103083869611
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00886630471488551
-  a: 0.0085662311606121352
+  v: 0.0013333322148669667
+  a: -0.0066389751354717433
   relative_time: 6.4999999999999849
 }
 trajectory_point {
   path_point {
-    x: 586344.38712507894
-    y: 4140697.2098362474
-    theta: 1.32430292553048
-    kappa: -0.03427387288811061
-    s: 7.2168381732668712
+    x: 586344.14074314514
+    y: 4140699.9448673236
+    theta: 1.3506387332595144
+    kappa: -0.043008319766789926
+    s: 9.9439690654780684
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0092949223940088765
-  a: 0.0085750920738408114
+  v: 0.0010246347399928074
+  a: -0.0057169778606855678
   relative_time: 6.5499999999999847
 }
 trajectory_point {
   path_point {
-    x: 586344.38724566437
-    y: 4140697.2102961647
-    theta: 1.3242882857401774
-    kappa: -0.034268460396519519
-    s: 7.217313635598841
+    x: 586344.14075247548
+    y: 4140699.944910788
+    theta: 1.3506367915768449
+    kappa: -0.0430081876002981
+    s: 9.9440135200570214
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0097234755292214085
-  a: 0.0085636494041787456
+  v: 0.0007608290286793282
+  a: -0.0048433045930444795
   relative_time: 6.5999999999999845
 }
 trajectory_point {
   path_point {
-    x: 586344.3873716786
-    y: 4140697.2107767863
-    theta: 1.3242729868750609
-    kappa: -0.034262804239344809
-    s: 7.2178105028244088
+    x: 586344.14075926249
+    y: 4140699.9449424041
+    theta: 1.3506353791973835
+    kappa: -0.043008091462423952
+    s: 9.9440458563069889
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.010150948941378569
-  a: 0.0085319031516259414
+  v: 0.00053949888056927572
+  a: -0.00401795533254848
   relative_time: 6.6499999999999844
 }
 trajectory_point {
   path_point {
-    x: 586344.38750310114
-    y: 4140697.2112780362
-    theta: 1.3242570313782767
-    kappa: -0.034256905320056187
-    s: 7.2183286955779424
+    x: 586344.140763939
+    y: 4140699.9449641891
+    theta: 1.3506344059972886
+    kappa: -0.043008025218618511
+    s: 9.9440681376011248
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.010576327451335821
-  a: 0.0084798533161823952
+  v: 0.00035822809530539589
+  a: -0.0032409300791975681
   relative_time: 6.6999999999999842
 }
 trajectory_point {
   path_point {
-    x: 586344.38763989939
-    y: 4140697.2117997888
-    theta: 1.3242404232558533
-    kappa: -0.034250765119945022
-    s: 7.2188680837348533
+    x: 586344.14076691284
+    y: 4140699.9449780425
+    theta: 1.3506337871294642
+    kappa: -0.043007983093509235
+    s: 9.9440823065025619
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.010998595879948628
-  a: 0.0084074998978481089
+  v: 0.00021460047253043547
+  a: -0.0025122288329917468
   relative_time: 6.749999999999984
 }
 trajectory_point {
   path_point {
-    x: 586344.38778202725
-    y: 4140697.2123418692
-    theta: 1.3242231680767058
-    kappa: -0.034244385698124311
-    s: 7.2194284864115961
+    x: 586344.14076856629
+    y: 4140699.944985745
+    theta: 1.3506334430235538
+    kappa: -0.043007959670900028
+    s: 9.9440901847644128
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.011416739048072449
-  a: 0.00831484289662308
+  v: 0.00010619981188713778
+  a: -0.0018318515939310109
   relative_time: 6.7999999999999838
 }
 trajectory_point {
   path_point {
-    x: 586344.387929426
-    y: 4140697.2129040528
-    theta: 1.3242052729726372
-    kappa: -0.034237769691528711
-    s: 7.2200096719656663
+    x: 586344.14076925663
+    y: 4140699.9449889604
+    theta: 1.3506332993859347
+    kappa: -0.043007949893771144
+    s: 9.94409347332978
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.011829741776562753
-  a: 0.008201882312507314
+  v: 3.06099130182487e-05
+  a: -0.0011997983620153641
   relative_time: 6.8499999999999837
 }
 trajectory_point {
   path_point {
-    x: 586344.388082024
-    y: 4140697.2134860666
-    theta: 1.3241867466383439
-    kappa: -0.034230920314914523
-    s: 7.2206113579956055
+    x: 586344.14076931519
+    y: 4140699.9449892333
+    theta: 1.350633287199716
+    kappa: -0.043007949064279315
+    s: 9.9440937523317388
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.012236588886275
-  a: 0.0080686181455008055
+  v: -1.4585424433482402e-05
+  a: -0.00061606913724480267
   relative_time: 6.8999999999999835
 }
 trajectory_point {
   path_point {
-    x: 586344.38823973679
-    y: 4140697.2140875882
-    theta: 1.3241675993314184
-    kappa: -0.034223841360859694
-    s: 7.2212332113409952
+    x: 586344.14076904836
+    y: 4140699.9449879904
+    theta: 1.350633342724735
+    kappa: -0.043007952843757642
+    s: 9.9440924810933531
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.012636265198064652
-  a: 0.007915050395603555
+  v: -3.1802400825314825e-05
+  a: -8.0663919619333724e-05
   relative_time: 6.9499999999999833
 }
 trajectory_point {
   path_point {
-    x: 586344.3884024669
-    y: 4140697.2147082463
-    theta: 1.3241478428723537
-    kappa: -0.034216537199763829
-    s: 7.2218748480824608
+    x: 586344.14076873707
+    y: 4140699.9449865404
+    theta: 1.3506334074975572
+    kappa: -0.043007957252715653
+    s: 9.9440909981276686
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.013027755532787174
-  a: 0.0077411790628155661
+  v: -2.3457216514501805e-05
+  a: 0.00040641729086104972
   relative_time: 6.9999999999999831
 }
 trajectory_point {
   path_point {
-    x: 586344.38857010426
-    y: 4140697.2153476207
-    theta: 1.3241274906445482
-    kappa: -0.0342090127798482
-    s: 7.2225358335416709
+    x: 586344.140768637
+    y: 4140699.9449860742
+    theta: 1.3506334283314756
+    kappa: -0.0430079586708393
+    s: 9.9440905211377082
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.013410044711298027
-  a: 0.0075470041471368352
+  v: 8.033928141706892e-06
+  a: 0.00084517449419634419
   relative_time: 7.0499999999999829
 }
 trajectory_point {
   path_point {
-    x: 586344.38874252571
-    y: 4140697.2160052415
-    theta: 1.3241065575943092
-    kappa: -0.034201273627155686
-    s: 7.2232156822813351
+    x: 586344.14076897816
+    y: 4140699.9449876635
+    theta: 1.3506333573165101
+    kappa: -0.043007953836990943
+    s: 9.944092147016482
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.013782117554452675
-  a: 0.0073325256485673642
+  v: 6.0254832786048491e-05
+  a: 0.0012356076903865497
   relative_time: 7.0999999999999828
 }
 trajectory_point {
   path_point {
-    x: 586344.38891959516
-    y: 4140697.21668059
-    theta: 1.3240850602308591
-    kappa: -0.034193325845550862
-    s: 7.2239138581052078
+    x: 586344.14076996571
+    y: 4140699.9449922633
+    theta: 1.350633151819409
+    kappa: -0.043007939849209337
+    s: 9.94409685184698
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.014142958883106582
-  a: 0.0070977435671071521
+  v: 0.00013078929706127496
+  a: 0.0015777168794316697
   relative_time: 7.1499999999999826
 }
 trajectory_point {
   path_point {
-    x: 586344.38910116383
-    y: 4140697.2173730992
-    theta: 1.324063016626339
-    kappa: -0.03418517611671993
-    s: 7.2246297740580845
+    x: 586344.14077177888
+    y: 4140699.94500071
+    theta: 1.3506327744836495
+    kappa: -0.043007914164709678
+    s: 9.9441054909021762
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.014491553518115208
-  a: 0.0068426579027562
+  v: 0.0002172211206101296
+  a: 0.0018715020613317007
   relative_time: 7.1999999999999824
 }
 trajectory_point {
   path_point {
-    x: 586344.38928706991
-    y: 4140697.2180821514
-    theta: 1.324040446415814
-    kappa: -0.034176831700170739
-    s: 7.2253627924258055
+    x: 586344.140774572
+    y: 4140699.9450137215
+    theta: 1.3506321932294385
+    kappa: -0.043007874599883568
+    s: 9.9441187986450235
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.014826886280334021
-  a: 0.0065672686555145075
+  v: 0.00031713410307535831
+  a: 0.0021169632360866428
   relative_time: 7.2499999999999822
 }
 trajectory_point {
   path_point {
-    x: 586344.38947713887
-    y: 4140697.21880708
-    theta: 1.3240173707972798
-    kappa: -0.034168300433232789
-    s: 7.2261122247352523
+    x: 586344.14077847381
+    y: 4140699.9450318976
+    theta: 1.3506313812537136
+    kappa: -0.043007819330299006
+    s: 9.94413738872846
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.015147941990618478
-  a: 0.006271575825382074
+  v: 0.00042811204409971132
+  a: 0.0023141004036964959
   relative_time: 7.2999999999999821
 }
 trajectory_point {
   path_point {
-    x: 586344.38967118331
-    y: 4140697.2195471716
-    theta: 1.3239938125316657
-    kappa: -0.034159590731057239
-    s: 7.2268773317543493
+    x: 586344.1407835877
+    y: 4140699.9450557204
+    theta: 1.3506303170301441
+    kappa: -0.04300774689070043
+    s: 9.9441617539954041
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.015453705469824046
-  a: 0.0059555794123589
+  v: 0.00054773874332592239
+  a: 0.0024629135641612634
   relative_time: 7.3499999999999819
 }
 trajectory_point {
   path_point {
-    x: 586344.38986900274
-    y: 4140697.2203016616
-    theta: 1.323969795942842
-    kappa: -0.03415071158661688
-    s: 7.2276573234920649
+    x: 586344.140789992
+    y: 4140699.945085553
+    theta: 1.3506289843091306
+    kappa: -0.043007656175008672
+    s: 9.9441922664787565
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.015743161538806186
-  a: 0.0056192794164449857
+  v: 0.00067359800039674868
+  a: 0.0025634027174809386
   relative_time: 7.3999999999999817
 }
 trajectory_point {
   path_point {
-    x: 586344.390070384
-    y: 4140697.2210697359
-    theta: 1.323945346917625
-    kappa: -0.034141672570706189
-    s: 7.2284513591984076
+    x: 586344.14079773892
+    y: 4140699.9451216417
+    theta: 1.3506273721178064
+    kappa: -0.04300754643632098
+    s: 9.944229177401402
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.016015295018420365
-  a: 0.00526267583764033
+  v: 0.00080327361495493351
+  a: 0.0026155678636555282
   relative_time: 7.4499999999999815
 }
 trajectory_point {
   path_point {
-    x: 586344.390275101
-    y: 4140697.221850533
-    theta: 1.3239204929057813
-    kappa: -0.034132483831941264
-    s: 7.2292585473644317
+    x: 586344.14080685633
+    y: 4140699.9451641138
+    theta: 1.350625474760037
+    kappa: -0.043007417286911043
+    s: 9.9442726171762033
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.01626909072952204
-  a: 0.0048857686759449341
+  v: 0.00093434938664321322
+  a: 0.0026194090026850324
   relative_time: 7.4999999999999813
 }
 trajectory_point {
   path_point {
-    x: 586344.39048291452
-    y: 4140697.2226431407
-    theta: 1.3238952629200347
-    kappa: -0.034123156096759852
-    s: 7.2300779457222317
+    x: 586344.14081734617
+    y: 4140699.9452129789
+    theta: 1.3506232918164196
+    kappa: -0.043007268698228907
+    s: 9.9443225954060122
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.016503533492966675
-  a: 0.004488557931358798
+  v: 0.0010644091151043502
+  a: 0.0025749261345694441
   relative_time: 7.5499999999999812
 }
 trajectory_point {
   path_point {
-    x: 586344.39069357293
-    y: 4140697.2234465987
-    theta: 1.3238696875360711
-    kappa: -0.034113700669421355
-    s: 7.2309085612449477
+    x: 586344.14082918479
+    y: 4140699.9452681281
+    theta: 1.3506208281442815
+    kappa: -0.043007101000901084
+    s: 9.9443790008836555
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.016717608129609738
-  a: 0.00407104360388192
+  v: 0.0011910365999810782
+  a: 0.0024821192593087703
   relative_time: 7.599999999999981
 }
 trajectory_point {
   path_point {
-    x: 586344.39090681158
-    y: 4140697.2242598981
-    theta: 1.323843798892544
-    kappa: -0.034104129432006837
-    s: 7.23174935014676
+    x: 586344.14084232389
+    y: 4140699.9453293346
+    theta: 1.3506180938776806
+    kappa: -0.043006914884730491
+    s: 9.9444416015919437
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.016910299460306689
-  a: 0.0036332256935143028
+  v: 0.0013118156409161388
+  a: 0.0023409883769030075
   relative_time: 7.6499999999999808
 }
 trajectory_point {
   path_point {
-    x: 586344.3911223528
-    y: 4140697.2250819788
-    theta: 1.3238176306910792
-    kappa: -0.034094454844418989
-    s: 7.2325992178828935
+    x: 586344.14085668908
+    y: 4140699.9453962529
+    theta: 1.3506151044274017
+    kappa: -0.043006711398696433
+    s: 9.9445100447036747
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.017080592305912988
-  a: 0.0031751042002559444
+  v: 0.0014243300375522891
+  a: 0.0021515334873521558
   relative_time: 7.6999999999999806
 }
 trajectory_point {
   path_point {
-    x: 586344.391339906
-    y: 4140697.2259117342
-    theta: 1.3237912181962812
-    kappa: -0.034084689944382171
-    s: 7.2334570191496148
+    x: 586344.14087218116
+    y: 4140699.9454684211
+    theta: 1.3506118804809559
+    kappa: -0.043006491950954656
+    s: 9.9445838565816214
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.017227471487284103
-  a: 0.002696679124106845
+  v: 0.0015261635895322725
+  a: 0.0019137545906562185
   relative_time: 7.7499999999999805
 }
 trajectory_point {
   path_point {
-    x: 586344.391559168
-    y: 4140697.2267480064
-    theta: 1.3237645982357369
-    kappa: -0.034074848347442392
-    s: 7.2343215578842326
+    x: 586344.14088867535
+    y: 4140699.9455452566
+    theta: 1.350608448002578
+    kappa: -0.043006258308837288
+    s: 9.9446624427785437
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.017349921825275495
-  a: 0.0021979504650670055
+  v: 0.0016149000964988227
+  a: 0.0016276516868151923
   relative_time: 7.79999999999998
 }
 trajectory_point {
   path_point {
-    x: 586344.39177982241
-    y: 4140697.2275895895
-    theta: 1.3237378092000212
-    kappa: -0.034064944246967294
-    s: 7.2351915872651
+    x: 586344.14090602146
+    y: 4140699.9456260609
+    theta: 1.350604838233223
+    kappa: -0.043006012598852915
+    s: 9.9447450880371822
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.017446928140742623
-  a: 0.0016789182231364249
+  v: 0.0016881233580947055
+  a: 0.0012932247758290771
   relative_time: 7.84999999999998
 }
 trajectory_point {
   path_point {
-    x: 586344.3920015404
-    y: 4140697.228435229
-    theta: 1.3237108910427011
-    kappa: -0.0340549924141462
-    s: 7.2360658097116124
+    x: 586344.140924044
+    y: 4140699.9457100164
+    theta: 1.350601087690565
+    kappa: -0.043005757306686494
+    s: 9.944830956290259
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.017517475254540955
-  a: 0.0011395823983151041
+  v: 0.0017434171739626522
+  a: 0.000910473857697873
   relative_time: 7.89999999999998
 }
 trajectory_point {
   path_point {
-    x: 586344.39222397993
-    y: 4140697.2292836206
-    theta: 1.3236838852803408
-    kappa: -0.03404500819799005
-    s: 7.236942876884207
+    x: 586344.14094254212
+    y: 4140699.945796188
+    theta: 1.3505972381689932
+    kappa: -0.043005495277199428
+    s: 9.944919090660477
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.017560547987525953
-  a: 0.000579942990603044
+  v: 0.0017783653437454007
+  a: 0.00047939893242158332
   relative_time: 7.94999999999998
 }
 trajectory_point {
   path_point {
-    x: 586344.39244678593
-    y: 4140697.23013341
-    theta: 1.3236568349925049
-    kappa: -0.034035007525331444
-    s: 7.2378213896843659
+    x: 586344.14096128976
+    y: 4140699.9458835209
+    theta: 1.35059333673961
+    kappa: -0.043005229714429515
+    s: 9.9450084134605259
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.017575131160553082
-  a: 2.4199392489876459e-16
+  v: 0.0017905516670857239
+  a: 2.0122792321330962e-16
   relative_time: 7.99999999999998
 }
 decision {
@@ -2264,10 +2264,10 @@ decision {
       reason_code: STOP_REASON_OBSTACLE
       reason: "stop by 1_0"
       stop_point {
-        x: 586345.63410481508
-        y: 4140704.01507118
+        x: 586345.17777458706
+        y: 4140704.2252595429
       }
-      stop_heading: 1.2802150975745192
+      stop_heading: 1.2517387544338563
     }
   }
   object_decision {
@@ -2279,10 +2279,10 @@ decision {
           reason_code: STOP_REASON_OBSTACLE
           distance_s: -5
           stop_point {
-            x: 586345.63410481508
-            y: 4140704.01507118
+            x: 586345.17777458706
+            y: 4140704.2252595429
           }
-          stop_heading: 1.2802150975745192
+          stop_heading: 1.2517387544338563
         }
       }
     }
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
index 72931ab3fbc29ba27268e0c025491acc1d1f4a6b..1c2e13f85086d1e0d8f98d4cd137c43de6cab38c 100755
--- a/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
@@ -8,2254 +8,2254 @@ trajectory_point {
   path_point {
     x: 587503.356047
     y: 4140719.15699
+    z: 0
     theta: 2.35595493774
-    kappa: -1.4121056333504611e-19
-    s: 1.3010426069826053e-18
+    kappa: -5.0063450587992888e-18
     dkappa: 0
     ddkappa: 0
   }
   v: 5.4222221374511719
-  a: 0.49652478276599993
+  a: 0.496524782766
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587503.16403008346
-    y: 4140719.3492641207
-    theta: 2.3550792048543481
-    kappa: -0.0028758108593388634
-    s: 0.271734859402699
+    x: 587503.16400866932
+    y: 4140719.3492433252
+    theta: 2.3554752472105052
+    kappa: -0.0015972098780856626
+    s: 0.27173527823466953
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4472335806047942
-  a: 0.5038858165268556
+  v: 5.4472586664005371
+  a: 0.504885718557559
   relative_time: 0.05
 }
 trajectory_point {
   path_point {
-    x: 587502.97112305241
-    y: 4140719.5424295492
-    theta: 2.3541994123212842
-    kappa: -0.0057649528435678443
-    s: 0.54472937443815006
+    x: 587502.97107836267
+    y: 4140719.5423896252
+    theta: 2.3549933286089888
+    kappa: -0.0032018384469608579
+    s: 0.54473270744491908
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4726060064186521
-  a: 0.51096408917540237
+  v: 5.4727056436418238
+  a: 0.512942714442857
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587502.77797126584
-    y: 4140719.7371532153
-    theta: 2.3520302523469976
-    kappa: -0.0081185039727433832
-    s: 0.81900124078797465
+    x: 587502.77755483915
+    y: 4140719.7367552184
+    theta: 2.353773115995172
+    kappa: -0.0045290966464025138
+    s: 0.8190124301204621
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.49832527683713
-  a: 0.51775960071164029
+  v: 5.4985478721797181
+  a: 0.52069577042189374
   relative_time: 0.15000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587502.58401365927
-    y: 4140719.9329009638
-    theta: 2.3496443498587696
-    kappa: -0.010394936275923675
-    s: 1.0945674472310132
+    x: 587502.5831584041
+    y: 4140719.9320886084
+    theta: 2.3524400865532868
+    kappa: -0.0058212256853171426
+    s: 1.0945938289012458
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5243772538046114
-  a: 0.52427235113556936
+  v: 5.5247701550189081
+  a: 0.52814488649466951
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 587502.39005137619
-    y: 4140720.1304834587
-    theta: 2.3463833178902864
-    kappa: -0.012239508994547588
-    s: 1.3714442756433254
+    x: 587502.388379454
+    y: 4140720.128902046
+    theta: 2.3505956376924018
+    kappa: -0.0068837012687868876
+    s: 1.3714955265774524
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5507477992654817
-  a: 0.53050234044718958
+  v: 5.5513572951640811
+  a: 0.53529006266118406
   relative_time: 0.25
 }
 trajectory_point {
   path_point {
-    x: 587502.19547599333
-    y: 4140720.3293234548
-    theta: 2.3428092936332066
-    kappa: -0.013940524762809421
-    s: 1.6496473009981909
+    x: 587502.19282682007
+    y: 4140720.3268328086
+    theta: 2.3485880377826804
+    kappa: -0.0078792859964287431
+    s: 1.6497353860894985
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5774227751641252
-  a: 0.536449568646501
+  v: 5.5782940956199223
+  a: 0.54213129892143763
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587502.00104628084
-    y: 4140720.5301735122
-    theta: 2.338675450727127
-    kappa: -0.015296718796259916
-    s: 1.9291913913661072
+    x: 587501.99698018469
+    y: 4140720.5263751033
+    theta: 2.3462362579311695
+    kappa: -0.008689582324190058
+    s: 1.9293305105280341
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.604388043444926
-  a: 0.54211403573350347
+  v: 5.60556535939112
+  a: 0.54866859527543
   relative_time: 0.35
 }
 trajectory_point {
   path_point {
-    x: 587501.806266006
-    y: 4140720.7325718091
-    theta: 2.3342250674578451
-    kappa: -0.016466614849517107
-    s: 2.2100907079147922
+    x: 587501.80048449489
+    y: 4140720.7272027619
+    theta: 2.3437146402660813
+    kappa: -0.0094138504940234624
+    s: 2.2102972431339443
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.63162946605227
-  a: 0.547495741708197
+  v: 5.633155889482361
+  a: 0.55490195172316126
   relative_time: 0.39999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587501.611677381
-    y: 4140720.9370433791
-    theta: 2.329443733323084
-    kappa: -0.0173606765918033
-    s: 2.4923587049091829
+    x: 587501.60370152572
+    y: 4140720.9296864066
+    theta: 2.3409617026891691
+    kappa: -0.01000120887407574
+    s: 2.4926511672983485
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6591329049305408
-  a: 0.55259468657058175
+  v: 5.661050488898332
+  a: 0.56083136826463142
   relative_time: 0.44999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587501.41699802678
-    y: 4140721.1433367264
-    theta: 2.3244053041191006
-    kappa: -0.018046468226152468
-    s: 2.7760081297114345
+    x: 587501.40637636487
+    y: 4140721.1335986773
+    theta: 2.3380556762250753
+    kappa: -0.010502006260615163
+    s: 2.7764071065626008
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6868842220241227
-  a: 0.55741087032065761
+  v: 5.68923396064372
+  a: 0.56645684489984049
   relative_time: 0.49999999999999994
 }
 trajectory_point {
   path_point {
-    x: 587501.2225001672
-    y: 4140721.3517059716
-    theta: 2.3192015561185446
-    kappa: -0.018509188364988941
-    s: 3.0610510227809233
+    x: 587501.208777243
+    y: 4140721.3392121843
+    theta: 2.3350022115403615
+    kappa: -0.010891406349932291
+    s: 3.0615791246182891
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7148692792774014
-  a: 0.56194429295842463
+  v: 5.717691107723212
+  a: 0.57177838162878847
   relative_time: 0.54999999999999993
 }
 trajectory_point {
   path_point {
-    x: 587501.02812469611
-    y: 4140721.5621114336
-    theta: 2.3138384813086437
-    kappa: -0.01875907864047445
-    s: 3.3474987176742435
+    x: 587501.01074041927
+    y: 4140721.5463879053
+    theta: 2.3318300396598048
+    kappa: -0.01119418799979259
+    s: 3.3481805253072356
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7430739386347609
-  a: 0.5661949544838828
+  v: 5.7464067331414945
+  a: 0.57679597845147534
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 587500.83391083183
-    y: 4140721.7745817173
-    theta: 2.3084358306481954
-    kappa: -0.01882544074774117
-    s: 3.6353618410452073
+    x: 587500.81243488251
+    y: 4140721.7552969679
+    theta: 2.3285742775878049
+    kappa: -0.011402664889229317
+    s: 3.6362238526214967
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7714840620405852
-  a: 0.57016285489703211
+  v: 5.7753656399032547
+  a: 0.58150963536790112
   relative_time: 0.65
 }
 trajectory_point {
   path_point {
-    x: 587500.63997087884
-    y: 4140721.9892328633
-    theta: 2.3029918587475011
-    kappa: -0.018688682688386023
-    s: 3.9246503126448498
+    x: 587500.61376459128
+    y: 4140721.9658644171
+    theta: 2.3252429315820975
+    kappa: -0.011528425556233637
+    s: 3.9257208907033645
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8000855114392582
-  a: 0.57384799419787258
+  v: 5.8045526310131805
+  a: 0.58591935237806581
   relative_time: 0.70000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587500.44616238656
-    y: 4140722.2059253352
-    theta: 2.2976070560005519
-    kappa: -0.018397914488158106
-    s: 4.2153733453214208
+    x: 587500.41486337257
+    y: 4140722.178222701
+    theta: 2.3218850772775141
+    kappa: -0.011573736460573698
+    s: 4.2166826638453623
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8288641487751667
-  a: 0.5772503723864042
+  v: 5.8339525094759574
+  a: 0.59002512948196939
   relative_time: 0.75000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587500.2526812841
-    y: 4140722.4248456238
-    theta: 2.2922969837247371
-    kappa: -0.017924851154938744
-    s: 4.5075394450203943
+    x: 587500.215657469
+    y: 4140722.3923167782
+    theta: 2.3185013921917843
+    kappa: -0.011545588714572886
+    s: 4.5091194364902529
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8578058359926937
-  a: 0.58036998946262686
+  v: 5.8635500782962726
+  a: 0.59382696667961177
   relative_time: 0.80000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587500.05928404757
-    y: 4140722.6457668603
-    theta: 2.2871265144741519
-    kappa: -0.017320523595436715
-    s: 4.8011564107844578
+    x: 587500.01620747114
+    y: 4140722.6082067164
+    theta: 2.315129615814945
+    kappa: -0.011446243478725325
+    s: 4.8030407132310282
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8868964350362241
-  a: 0.58320684542654067
+  v: 5.8933301404788141
+  a: 0.59732486397099316
   relative_time: 0.8500000000000002
 }
 trajectory_point {
   path_point {
-    x: 587499.86614662712
-    y: 4140722.8688523765
-    theta: 2.2821240731017509
-    kappa: -0.016562628070166362
-    s: 5.0962313347535231
+    x: 587499.81641537731
+    y: 4140722.825813842
+    theta: 2.3117661223176427
+    kappa: -0.011284225369493264
+    s: 5.0984552388109163
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9161218078501427
-  a: 0.58576094027814563
+  v: 5.9232774990282682
+  a: 0.60051882135611334
   relative_time: 0.90000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587499.6730716998
-    y: 4140723.0939205494
-    theta: 2.2773435598599869
-    kappa: -0.015693167586482808
-    s: 5.3927706021647186
+    x: 587499.61640555132
+    y: 4140723.0452543423
+    theta: 2.30845613918531
+    kappa: -0.011059740149540776
+    s: 5.3953709981233819
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9454678163788337
-  a: 0.58803227401744174
+  v: 5.9533769569493211
+  a: 0.60340883883497254
   relative_time: 0.95000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587499.48013266572
-    y: 4140723.3210418066
-    theta: 2.2728034081096307
-    kappa: -0.014703984740893045
-    s: 5.6907798913523919
+    x: 587499.41600176
+    y: 4140723.2663772362
+    theta: 2.305184385943738
+    kappa: -0.010786027173939217
+    s: 5.69379521621212
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9749203225666818
-  a: 0.590020846644429
+  v: 5.983613317246661
+  a: 0.60599491640757053
   relative_time: 1.0000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587499.28723127081
-    y: 4140723.5501233689
-    theta: 2.2685706488748281
-    kappa: -0.013623285947499227
-    s: 5.9902641737481126
+    x: 587499.21541869221
+    y: 4140723.4893774809
+    theta: 2.3020084258376339
+    kappa: -0.010458721881779848
+    s: 5.9937343582710643
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.004465188358072
-  a: 0.59172665815910741
+  v: 6.0139713829249741
+  a: 0.60827705407390742
   relative_time: 1.0500000000000003
 }
 trajectory_point {
   path_point {
-    x: 587499.09428030509
-    y: 4140723.7810973441
-    theta: 2.264617083816991
-    kappa: -0.012458426657994718
-    s: 6.2912277138806658
+    x: 587499.0143569496
+    y: 4140723.7139930846
+    theta: 2.2988885310648386
+    kappa: -0.010096230027875609
+    s: 6.2951941296443783
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0340882756973873
-  a: 0.593149708561477
+  v: 6.0444359569889468
+  a: 0.61025525183398321
   relative_time: 1.1000000000000003
 }
 trajectory_point {
   path_point {
-    x: 587498.901329241
-    y: 4140724.0139977476
-    theta: 2.2610590069899485
-    kappa: -0.011268426848184425
-    s: 6.5936740693760569
+    x: 587498.813177577
+    y: 4140723.9405461322
+    theta: 2.2959110715374855
+    kappa: -0.0096911078788103726
+    s: 6.5981794758264654
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0637754465290143
-  a: 0.59428999785153758
+  v: 6.0749918424432678
+  a: 0.61192950968779791
   relative_time: 1.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 587498.70811106835
-    y: 4140724.2486068089
-    theta: 2.2577805783236848
-    kappa: -0.010058688042999852
-    s: 6.8976060909575123
+    x: 587498.61141667259
+    y: 4140724.1686294232
+    theta: 2.2929949114969297
+    kappa: -0.0092642550065534911
+    s: 6.9026945824619563
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0935125627973363
-  a: 0.59514752602928933
+  v: 6.1056238422926228
+  a: 0.61329982763535151
   relative_time: 1.2000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587498.51484663389
-    y: 4140724.4851008654
-    theta: 2.254926159788214
-    kappa: -0.0089349152855243176
-    s: 7.2030259224454758
+    x: 587498.40957846539
+    y: 4140724.3986868071
+    theta: 2.2902642087797531
+    kappa: -0.0088080324699569227
+    s: 7.208742875345723
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1232854864467381
-  a: 0.59572229309473235
+  v: 6.1363167595416988
+  a: 0.614366205676644
   relative_time: 1.2500000000000004
 }
 trajectory_point {
   path_point {
-    x: 587498.32110324583
-    y: 4140724.7231275067
-    theta: 2.2522845025857841
-    kappa: -0.0078529987544264084
-    s: 7.5099350007576113
+    x: 587498.20702659385
+    y: 4140724.6301619583
+    theta: 2.2875836035684953
+    kappa: -0.0083408418936024385
+    s: 7.5163270204228674
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1530800794216045
-  a: 0.5960142990478664
+  v: 6.1670553971951829
+  a: 0.61512864381167531
   relative_time: 1.3000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587498.12721499847
-    y: 4140724.9629544988
-    theta: 2.2501062815423811
-    kappa: -0.00678090735901383
-    s: 7.8183340559088013
+    x: 587498.00441973226
+    y: 4140724.8636283353
+    theta: 2.2851274120684471
+    kappa: -0.0078615719424231269
+    s: 7.825448923788727
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.18288220366632
-  a: 0.59602354388869161
+  v: 6.1978245582577616
+  a: 0.61558714204044562
   relative_time: 1.3500000000000005
 }
 trajectory_point {
   path_point {
-    x: 587497.93264812639
-    y: 4140725.2041493552
-    theta: 2.248072027709167
-    kappa: -0.0057085273402996805
-    s: 8.1282231110111471
+    x: 587497.80096042866
+    y: 4140725.098393003
+    theta: 2.2826977785069897
+    kappa: -0.0073783268144213621
+    s: 8.1361097316888742
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2126777211252682
-  a: 0.59575002761720786
+  v: 6.2286090457341228
+  a: 0.61574170036295484
   relative_time: 1.4000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587497.737825932
-    y: 4140725.4470499251
-    theta: 2.2465550714070854
-    kappa: -0.0046999775748575411
-    s: 8.4396014822739716
+    x: 587497.59747800929
+    y: 4140725.3351708949
+    theta: 2.2805288689112957
+    kappa: -0.0069068264888983329
+    s: 8.4483098305191149
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2424524937428352
-  a: 0.59519375023341536
+  v: 6.2593936626289528
+  a: 0.61559231877920284
   relative_time: 1.4500000000000006
 }
 trajectory_point {
   path_point {
-    x: 587497.54217661219
-    y: 4140725.6911946842
-    theta: 2.2451113773658471
-    kappa: -0.0036971459406558425
-    s: 8.7524677790038155
+    x: 587497.39311804879
+    y: 4140725.5732234772
+    theta: 2.2783857256941267
+    kappa: -0.0064379998822198768
+    s: 8.76204884682549
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2721923834634046
-  a: 0.594354711737314
+  v: 6.2901632119469388
+  a: 0.61513899728918986
   relative_time: 1.5000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587497.3461236764
-    y: 4140725.93691913
-    theta: 2.2442079077825463
-    kappa: -0.0028293201279839517
-    s: 9.0668199036044363
+    x: 587497.18856399867
+    y: 4140725.8131340891
+    theta: 2.2764689805219978
+    kappa: -0.006003277269164828
+    s: 9.0773256473042725
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3018832522313613
-  a: 0.59323291212890372
+  v: 6.320902496692768
+  a: 0.61438173589291567
   relative_time: 1.5500000000000007
 }
 trajectory_point {
   path_point {
-    x: 587497.14919178654
-    y: 4140726.1838394655
-    theta: 2.2433667166328988
-    kappa: -0.0019865820698040091
-    s: 9.3826550515768155
+    x: 587496.98323191155
+    y: 4140726.0543990675
+    theta: 2.2746066597210333
+    kappa: -0.0055845471855033718
+    s: 9.3941383388019748
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.33151096199109
-  a: 0.59182835140818457
+  v: 6.3515963198711258
+  a: 0.61332053459038038
   relative_time: 1.6000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587496.95160506456
-    y: 4140726.4321297808
-    theta: 2.2429131688162589
-    kappa: -0.0013135480946465
-    s: 9.6999697115191488
+    x: 587496.77750882111
+    y: 4140726.2973437952
+    theta: 2.2729112341134274
+    kappa: -0.0052137554985791125
+    s: 9.71248426831534
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3610613746869751
-  a: 0.59014102957515657
+  v: 6.382229484486702
+  a: 0.611955393381584
   relative_time: 1.6500000000000008
 }
 trajectory_point {
   path_point {
-    x: 587496.753141844
-    y: 4140726.6816079919
-    theta: 2.2425559646791497
-    kappa: -0.00071784234782221309
-    s: 10.018759665126854
+    x: 587496.5711043824
+    y: 4140726.541715431
+    theta: 2.2712903206862145
+    kappa: -0.0048821324377184266
+    s: 10.032360022991345
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3905203522634011
-  a: 0.58817094662981972
+  v: 6.41278679354418
+  a: 0.61028631226652652
   relative_time: 1.7000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587496.55386014283
-    y: 4140726.9323139507
-    theta: 2.2424234862971781
-    kappa: -0.00030438613003498069
-    s: 10.339019987192572
+    x: 587496.36411607428
+    y: 4140726.7875908157
+    theta: 2.2697694364830197
+    kappa: -0.0046023150065062539
+    s: 10.353761430127202
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4198737566647521
-  a: 0.585918102572174
+  v: 6.44325305004825
+  a: 0.60831329124520794
   relative_time: 1.7500000000000009
 }
 trajectory_point {
   path_point {
-    x: 587496.35367380583
-    y: 4140727.1841719337
-    theta: 2.2423616461510134
-    kappa: -4.6680346804713745e-05
-    s: 10.660745045606154
+    x: 587496.1565464309
+    y: 4140727.034963767
+    theta: 2.2683210627078547
+    kappa: -0.0043964971037187186
+    s: 10.676683557170358
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4491074498354131
-  a: 0.58338249740221948
+  v: 6.4736130570035968
+  a: 0.60603633031762827
   relative_time: 1.8000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587496.15258738364
-    y: 4140727.4371774606
-    theta: 2.2423768045585426
-    kappa: 4.1199186737913533e-05
-    s: 10.983928501354674
+    x: 587495.94824911258
+    y: 4140727.2837039819
+    theta: 2.2669151860472612
+    kappa: -0.0042362969136857213
+    s: 11.001120711718496
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4782072937197688
-  a: 0.580564131119956
+  v: 6.5038516174149086
+  a: 0.6034554294837875
   relative_time: 1.850000000000001
 }
 trajectory_point {
   path_point {
-    x: 587495.95059383567
-    y: 4140727.6913159359
-    theta: 2.2423505365141483
-    kappa: -0.00013440186482912
-    s: 11.308563308522432
+    x: 587495.73948896746
+    y: 4140727.5340232258
+    theta: 2.2655449525951141
+    kappa: -0.004197894251525427
+    s: 11.327066441519527
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5071591502622033
-  a: 0.57746300372538373
+  v: 6.5339535342868711
+  a: 0.60057058874368552
   relative_time: 1.900000000000001
 }
 trajectory_point {
   path_point {
-    x: 587495.74770005385
-    y: 4140727.9465829339
-    theta: 2.2423038348110853
-    kappa: -0.00043998428235860474
-    s: 11.634641714290936
+    x: 587495.52990013256
+    y: 4140727.78560641
+    theta: 2.2641772370984423
+    kappa: -0.0041887960843149394
+    s: 11.654513534471604
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5359488814071014
-  a: 0.57407911521850252
+  v: 6.5639036106241724
+  a: 0.59738180809732255
   relative_time: 1.9500000000000011
 }
 trajectory_point {
   path_point {
-    x: 587495.54397596442
-    y: 4140728.2030232125
-    theta: 2.2420407627804959
-    kappa: -0.00086182412039670993
-    s: 11.962155258938921
+    x: 587495.3199717137
+    y: 4140728.0388491806
+    theta: 2.2627838343458428
+    kappa: -0.0042434776932842675
+    s: 11.98345401862311
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5645623490988481
-  a: 0.5704124655993128
+  v: 6.5936866494315005
+  a: 0.593889087544699
   relative_time: 2.0000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587495.33939287625
-    y: 4140728.4606023449
-    theta: 2.2417253518023728
-    kappa: -0.0012891305868898435
-    s: 12.291094851043361
+    x: 587495.10925850412
+    y: 4140728.2933696024
+    theta: 2.2613774571577427
+    kappa: -0.0042921256257442983
+    s: 12.31387924360971
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5929903486254871
-  a: 0.5666940912015086
+  v: 6.6232927849931329
+  a: 0.59034128281809561
   relative_time: 2.0500000000000007
 }
 trajectory_point {
   path_point {
-    x: 587495.13410750765
-    y: 4140728.7194328094
-    theta: 2.2412138247336957
-    kappa: -0.0016032753510644474
-    s: 12.621451161595475
+    x: 587494.89807159267
+    y: 4140728.5494133523
+    theta: 2.2599455975699403
+    kappa: -0.0043229539280064729
+    s: 12.645780302988227
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.621230415792521
-  a: 0.56289517121506771
+  v: 6.6527192725030568
+  a: 0.586703165476342
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587494.928037056
-    y: 4140728.9794366243
-    theta: 2.2406500673491072
-    kappa: -0.0018804408530321104
-    s: 12.953214693295294
+    x: 587494.68627401011
+    y: 4140728.8068540227
+    theta: 2.2585014026910426
+    kappa: -0.0043423925169478851
+    s: 12.979148101465306
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6492785233205165
-  a: 0.55901570563999037
+  v: 6.6819615963305141
+  a: 0.58297473551943835
   relative_time: 2.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 587494.72127963929
-    y: 4140729.2406791528
-    theta: 2.2399825204817394
-    kappa: -0.0020811347608359083
-    s: 13.286375747478882
+    x: 587494.47387348
+    y: 4140729.0656859
+    theta: 2.2570448774527447
+    kappa: -0.00435037647574082
+    s: 13.313973317966056
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6771306439300426
-  a: 0.55505569447627645
+  v: 6.7110152408447483
+  a: 0.57915599294738462
   relative_time: 2.2
 }
 trajectory_point {
   path_point {
-    x: 587494.51385013887
-    y: 4140729.5031592036
-    theta: 2.2392612370020273
-    kappa: -0.0022229940358415106
-    s: 13.62092442411833
+    x: 587494.2610540709
+    y: 4140729.3260459462
+    theta: 2.2555833752506755
+    kappa: -0.004342015452494366
+    s: 13.650246405634045
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7047827503416668
-  a: 0.55101513772392607
+  v: 6.739875690415
+  a: 0.57524693776018077
   relative_time: 2.25
 }
 trajectory_point {
   path_point {
-    x: 587494.30570529075
-    y: 4140729.766829703
-    theta: 2.2384995868996769
-    kappa: -0.0023216455637108044
-    s: 13.956850621821758
+    x: 587494.04750237369
+    y: 4140729.587664946
+    theta: 2.2541163985150221
+    kappa: -0.0043278312596073849
+    s: 13.987957591831307
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7322308152759573
-  a: 0.54689403538293913
+  v: 6.7685384294105138
+  a: 0.57124756995782688
   relative_time: 2.3
 }
 trajectory_point {
   path_point {
-    x: 587494.097013206
-    y: 4140730.0318097658
-    theta: 2.2377082290177173
-    kappa: -0.0023448598275834015
-    s: 14.294144037833313
+    x: 587493.83371092577
+    y: 4140729.850930348
+    theta: 2.2526580789790751
+    kappa: -0.0042937043061130036
+    s: 14.327096878138336
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7594708114534834
-  a: 0.54269238745331561
+  v: 6.79699894220053
+  a: 0.56715788954032287
   relative_time: 2.3499999999999996
 }
 trajectory_point {
   path_point {
-    x: 587493.88758013246
-    y: 4140730.2979331911
-    theta: 2.2369106814259161
-    kappa: -0.0023450285582149338
-    s: 14.632794168033168
+    x: 587493.6192235921
+    y: 4140730.1154567469
+    theta: 2.2512013073568018
+    kappa: -0.0042535519759557116
+    s: 14.667654040354087
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7864987115948114
-  a: 0.53841019393505563
+  v: 6.8252527131542928
+  a: 0.56297789650766872
   relative_time: 2.3999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587493.67764463578
-    y: 4140730.5653737667
-    theta: 2.2361284679284421
-    kappa: -0.0022763918680240376
-    s: 14.972790306937529
+    x: 587493.40430595342
+    y: 4140730.3814458223
+    theta: 2.2497561296957329
+    kappa: -0.0041996779565404143
+    s: 15.009618628495978
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8133104884205116
-  a: 0.53404745482815907
+  v: 6.8532952266410447
+  a: 0.55870759085986454
   relative_time: 2.4499999999999993
 }
 trajectory_point {
   path_point {
-    x: 587493.46702077915
-    y: 4140730.8339711614
-    theta: 2.2353663601673017
-    kappa: -0.00218230389750802
-    s: 15.314121547698631
+    x: 587493.18889491493
+    y: 4140730.6488311766
+    theta: 2.2483267682726149
+    kappa: -0.0041348103057603459
+    s: 15.352979966799893
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.83990211465115
-  a: 0.529604170132626
+  v: 6.881121967030027
+  a: 0.55434697259691035
   relative_time: 2.4999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587493.25579302281
-    y: 4140731.103777471
-    theta: 2.2346376066197449
-    kappa: -0.0020484001214509063
-    s: 15.656776782104734
+    x: 587492.97287628462
+    y: 4140730.9175067698
+    theta: 2.2469064293127676
+    kappa: -0.0040623429713205865
+    s: 15.697727153720171
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8662695630072967
-  a: 0.52508033984845637
+  v: 6.9087284186904832
+  a: 0.54989604171880591
   relative_time: 2.5499999999999989
 }
 trajectory_point {
   path_point {
-    x: 587493.04397214181
-    y: 4140731.3747863425
-    theta: 2.2339700113526537
-    kappa: -0.001878738677794302
-    s: 16.000744700580128
+    x: 587492.75657155376
+    y: 4140731.1877151672
+    theta: 2.24552313158066
+    kappa: -0.0039745179270094747
+    s: 16.043849061929617
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8924088062095183
-  a: 0.52047596397565021
+  v: 6.936110065991655
+  a: 0.54535479822555155
   relative_time: 2.5999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587492.83144985256
-    y: 4140731.6468984927
-    theta: 2.233329450206317
-    kappa: -0.001692144291211064
-    s: 16.346013792185126
+    x: 587492.5396150843
+    y: 4140731.459148027
+    theta: 2.2441516449506422
+    kappa: -0.0038813349911858853
+    s: 16.3913343383195
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9183158169783843
-  a: 0.5157910425142076
+  v: 6.9632623933027853
+  a: 0.540723242117147
   relative_time: 2.6499999999999986
 }
 trajectory_point {
   path_point {
-    x: 587492.61840010609
-    y: 4140731.9202346131
-    theta: 2.2328058427092885
-    kappa: -0.0014671256468880346
-    s: 16.692572344616082
+    x: 587492.32231265132
+    y: 4140731.73203436
+    theta: 2.242820573784412
+    kappa: -0.0037756032188378656
+    s: 16.740171403999547
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9439865680344619
-  a: 0.51102557546412841
+  v: 6.9901808849931166
+  a: 0.53600137339359244
   relative_time: 2.6999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587492.40463205427
-    y: 4140732.1946305991
-    theta: 2.232324840184162
-    kappa: -0.0012336308244257853
-    s: 17.040408444205369
+    x: 587492.10449471383
+    y: 4140732.0062225647
+    theta: 2.24151993783707
+    kappa: -0.003661850636689684
+    s: 17.090348454297949
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9694170320983195
-  a: 0.50617956282541265
+  v: 7.0168610254318917
+  a: 0.53118919205488768
   relative_time: 2.7499999999999982
 }
 trajectory_point {
   path_point {
-    x: 587492.19023273978
-    y: 4140732.4701388809
-    theta: 2.2319406971590987
-    kappa: -0.00098277266463457158
-    s: 17.389509975921385
+    x: 587491.8861225869
+    y: 4140732.2816665382
+    theta: 2.2402446394177806
+    kappa: -0.0035411824636075312
+    s: 17.441853458761361
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9946031818905254
-  a: 0.50125300459806044
+  v: 7.0432982989883532
+  a: 0.5262866981010329
   relative_time: 2.799999999999998
 }
 trajectory_point {
   path_point {
-    x: 587491.97515438916
-    y: 4140732.7467065481
-    theta: 2.2316502223652384
-    kappa: -0.00072410630038825363
-    s: 17.739864623368568
+    x: 587491.66742776544
+    y: 4140732.5585328736
+    theta: 2.2390294731854685
+    kappa: -0.0034090098849498584
+    s: 17.794674161154898
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0195409901316479
-  a: 0.49624590078207165
+  v: 7.0694881900317421
+  a: 0.521293891532028
   relative_time: 2.8499999999999979
 }
 trajectory_point {
   path_point {
-    x: 587491.75936739868
-    y: 4140733.0242947163
-    theta: 2.2314142903884195
-    kappa: -0.00046048524351323828
-    s: 18.091459868787371
+    x: 587491.44811492541
+    y: 4140732.8365716231
+    theta: 2.2378372412771039
+    kappa: -0.0032722986371167681
+    s: 18.148798079462136
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0442264295422552
-  a: 0.49115825137744634
+  v: 7.0954261829313028
+  a: 0.516210772347873
   relative_time: 2.8999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587491.54289836658
-    y: 4140733.3029079158
-    theta: 2.2313265923544114
-    kappa: -0.00019870941251046124
-    s: 18.444282993054291
+    x: 587491.22842068539
+    y: 4140733.1159528242
+    theta: 2.2367042419749406
+    kappa: -0.003126412067745634
+    s: 18.504212505885114
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0686554728429156
-  a: 0.48599005638418447
+  v: 7.121107762056277
+  a: 0.51103734054856809
   relative_time: 2.9499999999999975
 }
 trajectory_point {
   path_point {
-    x: 587491.32568739215
-    y: 4140733.5824832311
-    theta: 2.23129040210936
-    kappa: 5.8548042595101189e-05
-    s: 18.798321075681837
+    x: 587491.00823544327
+    y: 4140733.3965727426
+    theta: 2.2356194280367809
+    kappa: -0.0029741985505679774
+    s: 18.860904506844339
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0928240927541966
-  a: 0.48074131580228613
+  v: 7.1465284117759076
+  a: 0.505773596134113
   relative_time: 2.9999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587491.10774140689
-    y: 4140733.8630094281
-    theta: 2.2313569177416439
-    kappa: 0.00030497284319790389
-    s: 19.153560994818562
+    x: 587490.78748580068
+    y: 4140733.6783570419
+    theta: 2.2345685772637269
+    kappa: -0.0028172472717330568
+    s: 19.21886092297877
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1167282619966663
-  a: 0.47541202963175122
+  v: 7.1716836164594353
+  a: 0.50041953910450776
   relative_time: 3.0499999999999972
 }
 trajectory_point {
   path_point {
-    x: 587490.88902366068
-    y: 4140734.1444410784
-    theta: 2.231514731582017
-    kappa: 0.00053136563918417543
-    s: 19.509989427249032
+    x: 587490.56639879232
+    y: 4140733.9614656903
+    theta: 2.2336028684678677
+    kappa: -0.00265209574139317
+    s: 19.578068369145832
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.140363953290894
-  a: 0.4700021978725798
+  v: 7.1965688604761056
+  a: 0.49497516945975245
   relative_time: 3.099999999999997
 }
 trajectory_point {
   path_point {
-    x: 587490.66955985187
-    y: 4140734.42678102
-    theta: 2.2317220463631551
-    kappa: 0.0007466960403881139
-    s: 19.867592848393855
+    x: 587490.34470906935
+    y: 4140734.2456735484
+    theta: 2.2326705040846444
+    kappa: -0.0024838639873272167
+    s: 19.938513234421414
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1637271393574471
-  a: 0.4645118205247718
+  v: 7.2211796281951584
+  a: 0.48944048719984706
   relative_time: 3.1499999999999968
 }
 trajectory_point {
   path_point {
-    x: 587490.44926614664
-    y: 4140734.7099466636
-    theta: 2.2320409442575455
-    kappa: 0.000919265064284592
-    s: 20.226357532309656
+    x: 587490.12255027681
+    y: 4140734.5310672014
+    theta: 2.2318017493005535
+    kappa: -0.0023106746661449083
+    s: 20.300181682099872
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1868137929168938
-  a: 0.45894089758832735
+  v: 7.2455114039858373
+  a: 0.4838154923247916
   relative_time: 3.1999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587490.22820006486
-    y: 4140734.9939650404
-    theta: 2.2323951346190203
-    kappa: 0.0010685058725153325
-    s: 20.5862695516891
+    x: 587489.8999148556
+    y: 4140734.8176223291
+    theta: 2.2310021427921267
+    kappa: -0.0021364035358107871
+    s: 20.663059649694013
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2096198866898025
-  a: 0.45328942906324632
+  v: 7.269559672217385
+  a: 0.47810018483458605
   relative_time: 3.2499999999999964
 }
 trajectory_point {
   path_point {
-    x: 587490.00633775664
-    y: 4140735.2787996796
-    theta: 2.2328010285411812
-    kappa: 0.0011829681997107095
-    s: 20.947314777860868
+    x: 587489.67669110035
+    y: 4140735.1052339296
+    theta: 2.2302399490709965
+    kappa: -0.0019627055262916833
+    s: 21.027132848935111
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2321413933967413
-  a: 0.44755741494952872
+  v: 7.2933199172590433
+  a: 0.47229456472923037
   relative_time: 3.2999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587489.78362997528
-    y: 4140735.5643939204
-    theta: 2.2332442480321131
-    kappa: 0.0012397998532071269
-    s: 21.309478880789676
+    x: 587489.45305372518
+    y: 4140735.394018881
+    theta: 2.2295667726313946
+    kappa: -0.0018008286126125557
+    s: 21.392386765772905
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2543742857582778
-  a: 0.44174485524717466
+  v: 7.3167876234800548
+  a: 0.46639863200872467
   relative_time: 3.3499999999999961
 }
 trajectory_point {
   path_point {
-    x: 587489.56016307476
-    y: 4140735.8507966083
-    theta: 2.2336985633788027
-    kappa: 0.0012656414545632677
-    s: 21.67274732907628
+    x: 587489.2288461792
+    y: 4140735.6838368969
+    theta: 2.2289356811501464
+    kappa: -0.0016482285999814203
+    s: 21.758806660375591
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2763145364949811
-  a: 0.43585174995618403
+  v: 7.3399582752496633
+  a: 0.46041238667306883
   relative_time: 3.3999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587489.33585122088
-    y: 4140736.1379219973
-    theta: 2.2341535428056636
-    kappa: 0.0012189417813115986
-    s: 22.037105389957439
+    x: 587489.00407843851
+    y: 4140735.9746767743
+    theta: 2.228346952294888
+    kappa: -0.0015049675136974627
+    s: 22.126377567129829
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2979581183274185
-  a: 0.42987809907655694
+  v: 7.3628273569371094
+  a: 0.45433582872226297
   relative_time: 3.4499999999999957
 }
 trajectory_point {
   path_point {
-    x: 587489.11076413631
-    y: 4140736.4258054383
-    theta: 2.2345777742193413
-    kappa: 0.0011137785776247832
-    s: 22.402538129305956
+    x: 587488.77884509228
+    y: 4140736.2665921431
+    theta: 2.2278255300601182
+    kappa: -0.0013835408311644449
+    s: 22.495084294640748
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3193010039761592
-  a: 0.42382390260829322
+  v: 7.3853903529116369
+  a: 0.448168958156307
   relative_time: 3.4999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587488.88493876008
-    y: 4140736.7144565987
-    theta: 2.23497903987845
-    kappa: 0.00096433288672934363
-    s: 22.769030411630666
+    x: 587488.55302262085
+    y: 4140736.559468057
+    theta: 2.2273291913815823
+    kappa: -0.0012735270432340845
+    s: 22.864911425731933
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3403391661617707
-  a: 0.41768916055139305
+  v: 7.4076427475424875
+  a: 0.44191177497520095
   relative_time: 3.5499999999999954
 }
 trajectory_point {
   path_point {
-    x: 587488.65831397392
-    y: 4140737.0038080728
-    theta: 2.2352632770160845
-    kappa: 0.00070083231417279279
-    s: 23.136566900076421
+    x: 587488.32667574484
+    y: 4140736.8533346253
+    theta: 2.2268723352353108
+    kappa: -0.0011830136686955567
+    s: 23.235843317445422
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3610685776048213
-  a: 0.4114738729058563
+  v: 7.4295800251989039
+  a: 0.4355642791789448
   relative_time: 3.5999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587488.43101581
-    y: 4140737.2939387606
-    theta: 2.2354787348552438
-    kappa: 0.00042437392996826285
-    s: 23.50513205642411
+    x: 587488.09981956985
+    y: 4140737.1481832755
+    theta: 2.2264530752065532
+    kappa: -0.0010975779866795915
+    s: 23.607864101041734
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3814852110258791
-  a: 0.40517803967168303
+  v: 7.45119767025013
+  a: 0.42912647076753851
   relative_time: 3.649999999999995
 }
 trajectory_point {
   path_point {
-    x: 587488.20303715032
-    y: 4140737.5848230873
-    theta: 2.2355888221164641
-    kappa: 0.00013241281065826246
-    s: 23.874710141090649
+    x: 587487.87239281763
+    y: 4140737.4439465003
+    theta: 2.2260549365869258
+    kappa: -0.0010116810799937334
+    s: 23.980957681999833
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4015850391455116
-  a: 0.39880166084887325
+  v: 7.4724911670654048
+  a: 0.4225983497409822
   relative_time: 3.6999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587487.974432202
-    y: 4140737.8764832686
-    theta: 2.2355743824293355
-    kappa: -0.00011849012138112442
-    s: 24.24528521312898
+    x: 587487.64446592913
+    y: 4140737.7406578069
+    theta: 2.225697923848903
+    kappa: -0.00091638091554314808
+    s: 24.355107740017157
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4213640346842888
-  a: 0.3923447364374269
+  v: 7.4934560000139747
+  a: 0.41597991609927576
   relative_time: 3.7499999999999947
 }
 trajectory_point {
   path_point {
-    x: 587487.7452309716
-    y: 4140738.1689223135
-    theta: 2.2355004911331107
-    kappa: -0.00034880357973629875
-    s: 24.616841130228067
+    x: 587487.41599917191
+    y: 4140738.0382657032
+    theta: 2.2253755787345875
+    kappa: -0.00081513716518616646
+    s: 24.730297729009603
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4408181703627774
-  a: 0.38580726643734409
+  v: 7.51408765346508
+  a: 0.40927116984241929
   relative_time: 3.7999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587487.51547368139
-    y: 4140738.4621509779
-    theta: 2.2353287918716389
-    kappa: -0.00054248817838501134
-    s: 24.989361548712925
+    x: 587487.18697515561
+    y: 4140738.3367356872
+    theta: 2.2250783523373658
+    kappa: -0.00070965291020819372
+    s: 25.106510877111525
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4599434189015454
-  a: 0.37918925084862465
+  v: 7.5343816117879641
+  a: 0.40247211097041269
   relative_time: 3.8499999999999943
 }
 trajectory_point {
   path_point {
-    x: 587487.28520110378
-    y: 4140738.7561801653
-    theta: 2.2350895605462964
-    kappa: -0.0007073049596426355
-    s: 25.36282992354457
+    x: 587486.95745587884
+    y: 4140738.6360938493
+    theta: 2.2248392458516189
+    kappa: -0.00059714617630812674
+    s: 25.483730186675746
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4787357530211622
-  a: 0.37249068967126875
+  v: 7.5543333593518689
+  a: 0.39558273948325606
   relative_time: 3.8999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587487.05440417153
-    y: 4140739.0509814918
-    theta: 2.2348078281825785
-    kappa: -0.00085342635667027232
-    s: 25.737229508320063
+    x: 587486.72738441743
+    y: 4140738.9362747907
+    theta: 2.2246354267547055
+    kappa: -0.00048183956484120578
+    s: 25.861938434273547
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4971911454421951
-  a: 0.36571158290527628
+  v: 7.5739383805260374
+  a: 0.3886030553809493
   relative_time: 3.949999999999994
 }
 trajectory_point {
   path_point {
-    x: 587486.82317350991
-    y: 4140739.3466043342
-    theta: 2.2344575434434883
-    kappa: -0.00096312265099839046
-    s: 26.112543355272486
+    x: 587486.49676900636
+    y: 4140739.2372628381
+    theta: 2.2244651791179848
+    kappa: -0.00036385873282388409
+    s: 26.241118170694673
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5153055688852124
-  a: 0.35885193055064735
+  v: 7.5931921596797114
+  a: 0.38153305866349246
   relative_time: 3.9999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587486.5914839874
-    y: 4140739.6430073236
-    theta: 2.2340778537150014
-    kappa: -0.0010531073982352358
-    s: 26.488754438131291
+    x: 587486.26563436317
+    y: 4140739.5390548026
+    theta: 2.224362075962552
+    kappa: -0.00024271657676242561
+    s: 26.621251849366587
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5330826345727147
-  a: 0.35223862322118055
+  v: 7.6120981676854713
+  a: 0.37471471730397854
   relative_time: 4.0499999999999936
 }
 trajectory_point {
   path_point {
-    x: 587486.35933174693
-    y: 4140739.9401663369
-    theta: 2.2336730637117528
-    kappa: -0.001126374653220086
-    s: 26.865846127455992
+    x: 587486.03394540108
+    y: 4140739.8416022062
+    theta: 2.2242913772055939
+    kappa: -0.00012081342058440549
+    s: 27.002322324151429
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5505302238345049
-  a: 0.34567287352213572
+  v: 7.6306643769838178
+  a: 0.36794111036697247
   relative_time: 4.0999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587486.12684311043
-    y: 4140740.2381590912
-    theta: 2.2332338489117873
-    kappa: -0.001166965037061384
-    s: 27.243802008872336
+    x: 587485.80171474384
+    y: 4140740.1448933855
+    theta: 2.224260571208283
+    kappa: 1.8748253368862614e-06
+    s: 27.38431266103186
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5676507145521024
-  a: 0.33915468145351368
+  v: 7.6488930242958766
+  a: 0.36121223785247719
   relative_time: 4.1499999999999932
 }
 trajectory_point {
   path_point {
-    x: 587485.89392707334
-    y: 4140740.536893764
-    theta: 2.2327867162490063
-    kappa: -0.0011943357743001148
-    s: 27.622605786900156
+    x: 587485.56893403549
+    y: 4140740.4489007536
+    theta: 2.2242936120979926
+    kappa: 0.00012369119739008948
+    s: 27.767206037826597
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.58444648460703
-  a: 0.33268404701531429
+  v: 7.6667863463427741
+  a: 0.35452809976049265
   relative_time: 4.1999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587485.66061467573
-    y: 4140740.8363740416
-    theta: 2.2323303529099134
-    kappa: -0.0012055639445568258
-    s: 28.00224128495335
+    x: 587485.33560012991
+    y: 4140740.7536008535
+    theta: 2.2243607760388411
+    kappa: 0.00024420280021280217
+    s: 28.1509857441904
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6009199118808075
-  a: 0.32626097020753764
+  v: 7.6843465798456343
+  a: 0.34788869609101891
   relative_time: 4.2499999999999929
 }
 trajectory_point {
   path_point {
-    x: 587485.42699042789
-    y: 4140741.1366452361
-    theta: 2.231874317233955
-    kappa: -0.0011940440113150852
-    s: 28.382692445339906
+    x: 587485.10171377286
+    y: 4140741.0589733506
+    theta: 2.2244731051746252
+    kappa: 0.00036256057989051023
+    s: 28.535635181614108
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6170733742549572
-  a: 0.31988545103018373
+  v: 7.7015759615255845
+  a: 0.34129402684405596
   relative_time: 4.2999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587485.19297209824
-    y: 4140741.4376230454
-    theta: 2.231422819601184
-    kappa: -0.0011722537835406972
-    s: 28.763943329261878
+    x: 587484.86725042923
+    y: 4140741.3649786729
+    theta: 2.2246434972494376
+    kappa: 0.00047613939186752312
+    s: 28.921137863424594
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6329092496109991
-  a: 0.31355748948325252
+  v: 7.7184767281037487
+  a: 0.33474409201960381
   relative_time: 4.3499999999999925
 }
 trajectory_point {
   path_point {
-    x: 587484.95860387071
-    y: 4140741.7393217576
-    theta: 2.2309795050590648
-    kappa: -0.0011370435360430688
-    s: 29.145978116815392
+    x: 587484.63223530073
+    y: 4140741.6716154856
+    theta: 2.2248455315460478
+    kappa: 0.00058637481665605308
+    s: 29.307477414784803
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6484299158304561
-  a: 0.30727708556674405
+  v: 7.7350511163012534
+  a: 0.32823889161766245
   relative_time: 4.3999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587484.72393456125
-    y: 4140742.0417592479
-    theta: 2.2305567185944084
-    kappa: -0.001087178624349473
-    s: 29.528781106990667
+    x: 587484.39665685873
+    y: 4140741.9788544569
+    theta: 2.225089800664986
+    kappa: 0.000691518918178125
+    s: 29.694637572693726
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6636377507948472
-  a: 0.30104423928065827
+  v: 7.7513013628392233
+  a: 0.32177842563823184
   relative_time: 4.4499999999999922
 }
 trajectory_point {
   path_point {
-    x: 587484.48889407562
-    y: 4140742.3448613184
-    theta: 2.2301482276006062
-    kappa: -0.0010303783926130456
-    s: 29.912336717671984
+    x: 587484.16048520664
+    y: 4140742.2866522609
+    theta: 2.2253828438699226
+    kappa: 0.00078830454299527015
+    s: 30.082602185986413
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6785351323856954
-  a: 0.29485895062499523
+  v: 7.767229704438785
+  a: 0.315362694081312
   relative_time: 4.499999999999992
 }
 trajectory_point {
   path_point {
-    x: 587484.25352776342
-    y: 4140742.6486435276
-    theta: 2.2297630046226118
-    kappa: -0.00096462102952763048
-    s: 30.296629485637702
+    x: 587483.92376110773
+    y: 4140742.5950199151
+    theta: 2.2257035290369571
+    kappa: 0.00087924502217237772
+    s: 30.471355215333979
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6931244384845208
-  a: 0.28872121959975483
+  v: 7.7828383778210632
+  a: 0.308991696946903
   relative_time: 4.5499999999999918
 }
 trajectory_point {
   path_point {
-    x: 587484.01785157586
-    y: 4140742.9530987046
-    theta: 2.2294099082596386
-    kappa: -0.00089184479387815639
-    s: 30.681644066560256
+    x: 587483.68645936786
+    y: 4140742.9039179785
+    theta: 2.2260607016419915
+    kappa: 0.00096140025098183
+    s: 30.860880733243583
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7074080469728443
-  a: 0.28263104620493723
+  v: 7.7981296197071837
+  a: 0.30266543423500475
   relative_time: 4.5999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587483.78182017547
-    y: 4140743.2581724497
-    theta: 2.2290772495504045
-    kappa: -0.00081574261718282866
-    s: 31.067365235006164
+    x: 587483.44855180092
+    y: 4140743.2133047441
+    theta: 2.2264544743962973
+    kappa: 0.0010311810739608823
+    s: 31.251162924058445
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.721388335732188
-  a: 0.27658843044054238
+  v: 7.8131056668182719
+  a: 0.29638390594561731
   relative_time: 4.6499999999999915
 }
 trajectory_point {
   path_point {
-    x: 587483.54547766817
-    y: 4140743.5638797525
-    theta: 2.2287767172802968
-    kappa: -0.000735846499531659
-    s: 31.453777884436015
+    x: 587483.2100986992
+    y: 4140743.5232066917
+    theta: 2.2268678620840543
+    kappa: 0.0010927147084423931
+    s: 31.642186083957842
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7350676826440719
-  a: 0.27059337230657016
+  v: 7.8277687558754536
+  a: 0.29014711207874061
   relative_time: 4.6999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587483.308818166
-    y: 4140743.8701970391
-    theta: 2.22851167251071
-    kappa: -0.00065565336306633326
-    s: 31.840867027204471
+    x: 587482.97107315413
+    y: 4140743.8335834062
+    theta: 2.227305079464982
+    kappa: 0.0011425656193301164
+    s: 32.033934620957105
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7484484655900179
-  a: 0.26464587180302068
+  v: 7.8421211235998545
+  a: 0.28395505263437471
   relative_time: 4.7499999999999911
 }
 trajectory_point {
   path_point {
-    x: 587483.071810753
-    y: 4140744.1770816105
-    theta: 2.2282687836870783
-    kappa: -0.00057836420001629784
-    s: 32.228617794560279
+    x: 587482.73144325032
+    y: 4140744.1443906166
+    theta: 2.227762625214357
+    kappa: 0.0011760622397503544
+    s: 32.426393054907628
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7615330624515479
-  a: 0.25874592892989395
+  v: 7.8561650067125992
+  a: 0.27780772761251959
   relative_time: 4.7999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587482.83448582969
-    y: 4140744.4845382893
-    theta: 2.228058955093331
-    kappa: -0.0005098946162072881
-    s: 32.617015436646241
+    x: 587482.49127819261
+    y: 4140744.455662169
+    theta: 2.2282287362531794
+    kappa: 0.0011984452041510156
+    s: 32.819546017496855
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7743238511101813
-  a: 0.25289354368718991
+  v: 7.8699026419348144
+  a: 0.27170513701317528
   relative_time: 4.8499999999999908
 }
 trajectory_point {
   path_point {
-    x: 587482.59684293414
-    y: 4140744.7925481978
-    theta: 2.2278707893231222
-    kappa: -0.00046853283186076904
-    s: 33.006045322499268
+    x: 587482.2505513439
+    y: 4140744.767358196
+    theta: 2.2287029290044882
+    kappa: 0.0012057924128565736
+    s: 33.213378252248283
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7868232094474408
-  a: 0.24708871607490862
+  v: 7.8833362659876256
+  a: 0.26564728083634176
   relative_time: 4.8999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587482.35886610183
-    y: 4140745.1010806775
-    theta: 2.2276922309167633
-    kappa: -0.00044355023425052649
-    s: 33.395692940050317
+    x: 587482.00923055038
+    y: 4140745.0794345411
+    theta: 2.229175444031434
+    kappa: 0.0011919242872481372
+    s: 33.607874614521478
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7990335153448456
-  a: 0.24133144609305002
+  v: 7.8964681155921568
+  a: 0.259634159082019
   relative_time: 4.94999999999999
 }
 trajectory_point {
   path_point {
-    x: 587482.12058306416
-    y: 4140745.4101389293
-    theta: 2.2275213006276142
-    kappa: -0.00044081696259016433
-    s: 33.785943896124444
+    x: 587481.76740141015
+    y: 4140745.3919382724
+    theta: 2.2296416992295103
+    kappa: 0.001164406272267013
+    s: 34.003020071512047
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8109571466839185
-  a: 0.23562173374161416
+  v: 7.9093004274695353
+  a: 0.253665771750207
   relative_time: 4.99999999999999
 }
 trajectory_point {
   path_point {
-    x: 587481.88199927658
-    y: 4140745.7197091086
-    theta: 2.22734374329057
-    kappa: -0.00046450471892416642
-    s: 34.176783916440762
+    x: 587481.52505115338
+    y: 4140745.7048406042
+    theta: 2.2300976655076927
+    kappa: 0.001120056848808943
+    s: 34.398799702251658
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8225964813461806
-  a: 0.229959579020601
+  v: 7.921835438340886
+  a: 0.24774211884090583
   relative_time: 5.04999999999999
 }
 trajectory_point {
   path_point {
-    x: 587481.643106494
-    y: 4140746.0297669787
-    theta: 2.2271558973857948
-    kappa: -0.00050734530305279974
-    s: 34.568198845612471
+    x: 587481.282140685
+    y: 4140746.0180923594
+    theta: 2.2305240122470957
+    kappa: 0.0010495069544156255
+    s: 34.795198697608051
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8339538972131528
-  a: 0.22434498193001054
+  v: 7.9340753849273344
+  a: 0.24186320035411546
   relative_time: 5.09999999999999
 }
 trajectory_point {
   path_point {
-    x: 587481.403936159
-    y: 4140746.3403190351
-    theta: 2.2269450865042471
-    kappa: -0.00058016292412618059
-    s: 34.960174647146843
+    x: 587481.03875921178
+    y: 4140746.3317440012
+    theta: 2.230924741329682
+    kappa: 0.00096198360392506146
+    s: 35.192202360284995
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8450317721663554
-  a: 0.21877794246984283
+  v: 7.9460225039500063
+  a: 0.23602901628983583
   relative_time: 5.14999999999999
 }
 trajectory_point {
   path_point {
-    x: 587481.16451880243
-    y: 4140746.6513711852
-    theta: 2.2266977190943442
-    kappa: -0.00065770312157618287
-    s: 35.352697403445234
+    x: 587480.794907049
+    y: 4140746.645777293
+    theta: 2.2312949251135006
+    kappa: 0.00085537733360930928
+    s: 35.589796104822341
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.85583248408731
-  a: 0.21325846064009782
+  v: 7.9576790321300264
+  a: 0.230239566648067
   relative_time: 5.1999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587480.92483855539
-    y: 4140746.9628937747
-    theta: 2.2264276504672673
-    kappa: -0.000731535156547849
-    s: 35.745753315803057
+    x: 587480.55055444373
+    y: 4140746.9601506954
+    theta: 2.2315971041892868
+    kappa: 0.0007175510081693817
+    s: 35.987965457595969
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8663584108575391
-  a: 0.20778653644077555
+  v: 7.9690472061885211
+  a: 0.22449485142880893
   relative_time: 5.2499999999999893
 }
 trajectory_point {
   path_point {
-    x: 587480.68493433064
-    y: 4140747.2748994632
-    theta: 2.2261278920751812
-    kappa: -0.000792100776465532
-    s: 36.139328704409827
+    x: 587480.30579351541
+    y: 4140747.2749175509
+    theta: 2.2318527127721413
+    kappa: 0.00056271145551689908
+    s: 36.386696056817847
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8766119303585622
-  a: 0.20236216987187597
+  v: 7.9801292628466163
+  a: 0.21879487063206166
   relative_time: 5.2999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587480.44483675179
-    y: 4140747.5873946175
-    theta: 2.2258035057669212
-    kappa: -0.00083920993563822081
-    s: 36.533410008349122
+    x: 587480.06063521421
+    y: 4140747.5900683175
+    theta: 2.23206017184729
+    kappa: 0.00039080857782308233
+    s: 36.785973652535972
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8865954204719007
-  a: 0.19698536093339913
+  v: 7.9909274388254357
+  a: 0.21313962425782518
   relative_time: 5.349999999999989
 }
 trajectory_point {
   path_point {
-    x: 587480.20451814006
-    y: 4140747.9003409739
-    theta: 2.2254659014590041
-    kappa: -0.00087805697228563481
-    s: 36.927983785598585
+    x: 587479.815079075
+    y: 4140747.9055846906
+    theta: 2.2321618579611373
+    kappa: 0.00018870409028312943
+    s: 37.185784106634415
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8963112590790754
-  a: 0.191656109625345
+  v: 8.0014439708461076
+  a: 0.20752911230609947
   relative_time: 5.3999999999999888
 }
 trajectory_point {
   path_point {
-    x: 587479.964016056
-    y: 4140748.213750551
-    theta: 2.2251134659496943
-    kappa: -0.00090634004420064168
-    s: 37.32303671302995
+    x: 587479.56919405062
+    y: 4140748.2215025472
+    theta: 2.232191422224131
+    kappa: -3.0543630645974524e-05
+    s: 37.5861133928333
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9057618240616083
-  a: 0.18637441594771359
+  v: 8.0116810956297542
+  a: 0.20196333477688452
   relative_time: 5.4499999999999886
 }
 trajectory_point {
   path_point {
-    x: 587479.72337295522
-    y: 4140748.527639179
-    theta: 2.224750402045355
-    kappa: -0.00092308067931299372
-    s: 37.718555586409018
+    x: 587479.3229952238
+    y: 4140748.5378159937
+    theta: 2.23214930767958
+    kappa: -0.00026630001790860821
+    s: 37.986947596688793
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9149494933010205
-  a: 0.18114027990050488
+  v: 8.0216410498975037
+  a: 0.1964422916701804
   relative_time: 5.4999999999999885
 }
 trajectory_point {
   path_point {
-    x: 587479.48254316731
-    y: 4140748.8419552892
-    theta: 2.2243831563763989
-    kappa: -0.00093314113909858233
-    s: 38.114527320395688
+    x: 587479.07653645868
+    y: 4140748.8545494596
+    theta: 2.231976129615588
+    kappa: -0.00052808418768521259
+    s: 38.388272915593134
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9238766446788329
-  a: 0.17595370148371892
+  v: 8.031326070370481
+  a: 0.19096598298598705
   relative_time: 5.5499999999999883
 }
 trajectory_point {
   path_point {
-    x: 587479.24155470729
-    y: 4140749.1567039839
-    theta: 2.2240123831165262
-    kappa: -0.00093529749536839948
-    s: 38.510938948543895
+    x: 587478.82985841762
+    y: 4140749.1717172125
+    theta: 2.2317005552751885
+    kappa: -0.00080708915108269235
+    s: 38.790075658774612
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9325456560765657
-  a: 0.17081468069735567
+  v: 8.04073839376981
+  a: 0.18553440872430446
   relative_time: 5.5999999999999881
 }
 trajectory_point {
   path_point {
-    x: 587479.00045701186
-    y: 4140749.471906824
-    theta: 2.223642870685461
-    kappa: -0.00092798670523522742
-    s: 38.9077776233017
+    x: 587478.58297270583
+    y: 4140749.4893110488
+    theta: 2.2313356271117017
+    kappa: -0.0010959915699750209
+    s: 39.192342247297567
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9409589053757417
-  a: 0.16572321754141514
+  v: 8.04988025681662
+  a: 0.1801475688851327
   relative_time: 5.6499999999999879
 }
 trajectory_point {
   path_point {
-    x: 587478.75919747865
-    y: 4140749.7875074684
-    theta: 2.2232763821735713
-    kappa: -0.00091565546560119684
-    s: 39.305030616011187
+    x: 587478.33596605156
+    y: 4140749.8073812732
+    theta: 2.2308320749746757
+    kappa: -0.0013855428750630481
+    s: 39.595059214062417
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.949118770457881
-  a: 0.1606793120158973
+  v: 8.058753896232032
+  a: 0.1748054634684717
   relative_time: 5.6999999999999877
 }
 trajectory_point {
   path_point {
-    x: 587478.51779437636
-    y: 4140750.1035039928
-    theta: 2.222914105301498
-    kappa: -0.00089791942179803948
-    s: 39.702685316908557
+    x: 587478.08889250481
+    y: 4140750.1259527393
+    theta: 2.2302078644119123
+    kappa: -0.0016551334799288192
+    s: 39.9982132038056
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9570276292045055
-  a: 0.15568296412080218
+  v: 8.0673615487371748
+  a: 0.16950809247432147
   relative_time: 5.7499999999999876
 }
 trajectory_point {
   path_point {
-    x: 587478.27631359489
-    y: 4140750.4199309042
-    theta: 2.2225634992716423
-    kappa: -0.000873062157619381
-    s: 40.100729235124071
+    x: 587477.84173108893
+    y: 4140750.4449922522
+    theta: 2.2294977582202624
+    kappa: -0.0019093079504734836
+    s: 40.401790973099644
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9646878594971344
-  a: 0.15073417385612981
+  v: 8.0757054510531727
+  a: 0.16425545590268203
   relative_time: 5.7999999999999874
 }
 trajectory_point {
   path_point {
-    x: 587478.03468867287
-    y: 4140750.7367218328
-    theta: 2.2222211511095842
-    kappa: -0.00084491324431976785
-    s: 40.499149998682057
+    x: 587477.59456677793
+    y: 4140750.76454889
+    theta: 2.2286820596078
+    kappa: -0.0021427505292687075
+    s: 40.805779390353109
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9721018392172915
-  a: 0.14583294122188015
+  v: 8.0837878399011522
+  a: 0.15904755375355339
   relative_time: 5.8499999999999872
 }
 trajectory_point {
   path_point {
-    x: 587477.7929267498
-    y: 4140751.0538668032
-    theta: 2.2218873617130486
-    kappa: -0.00081356292207159181
-    s: 40.897935354500945
+    x: 587477.34749818675
+    y: 4140751.0846819081
+    theta: 2.2277593454048503
+    kappa: -0.0023516862061765992
+    s: 41.210165435810637
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9792719462464969
-  a: 0.14097926621805321
+  v: 8.0916109520022363
+  a: 0.15388438602693555
   relative_time: 5.899999999999987
 }
 trajectory_point {
   path_point {
-    x: 587477.55109954427
-    y: 4140751.3714051424
-    theta: 2.2215727537450558
-    kappa: -0.00077784696404137169
-    s: 41.297073168393219
+    x: 587477.10044064792
+    y: 4140751.4053092077
+    theta: 2.2267720985199828
+    kappa: -0.0025441842416432984
+    s: 41.614936201552908
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9862005584662707
-  a: 0.13617314884464896
+  v: 8.0991770240775534
+  a: 0.14876595272282847
   relative_time: 5.9499999999999869
 }
 trajectory_point {
   path_point {
-    x: 587477.30914295092
-    y: 4140751.6892728461
-    theta: 2.22126970764997
-    kappa: -0.000740598888081858
-    s: 41.696551425065437
+    x: 587476.85343526222
+    y: 4140751.7264461545
+    theta: 2.2257172852418416
+    kappa: -0.0027185331486999855
+    s: 42.020078891496645
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9928900537581349
-  a: 0.13141458910166745
+  v: 8.106488292848228
+  a: 0.14369225384123216
   relative_time: 5.9999999999999867
 }
 trajectory_point {
   path_point {
-    x: 587477.06706282462
-    y: 4140752.0074596144
-    theta: 2.2209782383164454
-    kappa: -0.00070196224721899255
-    s: 42.096358390400368
+    x: 587476.60669241357
+    y: 4140752.0482378537
+    theta: 2.2245738499849907
+    kappa: -0.0028638255385413213
+    s: 42.425580988366974
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.999352558405862
-  a: 0.12709444110887322
+  v: 8.1135570306319575
+  a: 0.13906609386375549
   relative_time: 6.0499999999999865
 }
 trajectory_point {
   path_point {
-    x: 587476.82491345471
-    y: 4140752.3259929223
-    theta: 2.2207077470569239
-    kappa: -0.00066223077696862429
-    s: 42.49648310289345
+    x: 587476.36005139991
+    y: 4140752.3705453495
+    theta: 2.2233855795783919
+    kappa: -0.0029914478839353394
+    s: 42.83143076151741
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0056003822991624
-  a: 0.12282735892455336
+  v: 8.1203957858701852
+  a: 0.134492952021225
   relative_time: 6.0999999999999863
 }
 trajectory_point {
   path_point {
-    x: 587476.58265222318
-    y: 4140752.6448269081
-    theta: 2.2204514012004615
-    kappa: -0.0006227432452568381
-    s: 42.896914894839213
+    x: 587476.11352465523
+    y: 4140752.69336358
+    theta: 2.2221545456592784
+    kappa: -0.00310127147613376
+    s: 43.237616778093347
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0116361787284589
-  a: 0.118613342548708
+  v: 8.1270072094696619
+  a: 0.1299728283136404
   relative_time: 6.1499999999999861
 }
 trajectory_point {
   path_point {
-    x: 587476.34027444
-    y: 4140752.9639447657
-    theta: 2.2202072741192214
-    kappa: -0.0005837259125492058
-    s: 43.29764323119673
+    x: 587475.867343713
+    y: 4140753.0168540804
+    theta: 2.2208704447334937
+    kappa: -0.0031815795921022049
+    s: 43.64412773778551
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0174626009841745
-  a: 0.11445239198133716
+  v: 8.1333939523371317
+  a: 0.12550572274100177
   relative_time: 6.199999999999986
 }
 trajectory_point {
   path_point {
-    x: 587476.09781483363
-    y: 4140753.28335978
-    theta: 2.21998178245099
-    kappa: -0.00054640056816204015
-    s: 43.698657709589575
+    x: 587475.62135344069
+    y: 4140753.3408834268
+    theta: 2.2195619532996016
+    kappa: -0.0032416475840242112
+    s: 44.05095247282997
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0230823023567339
-  a: 0.1103445072224408
+  v: 8.1395586653793437
+  a: 0.12109163530330906
   relative_time: 6.2499999999999858
 }
 trajectory_point {
   path_point {
-    x: 587475.8552584392
-    y: 4140753.603047655
-    theta: 2.2197707923651477
-    kappa: -0.00051163012886013925
-    s: 44.099948060305856
+    x: 587475.37552313379
+    y: 4140753.6654140833
+    theta: 2.218235226548086
+    kappa: -0.0032835209972782867
+    s: 44.458079948008134
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.02849793613656
-  a: 0.10628968827201894
+  v: 8.1455039995030454
+  a: 0.11673056600056228
   relative_time: 6.2999999999999856
 }
 trajectory_point {
   path_point {
-    x: 587475.61259430659
-    y: 4140753.922987347
-    theta: 2.2195703840722616
-    kappa: -0.00047923408850694384
-    s: 44.501504146298188
+    x: 587475.13001437159
+    y: 4140753.9905539672
+    theta: 2.2168927828238321
+    kappa: -0.0032991019711798861
+    s: 44.865499260646743
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0337121556140776
-  a: 0.10228793513007159
+  v: 8.1512326056149824
+  a: 0.11242251483276143
   relative_time: 6.3499999999999854
 }
 trajectory_point {
   path_point {
-    x: 587475.36984668346
-    y: 4140754.2431846736
-    theta: 2.2193836645885074
-    kappa: -0.00045079172410787259
-    s: 44.903315963183729
+    x: 587474.884810063
+    y: 4140754.3162752222
+    theta: 2.2155500008583715
+    kappa: -0.0032894848673760187
+    s: 45.273199640617875
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0387276140797077
-  a: 0.098339247796598722
+  v: 8.156747134621904
+  a: 0.10816748179990651
   relative_time: 6.3999999999999853
 }
 trajectory_point {
   path_point {
-    x: 587475.12701586308
-    y: 4140754.5636274386
-    theta: 2.2192084631654625
-    kappa: -0.00042780596884795397
-    s: 45.305373639244131
+    x: 587474.63979935274
+    y: 4140754.6424803659
+    theta: 2.2142129473475922
+    kappa: -0.0032608183803317253
+    s: 45.681170450338954
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0435469648238787
-  a: 0.094443626271600356
+  v: 8.1620502374305559
+  a: 0.10396546690199752
   relative_time: 6.4499999999999851
 }
 trajectory_point {
   path_point {
-    x: 587474.88408549374
-    y: 4140754.8842910193
-    theta: 2.2190396402179395
-    kappa: -0.00040912776538286409
-    s: 45.7076674354256
+    x: 587474.39505870035
+    y: 4140754.9692132012
+    theta: 2.2128883276027294
+    kappa: -0.0032092753199486957
+    s: 46.08940118477274
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.04817286113701
-  a: 0.090601070555076491
+  v: 8.1671445649476855
+  a: 0.099816470139034469
   relative_time: 6.4999999999999849
 }
 trajectory_point {
   path_point {
-    x: 587474.64106884785
-    y: 4140755.2051734114
-    theta: 2.218877421845836
-    kappa: -0.00039605042847960103
-    s: 46.11018774533882
+    x: 587474.15072224243
+    y: 4140755.2965597808
+    theta: 2.2115985624059689
+    kappa: -0.0031277454263311851
+    s: 46.497881471427306
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0526079563095259
-  a: 0.086811580647027112
+  v: 8.172032768080042
+  a: 0.095720491511017344
   relative_time: 6.5499999999999847
 }
 trajectory_point {
   path_point {
-    x: 587474.39798627025
-    y: 4140755.5262781186
-    theta: 2.2187193167894983
-    kappa: -0.00039194105078242626
-    s: 46.512925095259043
+    x: 587473.90658057982
+    y: 4140755.6243499964
+    theta: 2.210334123663138
+    kappa: -0.0030536317782041255
+    s: 46.9066010703561
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0568549036318533
-  a: 0.083075156547452234
+  v: 8.1767174977343711
+  a: 0.091677531017946151
   relative_time: 6.5999999999999845
 }
 trajectory_point {
   path_point {
-    x: 587474.15481967432
-    y: 4140755.8475797167
-    theta: 2.2185612504132255
-    kappa: -0.00039283047042339072
-    s: 46.915870144126011
+    x: 587473.66263686889
+    y: 4140755.9525733562
+    theta: 2.2090949831350533
+    kappa: -0.0029940066078378716
+    s: 47.315549874157888
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0609163563944133
-  a: 0.079391798256351856
+  v: 8.1812014048174184
+  a: 0.087687588659820875
   relative_time: 6.6499999999999844
 }
 trajectory_point {
   path_point {
-    x: 587473.91157590784
-    y: 4140756.1690718364
-    theta: 2.21840258609469
-    kappa: -0.00039869813458877872
-    s: 47.319013683543993
+    x: 587473.41911620647
+    y: 4140756.2813835852
+    theta: 2.2078747024794758
+    kappa: -0.0029853861016935971
+    s: 47.724717907976768
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.06479496788763
-  a: 0.075761505773725979
+  v: 8.185487140235935
+  a: 0.083750664436641545
   relative_time: 6.6999999999999842
 }
 trajectory_point {
   path_point {
-    x: 587473.668292285
-    y: 4140756.4907713095
-    theta: 2.2182384858582811
-    kappa: -0.00041114435948754537
-    s: 47.722346637781783
+    x: 587473.1758169455
+    y: 4140756.6106178816
+    theta: 2.206648635683766
+    kappa: -0.0030041754724711646
+    s: 48.134095329502181
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0684933914019261
-  a: 0.072184279099574589
+  v: 8.1895773548966648
+  a: 0.079866758348408146
   relative_time: 6.749999999999984
 }
 trajectory_point {
   path_point {
-    x: 587473.42494253546
-    y: 4140756.8126470335
-    theta: 2.218069385419422
-    kappa: -0.00042662737523901104
-    s: 48.125860063772706
+    x: 587472.93272845482
+    y: 4140756.9402560312
+    theta: 2.2054127702639992
+    kappa: -0.0030475755265842244
+    s: 48.543672428968918
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0720142802277284
-  a: 0.068660118233897713
+  v: 8.1934746997063588
+  a: 0.076035870395120678
   relative_time: 6.7999999999999838
 }
 trajectory_point {
   path_point {
-    x: 587473.18153255386
-    y: 4140757.1346924184
-    theta: 2.217894973539809
-    kappa: -0.00044503202647547192
-    s: 48.52954515111459
+    x: 587472.69000713143
+    y: 4140757.270400723
+    theta: 2.20414928395204
+    kappa: -0.0031220679684557167
+    s: 48.953439629157074
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0753602876554584
-  a: 0.06518902317669531
+  v: 8.19718182557176
+  a: 0.072258000576779141
   relative_time: 6.8499999999999837
 }
 trajectory_point {
   path_point {
-    x: 587472.93809933891
-    y: 4140757.4569245307
-    theta: 2.2177097028765647
-    kappa: -0.000467348052284773
-    s: 48.933393222069789
+    x: 587472.44763317029
+    y: 4140757.60102438
+    theta: 2.2028456171477795
+    kappa: -0.003219232550435395
+    s: 49.363387485392124
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.07853406697554
-  a: 0.061770993927967421
+  v: 8.2007013833996183
+  a: 0.068533148893383536
   relative_time: 6.8999999999999835
 }
 trajectory_point {
   path_point {
-    x: 587472.69463064428
-    y: 4140757.7793233823
-    theta: 2.2175153251905395
-    kappa: -0.00049168261286927509
-    s: 49.337395731565188
+    x: 587472.205518623
+    y: 4140757.9320503059
+    theta: 2.2015061047833449
+    kappa: -0.0033312448273763043
+    s: 49.773506685544859
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0815382714783954
-  a: 0.058406030487714033
+  v: 8.20403602409668
+  a: 0.064861315344933848
   relative_time: 6.9499999999999833
 }
 trajectory_point {
   path_point {
-    x: 587472.45112539665
-    y: 4140758.1018776121
-    theta: 2.2173128333726209
-    kappa: -0.000517521577291563
-    s: 49.741544267192182
+    x: 587471.9637444471
+    y: 4140758.2635258646
+    theta: 2.2001200388209119
+    kappa: -0.0034584480997495622
+    s: 50.183788050031396
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0843755544544518
-  a: 0.055094132855935132
+  v: 8.207188398569695
+  a: 0.061242499931430106
   relative_time: 6.9999999999999831
 }
 trajectory_point {
   path_point {
-    x: 587472.2076166966
-    y: 4140758.4246018142
-    theta: 2.2170978996989281
-    kappa: -0.00054493264852239847
-    s: 50.145830549206686
+    x: 587471.7224912846
+    y: 4140758.5955700055
+    theta: 2.198662340691099
+    kappa: -0.0035989852132379454
+    s: 50.5942225318132
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0870485691941312
-  a: 0.051835301032630732
+  v: 8.2101611577254054
+  a: 0.057676702652872294
   relative_time: 7.0499999999999829
 }
 trajectory_point {
   path_point {
-    x: 587471.96410763473
-    y: 4140758.7474880768
-    theta: 2.2168706086088803
-    kappa: -0.00057291931183975071
-    s: 50.550246430529157
+    x: 587471.48156005691
+    y: 4140758.9280257276
+    theta: 2.1971555474150319
+    kappa: -0.0037444051869946317
+    s: 51.004801216397084
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0895599689878566
-  a: 0.048629535017800832
+  v: 8.212956952470563
+  a: 0.054163923509260414
   relative_time: 7.0999999999999828
 }
 trajectory_point {
   path_point {
-    x: 587471.720585072
-    y: 4140759.0705164331
-    theta: 2.216634154730472
-    kappa: -0.00060096815137968169
-    s: 50.954783896744544
+    x: 587471.24096209684
+    y: 4140759.2608900475
+    theta: 2.1955988636172621
+    kappa: -0.0038935329579026785
+    s: 51.415515321835194
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0919124071260526
-  a: 0.045476834811445432
+  v: 8.2155784337119151
+  a: 0.050704162500594466
   relative_time: 7.1499999999999826
 }
 trajectory_point {
   path_point {
-    x: 587471.47707093868
-    y: 4140759.39369353
-    theta: 2.2163862609673384
-    kappa: -0.00062872633747312445
-    s: 51.359435066102336
+    x: 587471.00100594212
+    y: 4140759.5943735857
+    theta: 2.1939588153322447
+    kappa: -0.0040419731544265574
+    s: 51.826356198724994
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0941085368991423
-  a: 0.04237720041356452
+  v: 8.2180282523562056
+  a: 0.047297419626874435
   relative_time: 7.1999999999999824
 }
 trajectory_point {
   path_point {
-    x: 587471.23359552142
-    y: 4140759.7170324349
-    theta: 2.2161246354709978
-    kappa: -0.00065509597926063164
-    s: 51.764192189516542
+    x: 587470.76148254192
+    y: 4140759.9283132851
+    theta: 2.192265041896579
+    kappa: -0.0041850178913459607
+    s: 52.237315330209306
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.09615101159755
-  a: 0.039330631824158108
+  v: 8.2203090593101837
+  a: 0.043943694888100349
   relative_time: 7.2499999999999822
 }
 trajectory_point {
   path_point {
-    x: 587470.990130574
-    y: 4140760.0405023061
-    theta: 2.2158546934651966
-    kappa: -0.00068007754851049425
-    s: 52.169047650565687
+    x: 587470.52236445353
+    y: 4140760.26267849
+    theta: 2.1905234138196965
+    kappa: -0.004321471371552919
+    s: 52.648384331976281
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0980424845117
-  a: 0.036337129043226196
+  v: 8.2224235054805987
+  a: 0.040642988284272194
   relative_time: 7.2999999999999821
 }
 trajectory_point {
   path_point {
-    x: 587470.74668486649
-    y: 4140760.3641003678
-    theta: 2.2155762665101433
-    kappa: -0.00070344125146490214
-    s: 52.573993965492818
+    x: 587470.2838530934
+    y: 4140760.5976017797
+    theta: 2.1887199528210544
+    kappa: -0.0044448045435403856
+    s: 53.059554952259418
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.099785608932013
-  a: 0.033396692070768771
+  v: 8.2243742417741945
+  a: 0.037395299815389971
   relative_time: 7.3499999999999819
 }
 trajectory_point {
   path_point {
-    x: 587470.503320477
-    y: 4140760.6878640568
-    theta: 2.2152855564005574
-    kappa: -0.00072292470127913134
-    s: 52.9790237832055
+    x: 587470.04597386555
+    y: 4140760.933088671
+    theta: 2.1868648196546294
+    kappa: -0.0045489185721598272
+    s: 53.470819071837532
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1013830381489171
-  a: 0.030509320906785861
+  v: 8.22616391909772
+  a: 0.034200629481453665
   relative_time: 7.3999999999999817
 }
 trajectory_point {
   path_point {
-    x: 587470.259986893
-    y: 4140761.0117463088
-    theta: 2.2149892175156389
-    kappa: -0.00073955498576791382
-    s: 53.384129885275819
+    x: 587469.8085621316
+    y: 4140761.26901114
+    theta: 2.1849775412987578
+    kappa: -0.0046359522036268589
+    s: 53.8821687040348
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.102837425452833
-  a: 0.027675015551277438
+  v: 8.2277951883579217
+  a: 0.031058977282463304
   relative_time: 7.4499999999999815
 }
 trajectory_point {
   path_point {
-    x: 587470.016688873
-    y: 4140761.3357418263
-    theta: 2.2146876194342364
-    kappa: -0.00075319256825000756
-    s: 53.7893051859404
+    x: 587469.57168213092
+    y: 4140761.6054041847
+    theta: 2.1830588795934558
+    kappa: -0.0047022568485744973
+    s: 54.293595994720711
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1041514241341854
-  a: 0.024893776004243515
+  v: 8.229270700461548
+  a: 0.027970343218418875
   relative_time: 7.4999999999999813
 }
 trajectory_point {
   path_point {
-    x: 587469.77348224376
-    y: 4140761.6598837771
-    theta: 2.2143800511191052
-    kappa: -0.00076104388797233269
-    s: 54.194542732100366
+    x: 587469.33563308872
+    y: 4140761.9424661193
+    theta: 2.1811141824341167
+    kappa: -0.0047303376684377611
+    s: 54.705093222310111
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.105327687483399
-  a: 0.022165602265684078
+  v: 8.2305931063153466
+  a: 0.024934727289320363
   relative_time: 7.5499999999999812
 }
 trajectory_point {
   path_point {
-    x: 587469.5303357048
-    y: 4140761.9841400739
-    theta: 2.2140708545638095
-    kappa: -0.00076399280492223565
-    s: 54.599835703321375
+    x: 587469.10008895537
+    y: 4140762.2799567431
+    theta: 2.1791669732865007
+    kappa: -0.0047306906160851977
+    s: 55.116652797763187
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.106368868790895
-  a: 0.019490494335599157
+  v: 8.2317650568260632
+  a: 0.021952129495167796
   relative_time: 7.599999999999981
 }
 trajectory_point {
   path_point {
-    x: 587469.2872404923
-    y: 4140762.3084957628
-    theta: 2.2137612443171086
-    kappa: -0.00076247907080705828
-    s: 55.0051774118336
+    x: 587468.86505445046
+    y: 4140762.6178696542
+    theta: 2.1772219027194151
+    kappa: -0.00470200935774405
+    s: 55.528267264585445
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1072776213471
-  a: 0.016868452213988722
+  v: 8.2327892029004452
+  a: 0.019022549835961161
   relative_time: 7.6499999999999808
 }
 trajectory_point {
   path_point {
-    x: 587469.04423508991
-    y: 4140762.6329714521
-    theta: 2.2134534486562734
-    kappa: -0.00075403295024602365
-    s: 55.410561302531733
+    x: 587468.630829565
+    y: 4140762.956402272
+    theta: 2.1753032949225064
+    kappa: -0.0046221905827188827
+    s: 55.939929298827735
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1080565984424364
-  a: 0.014299475900852773
+  v: 8.233668195445242
+  a: 0.016145988311700443
   relative_time: 7.6999999999999806
 }
 trajectory_point {
   path_point {
-    x: 587468.80131854338
-    y: 4140762.9575583264
-    theta: 2.2131515842378588
-    kappa: -0.00073799059735327253
-    s: 55.815980952974996
+    x: 587468.39719995193
+    y: 4140763.2953946218
+    theta: 2.1734288843786533
+    kappa: -0.00449817748046346
+    s: 56.351631709086256
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1087084533673259
-  a: 0.011783565396191339
+  v: 8.2344046853671973
+  a: 0.013322444922385657
   relative_time: 7.7499999999999805
 }
 trajectory_point {
   path_point {
-    x: 587468.55846245144
-    y: 4140763.2822272363
-    theta: 2.2128560355832905
-    kappa: -0.00071601428822026716
-    s: 56.221430073387133
+    x: 587468.1640685586
+    y: 4140763.634770296
+    theta: 2.1716026729887732
+    kappa: -0.0043343394099278065
+    s: 56.76336743650252
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1092358394121966
-  a: 0.0093207207000044062
+  v: 8.2350013235730621
+  a: 0.010551919668016829
   relative_time: 7.79999999999998
 }
 trajectory_point {
   path_point {
-    x: 587468.315684874
-    y: 4140763.6069839792
-    theta: 2.2125698880097171
-    kappa: -0.0006865123794482942
-    s: 56.626902506656386
+    x: 587467.93156636518
+    y: 4140763.9746096293
+    theta: 2.1698545787583243
+    kappa: -0.004116315930791631
+    s: 57.1751295547634
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1096414098674678
-  a: 0.0069109418122919736
+  v: 8.2354607609695822
+  a: 0.0078344125485939053
   relative_time: 7.84999999999998
 }
 trajectory_point {
   path_point {
-    x: 587468.07301055407
-    y: 4140763.9318394642
-    theta: 2.2123020705883878
-    kappa: -0.00064613815470518008
-    s: 57.032392228335553
+    x: 587467.69973335951
+    y: 4140764.3149293447
+    theta: 2.1682331160653314
+    kappa: -0.0038299174680867481
+    s: 57.5869112701011
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1099278180235661
-  a: 0.0045542287330540276
+  v: 8.2357856484635033
+  a: 0.0051699235641169128
   relative_time: 7.89999999999998
 }
 trajectory_point {
   path_point {
-    x: 587467.83039630554
-    y: 4140764.2567540351
-    theta: 2.2120491909279121
-    kappa: -0.00059835448533619284
-    s: 57.437893346641928
+    x: 587467.46832790785
+    y: 4140764.6555554797
+    theta: 2.1667201346178433
+    kappa: -0.003492820371045978
+    s: 57.998705921293151
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1100977171709143
-  a: 0.0022505814622905684
+  v: 8.2359786369615762
+  a: 0.0025584527145858793
   relative_time: 7.94999999999998
 }
 trajectory_point {
   path_point {
-    x: 587467.58784423932
-    y: 4140764.5817220705
-    theta: 2.2118121416941623
-    kappa: -0.00054302132535462776
-    s: 57.843400102457338
+    x: 587467.23734034121
+    y: 4140764.9964731471
+    theta: 2.1653236849229134
+    kappa: -0.0031037021473896997
+    s: 58.410506979662436
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1101537605999372
-  a: 1.609823385706477e-15
+  v: 8.2360423773705449
+  a: 7.4940054162198066e-16
   relative_time: 7.99999999999998
 }
 decision {
diff --git a/modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt b/modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt
index 61a8ccf24aabd012cf29c21479de6b9b76649073..bc36927fa832266892335f5509deb2faf4bc99d0 100755
--- a/modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt
+++ b/modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt
@@ -10,7 +10,7 @@ trajectory_point {
     y: 4435948.39609
     z: 0
     theta: 1.9388221429050949
-    kappa: -2.5407432568958546e-20
+    kappa: -4.9315123824719705e-20
     dkappa: 0
     ddkappa: 0
   }
@@ -20,2242 +20,2242 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 435707.28317849169
+    x: 435707.28317848977
     y: 4435948.3961094664
-    theta: 1.9388221499817329
-    kappa: 1.9387628109317588e-08
-    s: 2.0865088313888582e-05
+    theta: 1.9388221586513783
+    kappa: 4.3205989752468247e-08
+    s: 2.0865088313888636e-05
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0012499035857454837
-  a: 0.049836006382690382
+  v: 0.001249903585745487
+  a: 0.049836006382690513
   relative_time: 0.05
 }
 trajectory_point {
   path_point {
-    x: 435707.28312622156
-    y: 4435948.3962449916
-    theta: 1.9388221992466361
-    kappa: 1.5435703628742243e-07
-    s: 0.00016612002127561158
+    x: 435707.2831262066
+    y: 4435948.396244986
+    theta: 1.9388222682711078
+    kappa: 3.4398991410659465e-07
+    s: 0.00016612002127561204
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0049675869335560289
-  a: 0.098711190482601546
+  v: 0.0049675869335560419
+  a: 0.0987111904826018
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 435707.28298522026
-    y: 4435948.3966105757
-    theta: 1.9388223321412759
-    kappa: 5.1844403598755894e-07
-    s: 0.00055795275913492627
+    x: 435707.28298516996
+    y: 4435948.3966105562
+    theta: 1.9388225639760308
+    kappa: 1.15537019690087e-06
+    s: 0.00055795275913492778
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.011105008929292651
-  a: 0.1466255522997334
+  v: 0.011105008929292681
+  a: 0.14662555229973379
   relative_time: 0.15000000000000002
 }
 trajectory_point {
   path_point {
-    x: 435707.28271238261
-    y: 4435948.39731798
-    theta: 1.9388225892924373
-    kappa: 1.2229524729007028e-06
-    s: 0.0013161492064346619
+    x: 435707.28271226404
+    y: 4435948.3973179338
+    theta: 1.9388231361651318
+    kappa: 2.7253912502324407e-06
+    s: 0.0013161492064346653
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.019614128458816379
-  a: 0.19357909183408592
+  v: 0.019614128458816431
+  a: 0.19357909183408642
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 435707.28226546792
-    y: 4435948.3984767254
-    theta: 1.9388230105122213
-    kappa: 2.3769542269555167e-06
-    s: 0.0025580932120107
+    x: 435707.28226523747
+    y: 4435948.3984766379
+    theta: 1.9388240734246336
+    kappa: 5.2971234744570182e-06
+    s: 0.0025580932120107067
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.030446904407988251
-  a: 0.23957180908565914
+  v: 0.030446904407988331
+  a: 0.23957180908565978
   relative_time: 0.25
 }
 trajectory_point {
   path_point {
-    x: 435707.2816031
-    y: 4435948.4001940927
-    theta: 1.9388236347980425
-    kappa: 4.0872892123183515e-06
-    s: 0.0043987665689919735
+    x: 435707.28160270373
+    y: 4435948.400193939
+    theta: 1.9388254625279964
+    kappa: 9.1086632581888732e-06
+    s: 0.0043987665689919839
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0435552956626693
-  a: 0.284603704054453
+  v: 0.043555295662669413
+  a: 0.28460370405445379
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 435707.28068476694
-    y: 4435948.4025751175
-    theta: 1.9388245003326317
-    kappa: 6.4585653773932405e-06
-    s: 0.0069507490148004655
+    x: 435707.28068414069
+    y: 4435948.4025748754
+    theta: 1.9388273884359186
+    kappa: 1.4393132978300852e-05
+    s: 0.0069507490148004855
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.058891261108720554
-  a: 0.32867477674046763
+  v: 0.058891261108720713
+  a: 0.32867477674046852
   relative_time: 0.35
 }
 trajectory_point {
   path_point {
-    x: 435707.27947082109
-    y: 4435948.4057225967
-    theta: 1.9388256444840333
-    kappa: 9.5931587048219125e-06
-    s: 0.010324218231151218
+    x: 435707.27946989087
+    y: 4435948.4057222381
+    theta: 1.9388299342963367
+    kappa: 2.137868099992435e-05
+    s: 0.010324218231151244
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.07640675963200308
-  a: 0.371785027143703
+  v: 0.076406759632003274
+  a: 0.37178502714370387
   relative_time: 0.39999999999999997
 }
 trajectory_point {
   path_point {
-    x: 435707.27792247938
-    y: 4435948.4097370878
-    theta: 1.9388271038056075
-    kappa: 1.3591213211483771e-05
-    s: 0.014626949844052309
+    x: 435707.27792116156
+    y: 4435948.4097365793
+    theta: 1.9388331814444255
+    kappa: 3.0288481676449332e-05
+    s: 0.014626949844052349
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.096053750118377856
-  a: 0.41393445526415895
+  v: 0.0960537501183781
+  a: 0.41393445526416006
   relative_time: 0.44999999999999996
 }
 trajectory_point {
   path_point {
-    x: 435707.27600182296
-    y: 4435948.4147169041
-    theta: 1.9388289140360291
-    kappa: 1.8550640948495918e-05
-    s: 0.019964317423804886
+    x: 435707.27600002429
+    y: 4435948.41471621
+    theta: 1.9388372094025992
+    kappa: 4.1340735349524328e-05
+    s: 0.019964317423804938
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.11778419145370597
-  a: 0.45512306110183565
+  v: 0.11778419145370628
+  a: 0.45512306110183687
   relative_time: 0.49999999999999994
 }
 trajectory_point {
   path_point {
-    x: 435707.27367179748
-    y: 4435948.42075812
-    theta: 1.9388311100992879
-    kappa: 2.4567122001213152e-05
-    s: 0.026439292485003148
+    x: 435707.2736694155
+    y: 4435948.4207572006
+    theta: 1.9388420958805095
+    kappa: 5.4748668349056449e-05
+    s: 0.026439292485003214
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.1415500425238484
-  a: 0.49535084465673296
+  v: 0.14155004252384881
+  a: 0.49535084465673429
   relative_time: 0.54999999999999993
 }
 trajectory_point {
   path_point {
-    x: 435707.270896213
-    y: 4435948.4279545667
-    theta: 1.9388337261046884
-    kappa: 3.173410448922791e-05
-    s: 0.034152444486534324
+    x: 435707.2708931361
+    y: 4435948.4279533792
+    theta: 1.9388479167750483
+    kappa: 7.0720532993211361e-05
+    s: 0.034152444486534421
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.16730326221466627
-  a: 0.534617805928851
+  v: 0.16730326221466674
+  a: 0.53461780592885255
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 435707.26763974392
-    y: 4435948.4363978356
-    theta: 1.9388367953468502
-    kappa: 4.01428045663704e-05
-    s: 0.043201940831578738
+    x: 435707.26763585163
+    y: 4435948.4363963343
+    theta: 1.9388547461703465
+    kappa: 8.9459607588413255e-05
+    s: 0.043201940831578842
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.19499580941202058
-  a: 0.57292394491818988
+  v: 0.19499580941202108
+  a: 0.57292394491819143
   relative_time: 0.65
 }
 trajectory_point {
   path_point {
-    x: 435707.26386792876
-    y: 4435948.4461772768
-    theta: 1.9388403503057083
-    kappa: 4.9882206420708435e-05
-    s: 0.0536835468676097
+    x: 435707.26386309229
+    y: 4435948.4461754113
+    theta: 1.9388626563377738
+    kappa: 0.00011116419642934494
+    s: 0.053683546867609837
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.22457964300177233
-  a: 0.61026926162474926
+  v: 0.22457964300177294
+  a: 0.610269261624751
   relative_time: 0.70000000000000007
 }
 trajectory_point {
   path_point {
-    x: 435707.25954717089
-    y: 4435948.45738
-    theta: 1.9388444226465122
-    kappa: 6.1039062274547535e-05
-    s: 0.065690625886393628
+    x: 435707.25954125263
+    y: 4435948.4573777169
+    theta: 1.9388717177359396
+    kappa: 0.00013602762979894777
+    s: 0.065690625886393822
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.25600672186978263
-  a: 0.64665375604852948
+  v: 0.25600672186978329
+  a: 0.64665375604853126
   relative_time: 0.75000000000000011
 }
 trajectory_point {
   path_point {
-    x: 435707.25464473775
-    y: 4435948.4700908717
-    theta: 1.9388490432198264
-    kappa: 7.3697892384430954e-05
-    s: 0.079314139123989991
+    x: 435707.25463759212
+    y: 4435948.470088115
+    theta: 1.9388819990106925
+    kappa: 0.00016423826396842162
+    s: 0.079314139123990185
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.28922900490191233
-  a: 0.68207742818953043
+  v: 0.28922900490191317
+  a: 0.68207742818953221
   relative_time: 0.80000000000000016
 }
 trajectory_point {
   path_point {
-    x: 435707.24912876124
-    y: 4435948.4843925191
-    theta: 1.9388542420615305
-    kappa: 8.7940985041139587e-05
-    s: 0.094642645760751265
+    x: 435707.24912023457
+    y: 4435948.48438923
+    theta: 1.93889356699512
+    kappa: 0.000195979481197225
+    s: 0.094642645760751487
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.32419845098402272
-  a: 0.71654027804775189
+  v: 0.32419845098402356
+  a: 0.71654027804775389
   relative_time: 0.8500000000000002
 }
 trajectory_point {
   path_point {
-    x: 435707.24296823767
-    y: 4435948.500365328
-    theta: 1.9388600483928187
-    kappa: 0.00010384839656969195
-    s: 0.11176230292132294
+    x: 435707.24295816862
+    y: 4435948.5003614435
+    theta: 1.9389064867095487
+    kappa: 0.00023142968973307497
+    s: 0.11176230292132328
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.36086701900197471
-  a: 0.75004230562319418
+  v: 0.3608670190019756
+  a: 0.75004230562319629
   relative_time: 0.90000000000000024
 }
 trajectory_point {
   path_point {
-    x: 435707.23613302765
-    y: 4435948.5180874411
-    theta: 1.9388664906202009
-    kappa: 0.00012149795132934449
-    s: 0.13075686567464379
+    x: 435707.23612124741
+    y: 4435948.5180828972
+    theta: 1.9389208213615434
+    kappa: 0.00027076232381194711
+    s: 0.13075686567464415
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.39918666784162926
-  a: 0.78258351091585709
+  v: 0.39918666784163032
+  a: 0.7825835109158592
   relative_time: 0.95000000000000029
 }
 trajectory_point {
   path_point {
-    x: 435707.22859385627
-    y: 4435948.5376347639
-    theta: 1.9388735963355008
-    kappa: 0.00014096524171359099
-    s: 0.15170768703394527
+    x: 435707.22858018847
+    y: 4435948.5376294916
+    theta: 1.9389366323459063
+    kappa: 0.00031414584365807542
+    s: 0.15170768703394566
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.4391093563888474
-  a: 0.81416389392574073
+  v: 0.43910935638884852
+  a: 0.81416389392574284
   relative_time: 1.0000000000000002
 }
 trajectory_point {
   path_point {
-    x: 435707.22032231296
-    y: 4435948.5590809565
-    theta: 1.9388813923158579
-    kappa: 0.0001623236281501632
-    s: 0.17469371795675223
+    x: 435707.22030657425
+    y: 4435948.5590748852
+    theta: 1.9389539792446764
+    kappa: 0.00036174373548395297
+    s: 0.17469371795675273
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.48058704352949033
-  a: 0.8447834546528451
+  v: 0.48058704352949161
+  a: 0.84478345465284721
   relative_time: 1.0500000000000003
 }
 trajectory_point {
   path_point {
-    x: 435707.21129085153
-    y: 4435948.58249744
-    theta: 1.9388899045237251
-    kappa: 0.00018564423910103048
-    s: 0.19979150734488241
+    x: 435707.21127285174
+    y: 4435948.5824904982
+    theta: 1.9389729198271282
+    kappa: 0.00041371451149033081
+    s: 0.19979150734488293
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.523571688149419
-  a: 0.87444219309717
+  v: 0.52357168814942034
+  a: 0.87444219309717242
   relative_time: 1.1000000000000003
 }
 trajectory_point {
   path_point {
-    x: 435707.20147279021
-    y: 4435948.6079533957
-    theta: 1.9388991581068709
-    kappa: 0.00021099597106239985
-    s: 0.22707520204444662
+    x: 435707.2014523322
+    y: 4435948.6079455046
+    theta: 1.9389935100497704
+    kappa: 0.00047021170986621908
+    s: 0.22707520204444728
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.5680152491344943
-  a: 0.90314010925871568
+  v: 0.568015249134496
+  a: 0.90314010925871813
   relative_time: 1.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 435707.19084231136
-    y: 4435948.6355157606
-    theta: 1.9389091773983775
-    kappa: 0.000238445488564716
-    s: 0.25661654684584873
+    x: 435707.190819192
+    y: 4435948.6355068432
+    theta: 1.9390158040563441
+    kappa: 0.00053138389478888607
+    s: 0.2566165468458495
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.61386968537057751
-  a: 0.93087720313748212
+  v: 0.61386968537057918
+  a: 0.93087720313748457
   relative_time: 1.2000000000000004
 }
 trajectory_point {
   path_point {
-    x: 435707.17937446223
-    y: 4435948.6652492341
-    theta: 1.9389199859166415
-    kappa: 0.00026805722417266143
-    s: 0.28848488448378568
+    x: 435707.17934847175
+    y: 4435948.6652392084
+    theta: 1.9390398541778209
+    kappa: 0.00059737465642385832
+    s: 0.2884848844837864
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.66108695574352971
-  a: 0.95765347473346918
+  v: 0.66108695574353149
+  a: 0.95765347473347173
   relative_time: 1.2500000000000004
 }
 trajectory_point {
   path_point {
-    x: 435707.167045154
-    y: 4435948.6972162705
-    theta: 1.9389316063653736
-    kappa: 0.000299893378485156
-    s: 0.32274715563724732
+    x: 435707.16701607674
+    y: 4435948.6972050536
+    theta: 1.9390657109324003
+    kappa: 0.000668322610924922
+    s: 0.32274715563724815
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.70961901913921155
-  a: 0.983468924046677
+  v: 0.70961901913921344
+  a: 0.98346892404667974
   relative_time: 1.3000000000000005
 }
 trajectory_point {
   path_point {
-    x: 435707.15383116249
-    y: 4435948.7314770836
-    theta: 1.9389440606335981
-    kappa: 0.00033401392013535764
-    s: 0.35946789892951675
+    x: 435707.15379877691
+    y: 4435948.7314645918
+    theta: 1.9390934230255066
+    kappa: 0.00074436140043412111
+    s: 0.35946789892951775
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.75941783444348454
-  a: 1.0083235510771056
+  v: 0.75941783444348654
+  a: 1.0083235510771082
   relative_time: 1.3500000000000005
 }
 trajectory_point {
   path_point {
-    x: 435707.13971012773
-    y: 4435948.7680896511
-    theta: 1.9389573697956524
-    kappa: 0.00037047658579066186
-    s: 0.39870925092817006
+    x: 435707.13967420673
+    y: 4435948.7680757949
+    theta: 1.9391230373497865
+    kappa: 0.00082561969308175809
+    s: 0.39870925092817117
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.81043536054220933
-  a: 1.0322173558247547
+  v: 0.81043536054221144
+  a: 1.0322173558247574
   relative_time: 1.4000000000000006
 }
 trajectory_point {
   path_point {
-    x: 435707.12466055411
-    y: 4435948.8071097042
-    theta: 1.9389715541111872
-    kappa: 0.0004093368801527017
-    s: 0.44053094614507637
+    x: 435707.12462086533
+    y: 4435948.8070943952
+    theta: 1.9391545989851047
+    kappa: 0.00091222118298639433
+    s: 0.44053094614507748
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.862623556321247
-  a: 1.0551503382896246
+  v: 0.86262355632124921
+  a: 1.0551503382896275
   relative_time: 1.4500000000000006
 }
 trajectory_point {
   path_point {
-    x: 435707.10866181063
-    y: 4435948.8485907344
-    theta: 1.9389866330251659
-    kappa: 0.00045064807595734794
-    s: 0.48499031703639772
+    x: 435707.10861811636
+    y: 4435948.84857388
+    theta: 1.9391881511985398
+    kappa: 0.0010042845902548494
+    s: 0.484990317036399
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.91593438066645871
-  a: 1.0771224984717152
+  v: 0.91593438066646127
+  a: 1.077122498471718
   relative_time: 1.5000000000000007
 }
 trajectory_point {
   path_point {
-    x: 435707.09169413056
-    y: 4435948.8925839923
-    theta: 1.9390026251678645
-    kappa: 0.00049446121397470955
-    s: 0.53214229400258972
+    x: 435707.09164618817
+    y: 4435948.8925655
+    theta: 1.9392237354443811
+    kappa: 0.0011019236609822022
+    s: 0.53214229400259094
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.97031979246370559
-  a: 1.0981338363710262
+  v: 0.97031979246370825
+  a: 1.0981338363710293
   relative_time: 1.5500000000000007
 }
 trajectory_point {
   path_point {
-    x: 435707.07373861142
-    y: 4435948.9391384888
-    theta: 1.93901954835487
-    kappa: 0.00054082510300913181
-    s: 0.58203940538839993
+    x: 435707.07368617365
+    y: 4435948.9391182605
+    theta: 1.939261391364121
+    kappa: 0.0012052471672517895
+    s: 0.5820394053884016
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0257317505988486
-  a: 1.1181843519875581
+  v: 1.0257317505988512
+  a: 1.1181843519875612
   relative_time: 1.6000000000000008
 }
 trajectory_point {
   path_point {
-    x: 435707.05477721529
-    y: 4435948.9883009894
-    theta: 1.9390374195870814
-    kappa: 0.0005897863198991996
-    s: 0.63473177748287046
+    x: 435707.05472003028
+    y: 4435948.988278931
+    theta: 1.9393011567864513
+    kappa: 0.0013143589071352064
+    s: 0.63473177748287224
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0821222139577487
-  a: 1.1372740453213108
+  v: 1.0821222139577515
+  a: 1.1372740453213139
   relative_time: 1.6500000000000008
 }
 trajectory_point {
   path_point {
-    x: 435707.03479276854
-    y: 4435949.0401160242
-    theta: 1.9390562550507082
-    kappa: 0.00064138920951773413
-    s: 0.69026713451933541
+    x: 435707.03473058017
+    y: 4435949.0400920352
+    theta: 1.9393430677272565
+    kappa: 0.0014293577046923071
+    s: 0.69026713451933719
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1394431414262667
-  a: 1.1554029163722839
+  v: 1.1394431414262696
+  a: 1.1554029163722872
   relative_time: 1.7000000000000008
 }
 trajectory_point {
   path_point {
-    x: 435707.01374798222
-    y: 4435949.09461778
-    theta: 1.9391046846866631
-    kappa: 0.00068307605460480062
-    s: 0.74869079867542188
+    x: 435707.01365418767
+    y: 4435949.0945815742
+    theta: 1.9394517388997381
+    kappa: 0.0015228724422623565
+    s: 0.748690798675424
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1976464918902641
-  a: 1.1725709651404781
+  v: 1.1976464918902674
+  a: 1.1725709651404812
   relative_time: 1.7500000000000009
 }
 trajectory_point {
   path_point {
-    x: 435706.99164414534
-    y: 4435949.1518527614
-    theta: 1.939159899473766
-    kappa: 0.00072493662442168181
-    s: 0.81004569007305083
+    x: 435706.9915136419
+    y: 4435949.1518023657
+    theta: 1.9395750197724215
+    kappa: 0.0016171844924009239
+    s: 0.810045690073053
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2566842242356016
-  a: 1.1887781916258928
+  v: 1.2566842242356049
+  a: 1.1887781916258959
   relative_time: 1.8000000000000009
 }
 trajectory_point {
   path_point {
-    x: 435706.96846970147
-    y: 4435949.2118599387
-    theta: 1.9392177886094963
-    kappa: 0.0007688247253628068
-    s: 0.87437232677843546
+    x: 435706.96830071107
+    y: 4435949.2117946651
+    theta: 1.939704271812055
+    kappa: 0.00171606457918791
+    s: 0.8743723267784379
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3165082973481403
-  a: 1.2040245958285281
+  v: 1.3165082973481441
+  a: 1.2040245958285312
   relative_time: 1.850000000000001
 }
 trajectory_point {
   path_point {
-    x: 435706.94421091897
-    y: 4435949.2746748682
-    theta: 1.9392783863951804
-    kappa: 0.00081476636281882378
-    s: 0.94170882480208262
+    x: 435706.94400164078
+    y: 4435949.274594022
+    theta: 1.9398395716030237
+    kappa: 0.0018195712928842068
+    s: 0.94170882480208529
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3770706701137416
-  a: 1.218310177748384
+  v: 1.3770706701137452
+  a: 1.2183101777483873
   relative_time: 1.900000000000001
 }
 trajectory_point {
   path_point {
-    x: 435706.91885493131
-    y: 4435949.3403308671
-    theta: 1.9393417249703684
-    kappa: 0.00086278590333108347
-    s: 1.0120908980987917
+    x: 435706.91860354313
+    y: 4435949.3402337413
+    theta: 1.9399809909020727
+    kappa: 0.0019277595314159445
+    s: 1.0120908980987946
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4383233014182657
-  a: 1.2316349373854609
+  v: 1.43832330141827
+  a: 1.2316349373854643
   relative_time: 1.9500000000000011
 }
 trajectory_point {
   path_point {
-    x: 435706.89238973759
-    y: 4435949.4088590089
-    theta: 1.9394078343128216
-    kappa: 0.00091290607459164027
-    s: 1.0855518585676558
+    x: 435706.89209439734
+    y: 4435949.408744893
+    theta: 1.9401285966381325
+    kappa: 0.0020406805003744932
+    s: 1.0855518585676585
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5002181501475755
-  a: 1.243998874739759
+  v: 1.5002181501475786
+  a: 1.2439988747397617
   relative_time: 2.0000000000000009
 }
 trajectory_point {
   path_point {
-    x: 435706.86480416136
-    y: 4435949.4802882345
-    theta: 1.9394767423402135
-    kappa: 0.0009651480425609915
-    s: 1.1621227290832634
+    x: 435706.86446300842
+    y: 4435949.4801564086
+    theta: 1.9402824511392454
+    kappa: 0.002158381886763074
+    s: 1.1621227290832663
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5627148236480903
-  a: 1.255777255301272
+  v: 1.5627148236480937
+  a: 1.2557772553012747
   relative_time: 2.0500000000000007
 }
 trajectory_point {
   path_point {
-    x: 435706.836087604
-    y: 4435949.5546459872
-    theta: 1.9395484755272157
-    kappa: 0.0010195318793200119
-    s: 1.24183292922465
+    x: 435706.83569876
+    y: 4435949.5544957258
+    theta: 1.9404426135102075
+    kappa: 0.0022809089130692473
+    s: 1.2418329292246528
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6257867946797449
-  a: 1.2670107759853444
+  v: 1.6257867946797484
+  a: 1.2670107759853471
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 435706.80622994783
-    y: 4435949.6319584642
-    theta: 1.9396230591462273
-    kappa: 0.0010760767455894758
-    s: 1.3247105427935253
+    x: 435706.80579151772
+    y: 4435949.6317890352
+    theta: 1.9406091401698868
+    kappa: 0.0024083047484822111
+    s: 1.3247105427935284
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6894068202486672
-  a: 1.2776994367919758
+  v: 1.6894068202486705
+  a: 1.2776994367919787
   relative_time: 2.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 435706.77521040855
-    y: 4435949.71224628
-    theta: 1.9397066515006658
-    kappa: 0.0011313487246476378
-    s: 1.4107822914419059
+    x: 435706.77470523852
+    y: 4435949.7120510172
+    theta: 1.9407967771862578
+    kappa: 0.0025327185679561953
+    s: 1.4107822914419093
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7535476573609847
-  a: 1.2878432377211668
+  v: 1.7535476573609885
+  a: 1.2878432377211695
   relative_time: 2.2
 }
 trajectory_point {
   path_point {
-    x: 435706.74297771108
-    y: 4435949.7955168
-    theta: 1.9398224427396817
-    kappa: 0.001172327350697124
-    s: 1.500073534672115
+    x: 435706.74233921221
+    y: 4435949.7952698041
+    theta: 1.9410565304074707
+    kappa: 0.0026268193808140069
+    s: 1.5000735346721186
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8181820630228258
-  a: 1.2974421787729167
+  v: 1.8181820630228298
+  a: 1.2974421787729196
   relative_time: 2.25
 }
 trajectory_point {
   path_point {
-    x: 435706.7095741654
-    y: 4435949.8818121087
-    theta: 1.9399424400881922
-    kappa: 0.0012147945196122752
-    s: 1.5926082698367821
+    x: 435706.70879749436
+    y: 4435949.881511501
+    theta: 1.9413257192283588
+    kappa: 0.0027243383925510006
+    s: 1.5926082698367858
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8832827942403187
-  a: 1.3064962599472261
+  v: 1.8832827942403225
+  a: 1.3064962599472287
   relative_time: 2.3
 }
 trajectory_point {
   path_point {
-    x: 435706.67499160056
-    y: 4435949.971153317
-    theta: 1.9400666728964304
-    kappa: 0.0012587606194164077
-    s: 1.6884091321388426
+    x: 435706.67407188041
+    y: 4435949.9707972053
+    theta: 1.9416044094656764
+    kappa: 0.0028252994575891591
+    s: 1.6884091321388466
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9488226080195907
-  a: 1.3150054812440943
+  v: 1.9488226080195947
+  a: 1.3150054812440972
   relative_time: 2.3499999999999996
 }
 trajectory_point {
   path_point {
-    x: 435706.63922233728
-    y: 4435950.0635602623
-    theta: 1.9401951687474863
-    kappa: 0.0013042354129984934
-    s: 1.7874973946315391
+    x: 435706.63815465942
+    y: 4435950.0631467421
+    theta: 1.9418926629653424
+    kappa: 0.0029297249948301543
+    s: 1.7874973946315431
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.01477426136677
-  a: 1.3229698426635221
+  v: 2.0147742613667745
+  a: 1.3229698426635248
   relative_time: 2.3999999999999995
 }
 trajectory_point {
   path_point {
-    x: 435706.60225918808
-    y: 4435950.1590515133
-    theta: 1.9403279534572315
-    kappa: 0.0013512280381131564
-    s: 1.8898929682184196
+    x: 435706.601038614
+    y: 4435950.15857867
+    theta: 1.9421905376015847
+    kappa: 0.0030376359876553437
+    s: 1.8898929682184238
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0811105112879851
-  a: 1.330389344205509
+  v: 2.0811105112879895
+  a: 1.3303893442055119
   relative_time: 2.4499999999999993
 }
 trajectory_point {
   path_point {
-    x: 435706.5640954572
-    y: 4435950.2576443683
-    theta: 1.9404650510742409
-    kappa: 0.0013997470073806772
-    s: 1.9956144016533393
+    x: 435706.56271702074
+    y: 4435950.2571102735
+    theta: 1.9424980872760491
+    kappa: 0.0031490519839257733
+    s: 1.9956144016533441
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1478041147893636
-  a: 1.3372639858700552
+  v: 2.1478041147893681
+  a: 1.3372639858700581
   relative_time: 2.4999999999999991
 }
 trajectory_point {
   path_point {
-    x: 435706.52471401554
-    y: 4435950.3593506208
-    theta: 1.9406093924415939
-    kappa: 0.0014474621134663887
-    s: 2.10467888154046
+    x: 435706.52315647568
+    y: 4435950.3587469673
+    theta: 1.94282290648187
+    kappa: 0.00325837537247793
+    s: 2.1046788815404649
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2148278288770329
-  a: 1.3435937676571605
+  v: 2.2148278288770373
+  a: 1.3435937676571634
   relative_time: 2.5499999999999989
 }
 trajectory_point {
   path_point {
-    x: 435706.48402595706
-    y: 4435950.4641527757
-    theta: 1.9407831469015695
-    kappa: 0.0014765759445370267
-    s: 2.2171022323342489
+    x: 435706.482170033
+    y: 4435950.46343279
+    theta: 1.9432153522002493
+    kappa: 0.0033281745436031505
+    s: 2.2171022323342537
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2821544105571214
-  a: 1.3493786895668252
+  v: 2.2821544105571263
+  a: 1.349378689566828
   relative_time: 2.5999999999999988
 }
 trajectory_point {
   path_point {
-    x: 435706.44211702788
-    y: 4435950.5720995851
-    theta: 1.9409621149950012
-    kappa: 0.0015065633543844001
-    s: 2.3328989163394804
+    x: 435706.43995376641
+    y: 4435950.5712597752
+    theta: 1.9436195737484545
+    kappa: 0.0034000680826726231
+    s: 2.3328989163394858
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3497566168357578
-  a: 1.3546187515990491
+  v: 2.3497566168357622
+  a: 1.354618751599052
   relative_time: 2.6499999999999986
 }
 trajectory_point {
   path_point {
-    x: 435706.39898248669
-    y: 4435950.6832032613
-    theta: 1.941146316958632
-    kappa: 0.001537427735503923
-    s: 2.452082033711235
+    x: 435706.39650289994
+    y: 4435950.6822401239
+    theta: 1.9440356167405732
+    kappa: 0.0034740641230497438
+    s: 2.4520820337112408
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4176072047190686
-  a: 1.3593139537538321
+  v: 2.417607204719074
+  a: 1.359313953753835
   relative_time: 2.6999999999999984
 }
 trajectory_point {
   path_point {
-    x: 435706.35461808543
-    y: 4435950.7974747457
-    theta: 1.9413357709219141
-    kappa: 0.0015691721276404839
-    s: 2.5746633224549003
+    x: 435706.35181315418
+    y: 4435950.7963847648
+    theta: 1.9444635220122137
+    kappa: 0.0035501699523935335
+    s: 2.5746633224549056
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4856789312131831
-  a: 1.3634642960311745
+  v: 2.4856789312131884
+  a: 1.3634642960311774
   relative_time: 2.7499999999999982
 }
 trajectory_point {
   path_point {
-    x: 435706.30902006885
-    y: 4435950.9149237126
-    theta: 1.9415304929068413
-    kappa: 0.0016017992177884458
-    s: 2.7006531584261682
+    x: 435706.3058807464
+    y: 4435950.91370336
+    theta: 1.9449033256186155
+    kappa: 0.0036283920126586409
+    s: 2.7006531584261744
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5539445533242287
-  a: 1.3670697784310759
+  v: 2.5539445533242335
+  a: 1.3670697784310788
   relative_time: 2.799999999999998
 }
 trajectory_point {
   path_point {
-    x: 435706.2621438285
-    y: 4435951.0355424834
-    theta: 1.9417351466320472
-    kappa: 0.0016283677945435359
-    s: 2.8300605553310394
+    x: 435706.25860480615
+    y: 4435951.0341659514
+    theta: 1.9453672325292721
+    kappa: 0.0036930160472092612
+    s: 2.8300605553310456
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6223768280583326
-  a: 1.3701304009535367
+  v: 2.6223768280583384
+  a: 1.3701304009535396
   relative_time: 2.8499999999999979
 }
 trajectory_point {
   path_point {
-    x: 435706.21392994368
-    y: 4435951.1593161616
-    theta: 1.9419561150089473
-    kappa: 0.0016393659748762995
-    s: 2.96289316472582
+    x: 435706.20986208
+    y: 4435951.1577323256
+    theta: 1.9458702979259006
+    kappa: 0.0037246412885326079
+    s: 2.9628931647258265
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6909485124216239
-  a: 1.3726461635985567
+  v: 2.6909485124216297
+  a: 1.3726461635985596
   relative_time: 2.8999999999999977
 }
 trajectory_point {
   path_point {
-    x: 435706.16447053617
-    y: 4435951.28628732
-    theta: 1.9421827917532541
-    kappa: 0.0016506482742732155
-    s: 3.0991572760171229
+    x: 435706.15986016934
+    y: 4435951.2844908237
+    theta: 1.9463863594972224
+    kappa: 0.0037570835135474845
+    s: 3.0991572760171295
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7596323634202307
-  a: 1.3746170663661359
+  v: 2.7596323634202364
+  a: 1.3746170663661388
   relative_time: 2.9499999999999975
 }
 trajectory_point {
   path_point {
-    x: 435706.11376381735
-    y: 4435951.4164605495
-    theta: 1.9424151850395948
-    kappa: 0.0016622151006978132
-    s: 3.2388578164618664
+    x: 435706.1085972662
+    y: 4435951.4144460307
+    theta: 1.9469154356449354
+    kappa: 0.0037903438953520484
+    s: 3.2388578164618735
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.82840113806028
-  a: 1.3760431092562744
+  v: 2.8284011380602858
+  a: 1.3760431092562773
   relative_time: 2.9999999999999973
 }
 trajectory_point {
   path_point {
-    x: 435706.06180849345
-    y: 4435951.5498391725
-    theta: 1.9426533007730447
-    kappa: 0.0016740667493313163
-    s: 3.381998351167276
+    x: 435706.05607206229
+    y: 4435951.5476012621
+    theta: 1.9474575395694862
+    kappa: 0.003824423282739204
+    s: 3.3819983511672835
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8972275933479
-  a: 1.3769242922689722
+  v: 2.8972275933479064
+  a: 1.3769242922689748
   relative_time: 3.0499999999999972
 }
 trajectory_point {
   path_point {
-    x: 435706.00855997886
-    y: 4435951.6864081556
-    theta: 1.9428972819876955
-    kappa: 0.0016800586278496721
-    s: 3.5285810830908835
+    x: 435706.00217883
+    y: 4435951.6839170186
+    theta: 1.948014651080654
+    kappa: 0.0038452713203259003
+    s: 3.528581083090891
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9660844862892191
-  a: 1.3772606154042291
+  v: 2.9660844862892253
+  a: 1.3772606154042319
   relative_time: 3.099999999999997
 }
 trajectory_point {
   path_point {
-    x: 435705.95394243964
-    y: 4435951.82613881
-    theta: 1.9431473705978788
-    kappa: 0.0016695913244277217
-    s: 3.6786068530405269
+    x: 435705.94675476442
+    y: 4435951.8233297318
+    theta: 1.948589827965068
+    kappa: 0.003831126605836354
+    s: 3.6786068530405354
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.0349445738903649
-  a: 1.3770520786620453
+  v: 3.0349445738903715
+  a: 1.3770520786620479
   relative_time: 3.1499999999999968
 }
 trajectory_point {
   path_point {
-    x: 435705.89807163726
-    y: 4435951.96907575
-    theta: 1.9434031978175521
-    kappa: 0.0016588838364916007
-    s: 3.8320751396743513
+    x: 435705.890058929
+    y: 4435951.9659414329
+    theta: 1.9491782033629397
+    kappa: 0.0038166573243364893
+    s: 3.8320751396743598
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1037806131574657
-  a: 1.3762986820424206
+  v: 3.1037806131574723
+  a: 1.3762986820424232
   relative_time: 3.1999999999999966
 }
 trajectory_point {
   path_point {
-    x: 435705.84094825713
-    y: 4435952.1152172172
-    theta: 1.9436647604746786
-    kappa: 0.0016479362954525828
-    s: 3.9889840595008077
+    x: 435705.83209201932
+    y: 4435952.1117503708
+    theta: 1.9497797696597783
+    kappa: 0.003801863653405466
+    s: 3.9889840595008157
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1725653610966491
-  a: 1.3750004255453552
+  v: 3.1725653610966562
+  a: 1.3750004255453581
   relative_time: 3.2499999999999964
 }
 trajectory_point {
   path_point {
-    x: 435705.78257348109
-    y: 4435952.264560191
-    theta: 1.9439320531219522
-    kappa: 0.0016367489277591754
-    s: 4.1493303668786528
+    x: 435705.77285523457
+    y: 4435952.260753531
+    theta: 1.9503945139627812
+    kappa: 0.0037867458990485047
+    s: 4.1493303668786616
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2412715747140437
-  a: 1.3731573091708489
+  v: 3.2412715747140504
+  a: 1.3731573091708518
   relative_time: 3.2999999999999963
 }
 trajectory_point {
   path_point {
-    x: 435705.7228144625
-    y: 4435952.4170477083
-    theta: 1.9441929173964296
-    kappa: 0.0016089035612596324
-    s: 4.3131094540169492
+    x: 435705.71203424328
+    y: 4435952.4128205087
+    theta: 1.9510008598067039
+    kappa: 0.00373477162102886
+    s: 4.313109454016959
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3098720110157762
-  a: 1.3707693329189021
+  v: 3.3098720110157833
+  a: 1.3707693329189048
   relative_time: 3.3499999999999961
 }
 trajectory_point {
   path_point {
-    x: 435705.66176551447
-    y: 4435952.5727102859
-    theta: 1.9444556658016052
-    kappa: 0.001575646148627296
-    s: 4.4803153509750695
+    x: 435705.64985513169
+    y: 4435952.5680351024
+    theta: 1.9516140576732663
+    kappa: 0.0036718215579833462
+    s: 4.4803153509750793
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3783394270079761
-  a: 1.3678364967895145
+  v: 3.3783394270079832
+  a: 1.3678364967895171
   relative_time: 3.3999999999999959
 }
 trajectory_point {
   path_point {
-    x: 435705.5994680727
-    y: 4435952.7315562721
-    theta: 1.944723787607666
-    kappa: 0.0015417085986080259
-    s: 4.6509407256626893
+    x: 435705.58640441368
+    y: 4435952.7264239425
+    theta: 1.9522397960104583
+    kappa: 0.0036075841220486538
+    s: 4.6509407256626991
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.44664657969677
-  a: 1.3643588007826859
+  v: 3.4466465796967771
+  a: 1.3643588007826886
   relative_time: 3.4499999999999957
 }
 trajectory_point {
   path_point {
-    x: 435705.53592531156
-    y: 4435952.893577572
-    theta: 1.9449972691134381
-    kappa: 0.0015070926404944665
-    s: 4.8249768838397911
+    x: 435705.52168532246
+    y: 4435952.88797896
+    theta: 1.9528780424376739
+    kappa: 0.0035420625864523844
+    s: 4.8249768838398017
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5147662260882866
-  a: 1.3603362448984166
+  v: 3.5147662260882937
+  a: 1.3603362448984193
   relative_time: 3.4999999999999956
 }
 trajectory_point {
   path_point {
-    x: 435705.47102607746
-    y: 4435953.0587197235
-    theta: 1.9452550950929171
-    kappa: 0.0014592797057329781
-    s: 5.0024137691166644
+    x: 435705.45542606182
+    y: 4435953.0525800474
+    theta: 1.9534867222096384
+    kappa: 0.0034471129834411857
+    s: 5.002413769116675
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5826711231886534
-  a: 1.3557688291367067
+  v: 3.5826711231886614
+  a: 1.3557688291367094
   relative_time: 3.5499999999999954
 }
 trajectory_point {
   path_point {
-    x: 435705.40482940356
-    y: 4435953.2269936455
-    theta: 1.945507281299981
-    kappa: 0.0014042544580737033
-    s: 5.1832399629539054
+    x: 435705.3877714664
+    y: 4435953.2202731241
+    theta: 1.9540872435134151
+    kappa: 0.0033371016076365733
+    s: 5.1832399629539161
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.650334028004
-  a: 1.3506565534975559
+  v: 3.6503340280040071
+  a: 1.3506565534975585
   relative_time: 3.5999999999999952
 }
 trajectory_point {
   path_point {
-    x: 435705.33739665372
-    y: 4435953.3984097093
-    theta: 1.9457641765335674
-    kappa: 0.0013482017360292066
-    s: 5.3674426846624144
+    x: 435705.31885357155
+    y: 4435953.3910974972
+    theta: 1.954698978331588
+    kappa: 0.0032250360135316226
+    s: 5.3674426846624268
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7177276975404521
-  a: 1.3449994179809643
+  v: 3.7177276975404592
+  a: 1.344999417980967
   relative_time: 3.649999999999995
 }
 trajectory_point {
   path_point {
-    x: 435705.26873300521
-    y: 4435953.5729547534
-    theta: 1.9460257610352485
-    kappa: 0.0012911258432525725
-    s: 5.5550077914034031
+    x: 435705.24867766869
+    y: 4435953.565040051
+    theta: 1.9553218792317184
+    kappa: 0.0031109248053728784
+    s: 5.5550077914034137
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7848248888041383
-  a: 1.3387974225869319
+  v: 3.7848248888041462
+  a: 1.3387974225869346
   relative_time: 3.6999999999999948
 }
 trajectory_point {
   path_point {
-    x: 435705.19870754087
-    y: 4435953.7505605463
-    theta: 1.946252204568677
-    kappa: 0.0012195678815755209
-    s: 5.7459197781883837
+    x: 435705.17691578285
+    y: 4435953.74195133
+    theta: 1.9558736940700479
+    kappa: 0.0029643825110391218
+    s: 5.7459197781883962
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8515983588011871
-  a: 1.332050567315459
+  v: 3.8515983588011951
+  a: 1.3320505673154615
   relative_time: 3.7499999999999947
 }
 trajectory_point {
   path_point {
-    x: 435705.12743847177
-    y: 4435953.9312555222
-    theta: 1.9464761354202709
-    kappa: 0.0011445760925966539
-    s: 5.9401617778791778
+    x: 435705.10385463794
+    y: 4435953.9219291713
+    theta: 1.9564234261632631
+    kappa: 0.0028109548695333863
+    s: 5.94016177787919
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9180208645377261
-  a: 1.324758852166545
+  v: 3.9180208645377341
+  a: 1.3247588521665477
   relative_time: 3.7999999999999945
 }
 trajectory_point {
   path_point {
-    x: 435705.05495428067
-    y: 4435954.1150313076
-    theta: 1.9467038842425919
-    kappa: 0.0010683057100148799
-    s: 6.1377155611879157
+    x: 435705.02954781638
+    y: 4435954.1049755951
+    theta: 1.9569825310437312
+    kappa: 0.00265491132029149
+    s: 6.1377155611879282
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.984065163019884
-  a: 1.3169222771401905
+  v: 3.984065163019892
+  a: 1.3169222771401929
   relative_time: 3.8499999999999943
 }
 trajectory_point {
   path_point {
-    x: 435704.98122304853
-    y: 4435954.3018542379
-    theta: 1.9469180431182225
-    kappa: 0.00098728881141050715
-    s: 6.3385615366770267
+    x: 435704.95389986166
+    y: 4435954.2910306025
+    theta: 1.9575133850961624
+    kappa: 0.0024880162463341272
+    s: 6.3385615366770409
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0497040112537874
-  a: 1.3085408422363951
+  v: 4.0497040112537963
+  a: 1.3085408422363976
   relative_time: 3.8999999999999941
 }
 trajectory_point {
   path_point {
-    x: 435704.90619680774
-    y: 4435954.4916828452
-    theta: 1.9470938458744644
-    kappa: 0.0008965873211528501
-    s: 6.5426787507592561
+    x: 435704.87678935588
+    y: 4435954.4800221547
+    theta: 1.9579686892463588
+    kappa: 0.0022999107650468866
+    s: 6.54267875075927
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1149101662455658
-  a: 1.2996145474551588
+  v: 4.1149101662455738
+  a: 1.2996145474551613
   relative_time: 3.949999999999994
 }
 trajectory_point {
   path_point {
-    x: 435704.82997637824
-    y: 4435954.6845329441
-    theta: 1.9472724468784197
-    kappa: 0.00080444214009179227
-    s: 6.7500448876976495
+    x: 435704.79845148633
+    y: 4435954.6720218761
+    theta: 1.9584312405251227
+    kappa: 0.0021088112188040187
+    s: 6.7500448876976638
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1796563850013451
-  a: 1.2901433927964818
+  v: 4.1796563850013539
+  a: 1.2901433927964843
   relative_time: 3.9999999999999938
 }
 trajectory_point {
   path_point {
-    x: 435704.7525704031
-    y: 4435954.8803826673
-    theta: 1.9474538258660619
-    kappa: 0.00071086371709280023
-    s: 6.9606364330680979
+    x: 435704.71889513603
+    y: 4435954.8670079941
+    theta: 1.9589009862821891
+    kappa: 0.0019147392774685186
+    s: 6.960636433068113
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2439259105647844
-  a: 1.2806010825711804
+  v: 4.2439259105647933
+  a: 1.2806010825711831
   relative_time: 4.0499999999999936
 }
 trajectory_point {
   path_point {
-    x: 435704.67389766593
-    y: 4435955.0791741349
-    theta: 1.9475739752610821
-    kappa: 0.0006090144769144237
-    s: 7.174429435461013
+    x: 435704.63788272289
+    y: 4435955.0648575136
+    theta: 1.9592423194048689
+    kappa: 0.0017012862379585129
+    s: 7.174429435461029
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3077128385414722
-  a: 1.2708394893264419
+  v: 4.3077128385414811
+  a: 1.2708394893264447
   relative_time: 4.0999999999999934
 }
 trajectory_point {
   path_point {
-    x: 435704.59404845349
-    y: 4435955.2809168138
-    theta: 1.9476906745901605
-    kappa: 0.0005050913799013834
-    s: 7.3913994908932814
+    x: 435704.555650576
+    y: 4435955.265640677
+    theta: 1.9595800103748571
+    kappa: 0.0014836048860352139
+    s: 7.3913994908932974
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3710062047804374
-  a: 1.2608586130622661
+  v: 4.3710062047804463
+  a: 1.260858613062269
   relative_time: 4.1499999999999932
 }
 trajectory_point {
   path_point {
-    x: 435704.5130392065
-    y: 4435955.4855903722
-    theta: 1.9478090693045109
-    kappa: 0.00039965850734977363
-    s: 7.6115216471742437
+    x: 435704.47222377569
+    y: 4435955.4693407817
+    theta: 1.959922607231307
+    kappa: 0.0012627610996793135
+    s: 7.6115216471742606
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.433795045130708
-  a: 1.2506584537786529
+  v: 4.4337950451307169
+  a: 1.2506584537786558
   relative_time: 4.1999999999999931
 }
 trajectory_point {
   path_point {
-    x: 435704.430840785
-    y: 4435955.6931558466
-    theta: 1.9478747466472544
-    kappa: 0.00029096441922840107
-    s: 7.83477040390569
+    x: 435704.38748528034
+    y: 4435955.6758822743
+    theta: 1.960153113129171
+    kappa: 0.001033630866880406
+    s: 7.8347704039057069
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.496068395441311
-  a: 1.2402390114756023
+  v: 4.4960683954413208
+  a: 1.2402390114756052
   relative_time: 4.2499999999999929
 }
 trajectory_point {
   path_point {
-    x: 435704.34748550487
-    y: 4435955.9035980161
-    theta: 1.9479197882038624
-    kappa: 0.0001800621532398005
-    s: 8.0611197124818617
+    x: 435704.30152340146
+    y: 4435955.8852732126
+    theta: 1.9603438912265174
+    kappa: 0.00079942823750681459
+    s: 8.06111971248188
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5578152915612771
-  a: 1.2296002861531143
+  v: 4.5578152915612868
+  a: 1.2296002861531172
   relative_time: 4.2999999999999927
 }
 trajectory_point {
   path_point {
-    x: 435704.26299821166
-    y: 4435956.1168981129
-    theta: 1.9479654414509286
-    kappa: 6.7653770087970359e-05
-    s: 8.2905429760894549
+    x: 435704.21439411031
+    y: 4435956.0975078009
+    theta: 1.9605372601985309
+    kappa: 0.0005620450004456451
+    s: 8.2905429760894744
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6190247693396316
-  a: 1.2187422778111889
+  v: 4.6190247693396413
+  a: 1.2187422778111918
   relative_time: 4.3499999999999925
 }
 trajectory_point {
   path_point {
-    x: 435704.1773861869
-    y: 4435956.3330298234
-    theta: 1.9479604314803234
-    kappa: -4.435139157819776e-05
-    s: 8.5230130497076146
+    x: 435704.12605944695
+    y: 4435956.3125411021
+    theta: 1.9606221160825312
+    kappa: 0.00032431887748328507
+    s: 8.5230130497076324
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6796858646254043
-  a: 1.2076649864498261
+  v: 4.679685864625414
+  a: 1.207664986449829
   relative_time: 4.3999999999999924
 }
 trajectory_point {
   path_point {
-    x: 435704.09066082811
-    y: 4435956.551967876
-    theta: 1.9479274024506137
-    kappa: -0.00015677738871175252
-    s: 8.7585022401079353
+    x: 435704.03655297111
+    y: 4435956.5303569641
+    theta: 1.9606514634376551
+    kappa: 8.49372589806112e-05
+    s: 8.7585022401079549
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7397876132676231
-  a: 1.1963684120690261
+  v: 4.7397876132676338
+  a: 1.1963684120690288
   relative_time: 4.4499999999999922
 }
 trajectory_point {
   path_point {
-    x: 435704.00283399742
-    y: 4435956.7736865934
-    theta: 1.9478939539294291
-    kappa: -0.00027063127364995867
-    s: 8.9969823058544662
+    x: 435703.94590970129
+    y: 4435956.7509392379
+    theta: 1.9606811835244471
+    kappa: -0.00015748467139327003
+    s: 8.9969823058544858
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7993190511153161
-  a: 1.1848525546687885
+  v: 4.7993190511153268
+  a: 1.1848525546687914
   relative_time: 4.499999999999992
 }
 trajectory_point {
   path_point {
-    x: 435703.91394960938
-    y: 4435956.9981724005
-    theta: 1.947805712135904
-    kappa: -0.0003797882292588048
-    s: 9.2384244573037062
+    x: 435703.85416522203
+    y: 4435956.9742714176
+    theta: 1.9605923999404833
+    kappa: -0.00039120950325894483
+    s: 9.2384244573037275
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8582692140175112
-  a: 1.1731174142491136
+  v: 4.8582692140175219
+  a: 1.1731174142491163
   relative_time: 4.5499999999999918
 }
 trajectory_point {
   path_point {
-    x: 435703.82400004368
-    y: 4435957.2253907248
-    theta: 1.9476927576018777
-    kappa: -0.00048761428871641388
-    s: 9.4827993566046072
+    x: 435703.76131629193
+    y: 4435957.2003204562
+    theta: 1.9604545375223055
+    kappa: -0.000623045637411715
+    s: 9.4827993566046285
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.9166271378232365
-  a: 1.1611629908100012
+  v: 4.9166271378232471
+  a: 1.1611629908100041
   relative_time: 4.5999999999999917
 }
 trajectory_point {
   path_point {
-    x: 435703.73298199195
-    y: 4435957.4553081132
-    theta: 1.9475784613129892
-    kappa: -0.00059672118409126083
-    s: 9.73007711769857
+    x: 435703.6673644351
+    y: 4435957.4290546682
+    theta: 1.9603150374761289
+    kappa: -0.00085763568883671178
+    s: 9.7300771176985918
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.97438185838152
-  a: 1.1489892843514515
+  v: 4.9743818583815314
+  a: 1.1489892843514542
   relative_time: 4.6499999999999915
 }
 trajectory_point {
   path_point {
-    x: 435703.64099057909
-    y: 4435957.68792947
-    theta: 1.9474015242050251
-    kappa: -0.00069434906439159094
-    s: 9.98022730631945
+    x: 435703.57244506234
+    y: 4435957.6604967276
+    theta: 1.9600388673082481
+    kappa: -0.001069245932680373
+    s: 9.9802273063194722
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.03152241154139
-  a: 1.1365962948734645
+  v: 5.0315224115414017
+  a: 1.1365962948734671
   relative_time: 4.6999999999999913
 }
 trajectory_point {
   path_point {
-    x: 435703.54796901415
-    y: 4435957.9231990064
-    theta: 1.9472117831981461
-    kappa: -0.00079084190967875238
-    s: 10.233218939993552
+    x: 435703.4764666874
+    y: 4435957.8945756
+    theta: 1.9597386422086966
+    kappa: -0.0012792784119697348
+    s: 10.233218939993574
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0880378331518754
-  a: 1.1239840223760398
+  v: 5.088037833151887
+  a: 1.1239840223760427
   relative_time: 4.7499999999999911
 }
 trajectory_point {
   path_point {
-    x: 435703.45392804116
-    y: 4435958.1610870585
-    theta: 1.9470141537364689
-    kappa: -0.00088638571009738
-    s: 10.489020488039632
+    x: 435703.37944891624
+    y: 4435958.1312652221
+    theta: 1.9594202899664779
+    kappa: -0.0014868810519523524
+    s: 10.489020488039653
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1439171590620036
-  a: 1.1111524668591779
+  v: 5.1439171590620143
+  a: 1.1111524668591808
   relative_time: 4.7999999999999909
 }
 trajectory_point {
   path_point {
-    x: 435703.3589987926
-    y: 4435958.4016109351
-    theta: 1.9467585180736853
-    kappa: -0.00096344118068281137
-    s: 10.747599871568895
+    x: 435703.28159394383
+    y: 4435958.3706137165
+    theta: 1.958978218671644
+    kappa: -0.0016580187838967273
+    s: 10.747599871568919
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1991494251208028
-  a: 1.0981016283228788
+  v: 5.1991494251208135
+  a: 1.0981016283228815
   relative_time: 4.8499999999999908
 }
 trajectory_point {
   path_point {
-    x: 435703.26306172751
-    y: 4435958.6446883343
-    theta: 1.9465001684459853
-    kappa: -0.0010413147107794704
-    s: 11.008924463485004
+    x: 435703.182700094
+    y: 4435958.6125032548
+    theta: 1.9585314540849381
+    kappa: -0.0018309733998116136
+    s: 11.008924463485029
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2537236671773
-  a: 1.0848315067671421
+  v: 5.2537236671773115
+  a: 1.0848315067671448
   relative_time: 4.8999999999999906
 }
 trajectory_point {
   path_point {
-    x: 435703.16620675789
-    y: 4435958.8903190419
-    theta: 1.9462200913025971
-    kappa: -0.0011081622348913598
-    s: 11.27296108848407
+    x: 435703.08292208496
+    y: 4435958.85696114
+    theta: 1.958035248601848
+    kappa: -0.0019806879354582361
+    s: 11.272961088484093
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3076289210805259
-  a: 1.071342102191968
+  v: 5.3076289210805374
+  a: 1.0713421021919707
   relative_time: 4.94999999999999
 }
 trajectory_point {
   path_point {
-    x: 435703.06847834494
-    y: 4435959.1384843374
-    theta: 1.9459104593802392
-    kappa: -0.0011590896641383295
-    s: 11.53967602305465
+    x: 435702.9823185287
+    y: 4435959.1039748657
+    theta: 1.9574752095409222
+    kappa: -0.0020990659232459894
+    s: 11.539676023054675
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3608542226795066
-  a: 1.0576334145973565
+  v: 5.3608542226795173
+  a: 1.0576334145973592
   relative_time: 4.99999999999999
 }
 trajectory_point {
   path_point {
-    x: 435702.96978111623
-    y: 4435959.389109781
-    theta: 1.9455977579756389
-    kappa: -0.0012105219546930395
-    s: 11.809034995477763
+    x: 435702.88071765436
+    y: 4435959.353437325
+    theta: 1.9569096186632617
+    kappa: -0.0022186174331731067
+    s: 11.80903499547779
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.41338860782327
-  a: 1.0437054439833078
+  v: 5.4133886078232818
+  a: 1.0437054439833104
   relative_time: 5.04999999999999
 }
 trajectory_point {
   path_point {
-    x: 435702.87030122743
-    y: 4435959.6422311757
-    theta: 1.945262492755484
-    kappa: -0.0012335691196419278
-    s: 12.081003185826873
+    x: 435702.77845654526
+    y: 4435959.6054479145
+    theta: 1.9562896052341514
+    kappa: -0.0022785676617852863
+    s: 12.081003185826898
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4652211123608456
-  a: 1.0295581903498214
+  v: 5.4652211123608581
+  a: 1.0295581903498241
   relative_time: 5.09999999999999
 }
 trajectory_point {
   path_point {
-    x: 435702.76992027403
-    y: 4435959.8977639442
-    theta: 1.9449195040757707
-    kappa: -0.0012501061371902548
-    s: 12.355545225967894
+    x: 435702.67529497429
+    y: 4435959.8598708333
+    theta: 1.9556535535173269
+    kappa: -0.0023264596171698282
+    s: 12.355545225967921
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5163407721412616
-  a: 1.0151916536968979
+  v: 5.5163407721412732
+  a: 1.0151916536969003
   relative_time: 5.14999999999999
 }
 trajectory_point {
   path_point {
-    x: 435702.66867240635
-    y: 4435960.1556828646
-    theta: 1.9445741237287744
-    kappa: -0.001255221783986381
-    s: 12.632625199559195
+    x: 435702.57130050135
+    y: 4435960.1166945463
+    theta: 1.9550106689713258
+    kappa: -0.0023492520519196344
+    s: 12.632625199559222
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5667366230135444
-  a: 1.0006058340245367
+  v: 5.5667366230135569
+  a: 1.0006058340245394
   relative_time: 5.1999999999999895
 }
 trajectory_point {
   path_point {
-    x: 435702.56667642627
-    y: 4435960.4159953743
-    theta: 1.9442277433451889
-    kappa: -0.0012289123606544188
-    s: 12.912206642051595
+    x: 435702.46666796372
+    y: 4435960.375958534
+    theta: 1.9543596056949717
+    kappa: -0.0023086226237979497
+    s: 12.912206642051622
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6163977008267238
-  a: 0.98580073133273838
+  v: 5.6163977008267363
+  a: 0.98580073133274093
   relative_time: 5.2499999999999893
 }
 trajectory_point {
   path_point {
-    x: 435702.46378137142
-    y: 4435960.67860249
-    theta: 1.9438783096877472
-    kappa: -0.0012023710248628627
-    s: 13.194252540688364
+    x: 435702.36111311062
+    y: 4435960.6375078829
+    theta: 1.9537028034455601
+    kappa: -0.0022676350551541894
+    s: 13.194252540688392
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6653130414298278
-  a: 0.97077634562150261
+  v: 5.66531304142984
+  a: 0.970776345621505
   relative_time: 5.2999999999999892
 }
 trajectory_point {
   path_point {
-    x: 435702.36017401476
-    y: 4435960.9435369652
-    theta: 1.9435539716964847
-    kappa: -0.0011358472558993628
-    s: 13.478725334505224
+    x: 435702.25498213014
+    y: 4435960.9014415
+    theta: 1.953093153048346
+    kappa: -0.0021343960627996806
+    s: 13.478725334505253
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7134716806718835
-  a: 0.95553267689082944
+  v: 5.7134716806718959
+  a: 0.95553267689083188
   relative_time: 5.349999999999989
 }
 trajectory_point {
   path_point {
-    x: 435702.25574425748
-    y: 4435961.2107147928
-    theta: 1.943234641681018
-    kappa: -0.0010578277109219805
-    s: 13.765586914330349
+    x: 435702.14804152149
+    y: 4435961.16762428
+    theta: 1.9524913527958832
+    kappa: -0.0019774657823650624
+    s: 13.765586914330381
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.76086265440192
-  a: 0.94006972514071885
+  v: 5.7608626544019321
+  a: 0.94006972514072129
   relative_time: 5.3999999999999888
 }
 trajectory_point {
   path_point {
-    x: 435702.15052516165
-    y: 4435961.480107341
-    theta: 1.9429353495015116
-    kappa: -0.00097452130678064679
-    s: 14.054798622784364
+    x: 435702.04035476514
+    y: 4435961.436039893
+    theta: 1.951935019898654
+    kappa: -0.0018043856569424661
+    s: 14.054798622784395
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8074749984689644
-  a: 0.92438749037117085
+  v: 5.8074749984689769
+  a: 0.92438749037117329
   relative_time: 5.4499999999999886
 }
 trajectory_point {
   path_point {
-    x: 435702.04458784685
-    y: 4435961.7517002905
-    theta: 1.9426755898902965
-    kappa: -0.00088194811500399615
-    s: 14.346321254280344
+    x: 435701.932028454
+    y: 4435961.7066888446
+    theta: 1.9514599020183419
+    kappa: -0.0016046579339233039
+    s: 14.346321254280376
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8532977487220457
-  a: 0.90848597258218566
+  v: 5.8532977487220581
+  a: 0.90848597258218788
   relative_time: 5.4999999999999885
 }
 trajectory_point {
   path_point {
-    x: 435701.9378252048
-    y: 4435962.0254091425
-    theta: 1.9424138065719507
-    kappa: -0.00078865371202934537
-    s: 14.640115055023816
+    x: 435701.82285820367
+    y: 4435961.979446345
+    theta: 1.950981082669087
+    kappa: -0.0014033741894725802
+    s: 14.64011505502385
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8983199410101914
-  a: 0.89236517177376284
+  v: 5.8983199410102047
+  a: 0.89236517177376506
   relative_time: 5.5499999999999883
 }
 trajectory_point {
   path_point {
-    x: 435701.83039813221
-    y: 4435962.30125335
-    theta: 1.9422051811278123
-    kappa: -0.0007177890258509204
-    s: 14.936139723012763
+    x: 435701.71312360751
+    y: 4435962.2543807384
+    theta: 1.9506224886842678
+    kappa: -0.0012453199661432914
+    s: 14.936139723012797
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9425306111824314
-  a: 0.87602508794590273
+  v: 5.9425306111824439
+  a: 0.87602508794590483
   relative_time: 5.5999999999999881
 }
 trajectory_point {
   path_point {
-    x: 435701.72218160867
-    y: 4435962.5791403465
-    theta: 1.9419970122138179
-    kappa: -0.00064723922513246484
-    s: 15.234354408037611
+    x: 435701.60258095764
+    y: 4435962.5313506313
+    theta: 1.9502630076300291
+    kappa: -0.0010867345465637461
+    s: 15.234354408037646
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.985918795087791
-  a: 0.8594657210986052
+  v: 5.9859187950878052
+  a: 0.85946572109860719
   relative_time: 5.6499999999999879
 }
 trajectory_point {
   path_point {
-    x: 435701.61326246173
-    y: 4435962.8590594949
-    theta: 1.9418131607251152
-    kappa: -0.00058983111181327682
-    s: 15.534717711681244
+    x: 435701.49137429206
+    y: 4435962.810368875
+    theta: 1.9499627559442787
+    kappa: -0.00095348165956008694
+    s: 15.534717711681278
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0284735285753008
-  a: 0.84268707123187025
+  v: 6.0284735285753142
+  a: 0.84268707123187225
   relative_time: 5.6999999999999877
 }
 trajectory_point {
   path_point {
-    x: 435701.50362660782
-    y: 4435963.1409603022
-    theta: 1.9416438391058883
-    kappa: -0.00054038429827427017
-    s: 15.837187687318993
+    x: 435701.379461415
+    y: 4435963.09137349
+    theta: 1.9496948300360597
+    kappa: -0.000834040086234241
+    s: 15.837187687319027
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0701838474939871
-  a: 0.825689138345698
+  v: 6.070183847494
+  a: 0.8256891383456999
   relative_time: 5.7499999999999876
 }
 trajectory_point {
   path_point {
-    x: 435701.39327400341
-    y: 4435963.42479714
-    theta: 1.9414821495102439
-    kappa: -0.00049687880809527415
-    s: 16.141721840118642
+    x: 435701.26683507086
+    y: 4435963.3743158011
+    theta: 1.9494491232458528
+    kappa: -0.00072649825257764165
+    s: 16.141721840118677
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.11103878769288
-  a: 0.80847192244008836
+  v: 6.1110387876928929
+  a: 0.80847192244009014
   relative_time: 5.7999999999999874
 }
 trajectory_point {
   path_point {
-    x: 435701.28225760185
-    y: 4435963.7105444111
-    theta: 1.9413385528712908
-    kappa: -0.00046677888682560784
-    s: 16.448277127040431
+    x: 435701.15355975222
+    y: 4435963.6591751454
+    theta: 1.9492489058543356
+    kappa: -0.00064315689617307193
+    s: 16.448277127040466
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1510273850210053
-  a: 0.79103542351504119
+  v: 6.1510273850210186
+  a: 0.791035423515043
   relative_time: 5.8499999999999872
 }
 trajectory_point {
   path_point {
-    x: 435701.17052892008
-    y: 4435963.998136498
-    theta: 1.9411946919486684
-    kappa: -0.00043727104548361257
-    s: 16.756809956837046
+    x: 435701.03956144693
+    y: 4435963.94587515
+    theta: 1.949050642515511
+    kappa: -0.00056158210932493379
+    s: 16.756809956837081
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1901386753273933
-  a: 0.77337964157055683
+  v: 6.1901386753274057
+  a: 0.77337964157055838
   relative_time: 5.899999999999987
 }
 trajectory_point {
   path_point {
-    x: 435701.05818399158
-    y: 4435964.2875633435
-    theta: 1.9410642803673275
-    kappa: -0.00042462169898751362
-    s: 17.067276190053626
+    x: 435700.92495317076
+    y: 4435964.2344132327
+    theta: 1.9488949364038106
+    kappa: -0.00051059118419475441
+    s: 17.067276190053661
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.22836169446107
-  a: 0.75550457660663484
+  v: 6.2283616944610838
+  a: 0.7555045766066365
   relative_time: 5.9499999999999869
 }
 trajectory_point {
   path_point {
-    x: 435700.94515561464
-    y: 4435964.5787509121
-    theta: 1.9409330754291263
-    kappa: -0.00041189540041903496
-    s: 17.379631139027758
+    x: 435700.80964767723
+    y: 4435964.524706631
+    theta: 1.9487382830563815
+    kappa: -0.0004592900566867265
+    s: 17.379631139027794
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2656854782710649
-  a: 0.73741022862327565
+  v: 6.2656854782710791
+  a: 0.7374102286232771
   relative_time: 5.9999999999999867
 }
 trajectory_point {
   path_point {
-    x: 435700.83153028134
-    y: 4435964.8716844525
-    theta: 1.9408059376210824
-    kappa: -0.00039881637106288644
-    s: 17.693829765741395
+    x: 435700.69374344195
+    y: 4435964.8167459844
+    theta: 1.9486018846923652
+    kappa: -0.00043147875018531557
+    s: 17.69382976574143
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3021113218592282
-  a: 0.71961801878464393
+  v: 6.3021113218592424
+  a: 0.71961801878464549
   relative_time: 6.0499999999999865
 }
 trajectory_point {
   path_point {
-    x: 435700.71727138007
-    y: 4435965.1663018791
-    theta: 1.9406792463285789
-    kappa: -0.00038559510943895418
-    s: 18.009827430631848
+    x: 435700.57719218719
+    y: 4435965.1104640961
+    theta: 1.9484690072334896
+    kappa: -0.0004083408360104245
+    s: 18.009827430631884
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.337646730537668
-  a: 0.70179283223437139
+  v: 6.3376467305376831
+  a: 0.70179283223437294
   relative_time: 6.0999999999999863
 }
 trajectory_point {
   path_point {
-    x: 435700.60242379172
-    y: 4435965.4625727888
-    theta: 1.9405601470689846
-    kappa: -0.00036877425664335088
-    s: 18.327579570732738
+    x: 435700.46004395763
+    y: 4435965.4058328709
+    theta: 1.9483400822216521
+    kappa: -0.000400297698632798
+    s: 18.327579570732777
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3722900554708044
-  a: 0.68393466897246058
+  v: 6.3722900554708186
+  a: 0.68393466897246213
   relative_time: 6.1499999999999861
 }
 trajectory_point {
   path_point {
-    x: 435700.48698937235
-    y: 4435965.76045002
-    theta: 1.9404460643006722
-    kappa: -0.000349458086152161
-    s: 18.647041540635911
+    x: 435700.34229578462
+    y: 4435965.7028031014
+    theta: 1.9482133103366506
+    kappa: -0.00040145153445577547
+    s: 18.64704154063595
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4060396478230537
-  a: 0.66604352899891139
+  v: 6.4060396478230679
+  a: 0.66604352899891284
   relative_time: 6.199999999999986
 }
 trajectory_point {
   path_point {
-    x: 435700.37098191155
-    y: 4435966.05989094
-    theta: 1.940335358824516
-    kappa: -0.00033424732874061315
-    s: 18.968168612491432
+    x: 435700.22396987939
+    y: 4435966.0013354691
+    theta: 1.9480823301207535
+    kappa: -0.0004112925127401553
+    s: 18.968168612491471
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4388938587588349
-  a: 0.64811941231372394
+  v: 6.4388938587588491
+  a: 0.64811941231372538
   relative_time: 6.2499999999999858
 }
 trajectory_point {
   path_point {
-    x: 435700.25443015934
-    y: 4435966.3608586285
-    theta: 1.9402297890807
-    kappa: -0.00032498986538636654
-    s: 19.290915976007593
+    x: 435700.10509275761
+    y: 4435966.3013922814
+    theta: 1.9479461738691035
+    kappa: -0.00043223201291568291
+    s: 19.290915976007632
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4708510394425653
-  a: 0.63016231891689811
+  v: 6.4708510394425796
+  a: 0.63016231891689944
   relative_time: 6.2999999999999856
 }
 trajectory_point {
   path_point {
-    x: 435700.13732732186
-    y: 4435966.663302307
-    theta: 1.9401247205080234
-    kappa: -0.00031771089730376516
-    s: 19.615238738450895
+    x: 435699.98566363967
+    y: 4435966.602924929
+    theta: 1.9478037516935236
+    kappa: -0.00045738144137285859
+    s: 19.615238738450934
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5019095410386631
-  a: 0.61217224880843391
+  v: 6.5019095410386782
+  a: 0.61217224880843524
   relative_time: 6.3499999999999854
 }
 trajectory_point {
   path_point {
-    x: 435700.0197249729
-    y: 4435966.9671937125
-    theta: 1.9400221822110968
-    kappa: -0.00031642212252141897
-    s: 19.94109192464607
+    x: 435699.86574297963
+    y: 4435966.9059089785
+    theta: 1.9476463862804525
+    kappa: -0.000493115287113208
+    s: 19.941091924646109
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5320677147115473
-  a: 0.59414920198833132
+  v: 6.5320677147115624
+  a: 0.59414920198833276
   relative_time: 6.3999999999999853
 }
 trajectory_point {
   path_point {
-    x: 435699.90159407316
-    y: 4435967.2724732859
-    theta: 1.9399190036291374
-    kappa: -0.00031580358648652246
-    s: 20.268430476976064
+    x: 435699.74529161339
+    y: 4435967.2102804538
+    theta: 1.9474840819236185
+    kappa: -0.00053045398541748672
+    s: 20.268430476976107
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5613239116256361
-  a: 0.57609317845659047
+  v: 6.56132391162565
+  a: 0.5760931784565918
   relative_time: 6.4499999999999851
 }
 trajectory_point {
   path_point {
-    x: 435699.78300195059
-    y: 4435967.5791186728
-    theta: 1.9398140267402042
-    kappa: -0.00032044001784433426
-    s: 20.597209255382058
+    x: 435699.62440376671
+    y: 4435967.5160280839
+    theta: 1.9472963036889646
+    kappa: -0.00057641408795683358
+    s: 20.5972092553821
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5896764829453458
-  a: 0.55800417821321124
+  v: 6.5896764829453609
+  a: 0.55800417821321258
   relative_time: 6.4999999999999849
 }
 trajectory_point {
   path_point {
-    x: 435699.66390664334
-    y: 4435967.8870651526
-    theta: 1.9397086044359848
-    kappa: -0.00032509612151297013
-    s: 20.927383037363434
+    x: 435699.5030068495
+    y: 4435967.8230745206
+    theta: 1.9471067127310748
+    kappa: -0.00062271041159294588
+    s: 20.927383037363477
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6171237798350964
-  a: 0.53988220125819375
+  v: 6.6171237798351115
+  a: 0.539882201258195
   relative_time: 6.5499999999999847
 }
 trajectory_point {
   path_point {
-    x: 435699.54439337278
-    y: 4435968.196297098
-    theta: 1.9395978211144274
-    kappa: -0.00033345822721522604
-    s: 21.258906517977806
+    x: 435699.38124099944
+    y: 4435968.1314264238
+    theta: 1.9468827902919119
+    kappa: -0.00067386004672290428
+    s: 21.258906517977849
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6436641534593042
-  a: 0.52172724759153777
+  v: 6.6436641534593193
+  a: 0.5217272475915391
   relative_time: 6.5999999999999845
 }
 trajectory_point {
   path_point {
-    x: 435699.42441015242
-    y: 4435968.506745751
-    theta: 1.9394865839981845
-    kappa: -0.00034186664858532821
-    s: 21.591734309841009
+    x: 435699.2589960866
+    y: 4435968.4409914734
+    theta: 1.9466579868752978
+    kappa: -0.00072521091961843566
+    s: 21.591734309841055
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6692959549823891
-  a: 0.50353931721324363
+  v: 6.6692959549824042
+  a: 0.50353931721324474
   relative_time: 6.6499999999999844
 }
 trajectory_point {
   path_point {
-    x: 435699.30404180672
-    y: 4435968.8183951359
-    theta: 1.9393695065698866
-    kappa: -0.00035156162657301772
-    s: 21.925820943127096
+    x: 435699.13643992523
+    y: 4435968.7517870078
+    theta: 1.9463993201739753
+    kappa: -0.000776182497790501
+    s: 21.925820943127142
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6940175355687677
-  a: 0.48531841012331106
+  v: 6.6940175355687828
+  a: 0.48531841012331223
   relative_time: 6.6999999999999842
 }
 trajectory_point {
   path_point {
-    x: 435699.18324424862
-    y: 4435969.1311793877
-    theta: 1.9392513786596268
-    kappa: -0.00036143658331070803
-    s: 22.261120865568344
+    x: 435699.01344989473
+    y: 4435969.0637156693
+    theta: 1.9461372643851058
+    kappa: -0.00082729663022096628
+    s: 22.26112086556839
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.71782724638286
-  a: 0.46706452632174011
+  v: 6.7178272463828748
+  a: 0.46706452632174128
   relative_time: 6.749999999999984
 }
 trajectory_point {
   path_point {
-    x: 435699.062090058
-    y: 4435969.4450776055
-    theta: 1.9391272109801696
-    kappa: -0.00037063532767704817
-    s: 22.597588442455248
+    x: 435698.89018450992
+    y: 4435969.3767908532
+    theta: 1.9458451411819522
+    kappa: -0.00087377663293559124
+    s: 22.597588442455297
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7407234385890806
-  a: 0.4487776658085309
+  v: 6.7407234385890966
+  a: 0.448777665808532
   relative_time: 6.7999999999999838
 }
 trajectory_point {
   path_point {
-    x: 435698.940546589
-    y: 4435969.7600281816
-    theta: 1.9390013367348105
-    kappa: -0.00037970155895441935
-    s: 22.935177956636526
+    x: 435698.76653534634
+    y: 4435969.6909207059
+    theta: 1.9455468505911133
+    kappa: -0.00091955444363782065
+    s: 22.935177956636576
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7627044633518514
-  a: 0.43045782858368331
+  v: 6.7627044633518665
+  a: 0.43045782858368437
   relative_time: 6.8499999999999837
 }
 trajectory_point {
   path_point {
-    x: 435698.81867786159
-    y: 4435970.076006718
-    theta: 1.9388708507904915
-    kappa: -0.00038640130045292564
-    s: 23.273843608519115
+    x: 435698.64265003777
+    y: 4435970.0061140526
+    theta: 1.9452249703838433
+    kappa: -0.00095686944896775311
+    s: 23.273843608519165
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7837686718355883
-  a: 0.41210501464719734
+  v: 6.7837686718356043
+  a: 0.4121050146471984
   relative_time: 6.8999999999999835
 }
 trajectory_point {
   path_point {
-    x: 435698.69645970262
-    y: 4435970.3929547127
-    theta: 1.93873852536777
-    kappa: -0.00039230074778231556
-    s: 23.613539516068172
+    x: 435698.51843202655
+    y: 4435970.32228362
+    theta: 1.9448957827941262
+    kappa: -0.00099189158550042284
+    s: 23.613539516068222
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.80391441520471
-  a: 0.39371922399907311
+  v: 6.8039144152047264
+  a: 0.39371922399907411
   relative_time: 6.9499999999999833
 }
 trajectory_point {
   path_point {
-    x: 435698.57394825539
-    y: 4435970.7108445382
-    theta: 1.9386045082610035
-    kappa: -0.00039436083667573842
-    s: 23.954219714807081
+    x: 435698.39401696326
+    y: 4435970.6394332359
+    theta: 1.9445523644095088
+    kappa: -0.0010149449422777678
+    s: 23.954219714807131
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.823140044623635
-  a: 0.3753004566393105
+  v: 6.823140044623651
+  a: 0.37530045663931144
   relative_time: 6.9999999999999831
 }
 trajectory_point {
   path_point {
-    x: 435698.45112677256
-    y: 4435971.02962039
-    theta: 1.938469533695647
-    kappa: -0.00039469222300462137
-    s: 24.295838157817439
+    x: 435698.26932022994
+    y: 4435970.957480222
+    theta: 1.944203030003903
+    kappa: -0.0010335433010911308
+    s: 24.295838157817489
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8414439112567811
-  a: 0.35684871256790951
+  v: 6.8414439112567962
+  a: 0.35684871256791051
   relative_time: 7.0499999999999829
 }
 trajectory_point {
   path_point {
-    x: 435698.32804111321
-    y: 4435971.34925049
-    theta: 1.9383356998756245
-    kappa: -0.00038986968418274756
-    s: 24.638348715739067
+    x: 435698.14445505978
+    y: 4435971.276419336
+    theta: 1.9438486900962484
+    kappa: -0.0010371754353269184
+    s: 24.63834871573912
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8588243662685651
-  a: 0.33836399178487025
+  v: 6.8588243662685811
+  a: 0.33836399178487114
   relative_time: 7.0999999999999828
 }
 trajectory_point {
   path_point {
-    x: 435698.2046823682
-    y: 4435971.6696818853
-    theta: 1.9382023512598916
-    kappa: -0.00038221920853052861
-    s: 24.98170517677001
+    x: 435698.01935359021
+    y: 4435971.59617437
+    theta: 1.9434915981532683
+    kappa: -0.0010339317505117561
+    s: 24.981705176770063
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8752797608234069
-  a: 0.31984629429019262
+  v: 6.8752797608234228
+  a: 0.31984629429019351
   relative_time: 7.1499999999999826
 }
 trajectory_point {
   path_point {
-    x: 435698.08108826913
-    y: 4435971.9908794835
-    theta: 1.9380739400764824
-    kappa: -0.00036817074973602849
-    s: 25.32586124666653
+    x: 435697.89411071327
+    y: 4435971.9167326456
+    theta: 1.943140824463887
+    kappa: -0.0010127612657773329
+    s: 25.325861246666584
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8908084460857237
-  a: 0.30129562008387661
+  v: 6.89080844608574
+  a: 0.30129562008387745
   relative_time: 7.1999999999999824
 }
 trajectory_point {
   path_point {
-    x: 435697.9572562754
-    y: 4435972.312792615
-    theta: 1.9379485435457895
-    kappa: -0.00035008483381786335
-    s: 25.670770548743111
+    x: 435697.76867297443
+    y: 4435972.23802348
+    theta: 1.9427930622737002
+    kappa: -0.00098207274447224065
+    s: 25.670770548743164
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9054087732199338
-  a: 0.28271196916592234
+  v: 6.90540877321995
+  a: 0.28271196916592312
   relative_time: 7.2499999999999822
 }
 trajectory_point {
   path_point {
-    x: 435697.83321594965
-    y: 4435972.6353828628
-    theta: 1.9378331293500017
-    kappa: -0.00032439527796699052
-    s: 26.016386623872457
+    x: 435697.64311596612
+    y: 4435972.5600264249
+    theta: 1.942465766573382
+    kappa: -0.00093045826401290952
+    s: 26.016386623872513
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9190790933904553
-  a: 0.26409534153632969
+  v: 6.9190790933904713
+  a: 0.26409534153633041
   relative_time: 7.2999999999999821
 }
 trajectory_point {
   path_point {
-    x: 435697.70897008409
-    y: 4435972.9586014347
-    theta: 1.9377245768510227
-    kappa: -0.00029342309874914212
-    s: 26.3626629304855
+    x: 435697.51739929838
+    y: 4435972.8826757707
+    theta: 1.9421501732401774
+    kappa: -0.00086660150894896917
+    s: 26.362662930485556
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9318177577617064
-  a: 0.24544573719509866
+  v: 6.9318177577617224
+  a: 0.24544573719509938
   relative_time: 7.3499999999999819
 }
 trajectory_point {
   path_point {
-    x: 435697.58454012719
-    y: 4435973.2824066188
-    theta: 1.9376312804913463
-    kappa: -0.00025361738958057546
-    s: 26.70955284457138
+    x: 435697.39157603146
+    y: 4435973.2059420822
+    theta: 1.9418695581777443
+    kappa: -0.00077886520925544121
+    s: 26.709552844571434
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9436231174981042
-  a: 0.22676315614222942
+  v: 6.94362311749812
+  a: 0.22676315614222997
   relative_time: 7.3999999999999817
 }
 trajectory_point {
   path_point {
-    x: 435697.459933364
-    y: 4435973.6067511775
-    theta: 1.937549197973734
-    kappa: -0.00020728769339325084
-    s: 27.057009659677469
+    x: 435697.26561893825
+    y: 4435973.52976464
+    theta: 1.9416104479580434
+    kappa: -0.00067629704702874881
+    s: 27.057009659677526
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9544935237640679
-  a: 0.20804759837772169
+  v: 6.9544935237640839
+  a: 0.20804759837772235
   relative_time: 7.4499999999999815
 }
 trajectory_point {
   path_point {
-    x: 435697.33516403905
-    y: 4435973.9315904565
-    theta: 1.9374886567316798
-    kappa: -0.00015086720123942584
-    s: 27.404986586909352
+    x: 435697.13956045586
+    y: 4435973.8541058116
+    theta: 1.9414035142063812
+    kappa: -0.0005467791100438943
+    s: 27.404986586909409
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9644273277240156
-  a: 0.18929906390157569
+  v: 6.9644273277240316
+  a: 0.18929906390157625
   relative_time: 7.4999999999999813
 }
 trajectory_point {
   path_point {
-    x: 435697.21024325484
-    y: 4435974.2568785083
-    theta: 1.9374445087002536
-    kappa: -8.6748687181456991e-05
-    s: 27.753436754930839
+    x: 435697.01338605839
+    y: 4435974.1789096612
+    theta: 1.9412297839379238
+    kappa: -0.00040001509471185558
+    s: 27.7534367549309
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9734228805423655
-  a: 0.17051755271379132
+  v: 6.9734228805423815
+  a: 0.17051755271379188
   relative_time: 7.5499999999999812
 }
 trajectory_point {
   path_point {
-    x: 435697.08517700061
-    y: 4435974.582567337
-    theta: 1.9374272351760877
-    kappa: -2.1199772279804059e-05
-    s: 28.102313209963967
+    x: 435696.88710427424
+    y: 4435974.50412911
+    theta: 1.9411230713173677
+    kappa: -0.00024767091329640982
+    s: 28.102313209964024
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9814785333835347
-  a: 0.15170306481436868
+  v: 6.98147853338355
+  a: 0.15170306481436913
   relative_time: 7.599999999999981
 }
 trajectory_point {
   path_point {
-    x: 435696.95998043875
-    y: 4435974.9086123779
-    theta: 1.9374306172573141
-    kappa: 4.5458366064588714e-05
-    s: 28.451568915788975
+    x: 435696.76071531815
+    y: 4435974.8297137851
+    theta: 1.9410593299050034
+    kappa: -9.1699917313367929e-05
+    s: 28.451568915789039
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9885926374119416
-  a: 0.13285560020330756
+  v: 6.9885926374119576
+  a: 0.132855600203308
   relative_time: 7.6499999999999808
 }
 trajectory_point {
   path_point {
-    x: 435696.834655271
-    y: 4435975.2349638129
-    theta: 1.9374583591322261
-    kappa: 0.00010616356455201408
-    s: 28.801156753744351
+    x: 435696.63421082729
+    y: 4435975.155609888
+    theta: 1.9410576440308203
+    kappa: 5.151204153275669e-05
+    s: 28.801156753744412
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9947635437920042
-  a: 0.11397515888060827
+  v: 6.99476354379202
+  a: 0.11397515888060861
   relative_time: 7.6999999999999806
 }
 trajectory_point {
   path_point {
-    x: 435696.70922069583
-    y: 4435975.5615784507
-    theta: 1.937504648048505
-    kappa: 0.00016234251232417589
-    s: 29.151029522726777
+    x: 435696.50760617462
+    y: 4435975.4817727562
+    theta: 1.9410952393532461
+    kappa: 0.00018667804453533521
+    s: 29.151029522726837
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9999896036881415
-  a: 0.0950617408462705
+  v: 6.9999896036881575
+  a: 0.095061740846270837
   relative_time: 7.7499999999999805
 }
 trajectory_point {
   path_point {
-    x: 435696.58367725648
-    y: 4435975.8884058492
-    theta: 1.9375713379936408
-    kappa: 0.0002128907618522051
-    s: 29.50113993919117
+    x: 435696.3808833384
+    y: 4435975.808144669
+    theta: 1.9411866015672623
+    kappa: 0.00030979824666651275
+    s: 29.501139939191233
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0042691682647709
-  a: 0.076115346100294468
+  v: 7.0042691682647868
+  a: 0.0761153461002948
   relative_time: 7.79999999999998
 }
 trajectory_point {
   path_point {
-    x: 435696.45804801281
-    y: 4435976.215404124
-    theta: 1.9376531903599945
-    kappa: 0.00025925686843443409
-    s: 29.851440637150667
+    x: 435696.25407183985
+    y: 4435976.1346862791
+    theta: 1.9413110540675438
+    kappa: 0.00042552305948064277
+    s: 29.851440637150731
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.00760058868631
-  a: 0.057135974642680054
+  v: 7.0076005886863264
+  a: 0.057135974642680276
   relative_time: 7.84999999999998
 }
 trajectory_point {
   path_point {
-    x: 435696.33233044518
-    y: 4435976.542521474
-    theta: 1.937752103666236
-    kappa: 0.00030027251363261162
-    s: 30.201884168176619
+    x: 435696.12714116857
+    y: 4435976.4613348208
+    theta: 1.9414826141497645
+    kappa: 0.00052987693552602848
+    s: 30.201884168176683
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0099822161171783
-  a: 0.038123626473427374
+  v: 7.0099822161171943
+  a: 0.038123626473427596
   relative_time: 7.89999999999998
 }
 trajectory_point {
   path_point {
-    x: 435696.20655206259
-    y: 4435976.8697175495
-    theta: 1.9378632645866329
-    kappa: 0.00033744570863123916
-    s: 30.552423001398612
+    x: 435696.00013607787
+    y: 4435976.7880566912
+    theta: 1.9416820513033273
+    kappa: 0.00062750627011577817
+    s: 30.552423001398676
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0114124017217927
-  a: 0.019078301592536206
+  v: 7.0114124017218087
+  a: 0.019078301592536429
   relative_time: 7.94999999999998
 }
 trajectory_point {
   path_point {
-    x: 435696.08070869744
-    y: 4435977.1969397292
-    theta: 1.9379880675293291
-    kappa: 0.00036957297671069207
-    s: 30.903009523504434
+    x: 435695.87301709031
+    y: 4435977.1147854263
+    theta: 1.9419213221100322
+    kappa: 0.00071445050949471751
+    s: 30.9030095235045
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0118894966645708
-  a: 6.8833827526759706e-15
+  v: 7.0118894966645868
+  a: 6.9944050551384862e-15
   relative_time: 7.99999999999998
 }
 decision {