Improve the lateral acceleration filter in motion velocity smoother

[brkay54]

Hello everyone,

In current implementation of lateral acceleration filtering in trajectory, we are estimating the curvature by selecting three point. Then, we are calculating the lateral acceleration with this curvature estimation. See here!

I want to suggest a new approach to estimate the curvature in trajectory points. We can estimate the curvature by using the orientation of trajectory points. I think orientation can give more accurate results because it is output of optimization.

We can calculate the desired steering angle with following equation:

After calculate the desired steering angles, tan(steering_angle) / wheelbase will give us the curvature of the trajectory point. We can set the velocities w.r.t. this calculation. What do you think? Please feel free to share your thoughts. Thank you!

[brkay54]

cc. @mehmetdogru @TakaHoribe

[brkay54]

I tested the both approach with control_performance_analysis (in this PR).

Results (Before is current implementation, after is implementation with new approach):

Series	Min Before	Min After	Max Before	Max After	Average Before	Average After
/control_performance/driving_status/lateral_acceleration/data	-0.414748	-0.404385	0.587819	0.570642	0.028891	0.025944
/control_performance/driving_status/lateral_jerk/data	-4.37723	-3.515305	2.620279	2.488174	0.001978	-0.002855
/control_performance/driving_status/longitudinal_acceleration/data	-0.912165	-0.992638	1.171741	1.169503	-0.00547	-0.001894
/control_performance/driving_status/longitudinal_jerk/data	-37.579326	-38.59962	50.363508	64.01396	-0.043433	0.003336
/control_performance/performance_vars/error/control_effort_energy	1E-06	3E-06	3.751512	3.640998	0.548236	0.517991
/control_performance/performance_vars/error/curvature_estimate	-0.196328	-0.15474	0.147634	0.14805	0.012307	0.011465
/control_performance/performance_vars/error/curvature_estimate_pp	-0.685987	-0.659468	0.857495	0.864488	0.0036	0.016651
/control_performance/performance_vars/error/error_energy	3E-06	3E-06	725.076238	744.538529	16.453592	14.461468
/control_performance/performance_vars/error/heading_error	-3.935703	-3.6235	0.131046	0.089199	-0.138134	-0.130147
/control_performance/performance_vars/error/heading_error_velocity	-0.228567	-0.232441	0.357787	0.351656	0.013681	0.012888
/control_performance/performance_vars/error/lateral_error	-0.201026	-0.203656	25.594785	24.925392	0.763545	0.690954
/control_performance/performance_vars/error/lateral_error_acceleration	-0.677725	-0.646119	0.977959	0.961904	0.020453	0.020365
/control_performance/performance_vars/error/lateral_error_velocity	-0.263545	-0.231912	3.613527	3.311932	0.080852	0.070448
/control_performance/performance_vars/error/longitudinal_error	-1.416918	-1.687636	0.415556	0.557804	-0.02788	-0.024898
/control_performance/performance_vars/error/longitudinal_error_acceleration	-0.879877	-0.888177	0.731259	0.756286	0.009436	0.000694
/control_performance/performance_vars/error/longitudinal_error_velocity	-4.887811	-4.10775	0.486414	0.093616	-0.102476	-0.100788
/control_performance/performance_vars/error/tracking_curvature_discontinuity_ability	0	0	0.194609	0.149417	0.002406	0.002328
/control_performance/performance_vars/error/value_approximation	4E-06	2E-06	1157.290071	1141.501573	22.946368	20.431148
/control_performance/performance_vars/error/vehicle_velocity_error	-0.25	-0.34384	4.550888	4.419915	0.089122	0.073407
iae_heading_error	0.070968	0.036298	2140.478331	1845.584953	1439.086766	1231.35816
iae_heading_velocity_error	0.260451	0.140809	839.786466	780.52647	438.247896	396.277805
iae_lateral_acceleration_error	0.062105	0.033544	2175.340398	2000.631767	1234.263002	1106.525029
iae_lateral_error	1211.975957	646.071468	11887.734163	9804.156735	8798.502473	7032.961661
iae_lateral_velocity_error	4.9E-05	1.5E-05	1779.449632	1556.360706	1223.726301	1037.138146
iae_longitudina_velocity_error	891.871092	475.582655	2458.981641	1861.455609	1950.962988	1390.603746
iae_longitudinal_acceleration_error	176.916485	93.702997	2746.845968	2458.832042	1752.962301	1549.778308
iae_longitudinal_error	0.00567	0.002785	447.159208	366.669203	306.049465	253.690967
iae_tracking_curvature_discontinuity_ability	0.000405	9E-06	31.055063	28.275924	14.034673	13.055478
rms_heading_error	0.070968	0.036298	2140.478331	1845.584953	1439.086766	1231.35816
rms_heading_velocity_error	0.260451	0.140809	839.786466	780.52647	438.247896	396.277805
rms_lateral_acceleration_error	0.062105	0.033544	2175.340398	2000.631767	1234.263002	1106.525029
rms_lateral_error	1211.975957	646.071468	11887.734163	9804.156735	8798.502473	7032.961661
rms_lateral_velocity_error	4.9E-05	1.5E-05	1779.449632	1556.360706	1223.726301	1037.138146
rms_longitudinal_acceleration_error	176.916485	93.702997	2746.845968	2458.832042	1752.962301	1549.778308
rms_longitudinal_error	0.00567	0.002785	447.159208	366.669203	306.049465	253.690967
rms_longitudinal_velocity_error	891.871092	475.582655	2458.981641	1861.455609	1950.962988	1390.603746
rms_tracking_curvature_discontinuity_ability	0.000405	9E-06	31.055063	28.275924	14.034673	13.055478

---

With new approach, maximum, total, average lateral errors decreased.

Related issue #567