From d70e4c5f34dac5ffb9ca43be87629825835e7d0e Mon Sep 17 00:00:00 2001 From: yasnem Date: Wed, 31 Jan 2018 11:55:08 +0100 Subject: [PATCH] Update model_predictive_trajectory_generator.py Mistake in computation of the Jacobian. --- .../model_predictive_trajectory_generator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py index cfbb750a..c8448962 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py @@ -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)