From 0c15ba09508c44eca10e174663e73714be0fa7cb Mon Sep 17 00:00:00 2001
From: Sylvain CALINON <sylvain.calinon@idiap.ch>
Date: Sat, 26 Oct 2024 11:54:58 +0200
Subject: [PATCH] asym matrix corrected

---
 matlab/iLQR_curvature.m | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/matlab/iLQR_curvature.m b/matlab/iLQR_curvature.m
index a30ecd3..eb08c07 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
-- 
GitLab