github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 15:48:13 -05:00
Code Issues Packages Projects Releases Wiki Activity

Update model_predictive_trajectory_generator.py

Browse Source 
Mistake in computation of the Jacobian.
This commit is contained in:
yasnem
2018-01-31 11:55:08 +01:00
committed by GitHub
parent b5ecb67611
commit d70e4c5f34
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
View File

@@ -42,7 +42,7 @@ def calc_J(target, p, h, k0):
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0] - h[0, 0], p[1, 0], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d1 = np.matrix((dp - dn) / (2.0 * h[1, 0])).T
d1 = np.matrix((dp - dn) / (2.0 * h[0, 0])).T
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0], p[1, 0] + h[1, 0], p[2, 0], k0)
@@ -50,7 +50,7 @@ def calc_J(target, p, h, k0):
xn, yn, yawn = motion_model.generate_last_state(
p[0, 0], p[1, 0] - h[1, 0], p[2, 0], k0)
dn = calc_diff(target, [xn], [yn], [yawn])
d2 = np.matrix((dp - dn) / (2.0 * h[2, 0])).T
d2 = np.matrix((dp - dn) / (2.0 * h[1, 0])).T
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0], p[1, 0], p[2, 0] + h[2, 0], k0)