diff --git a/matlab/iLQR_curvature.m b/matlab/iLQR_curvature.m
index a30ecd3583d58f699ee3302059423362a1b95588..eb08c07a56a6f3f24b8be3a3142e04c0fb7cda2a 100644
--- a/matlab/iLQR_curvature.m
+++ b/matlab/iLQR_curvature.m
@@ -228,7 +228,7 @@ function [f, J] = f_curvature(x, param)
 	for t=1:param.nbData
 		a = x(:,t)' * Sa * x(:,t);
 		b = x(:,t)' * Sb * x(:,t) + 1E-8; 
-		Jtmp = 2 * b^(-3/2) * Sa * x(:,t) - 3 * a * b^(-5/2) * Sb * x(:,t);
+		Jtmp = b^(-3/2) * (Sa + Sa') * x(:,t) - 3 * a * b^(-5/2) * Sb * x(:,t);
 		J = blkdiag(J, Jtmp');
 	end
 end