asym matrix corrected

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