From 97c78802bb3291005af85cfca7bd206fb59e9127 Mon Sep 17 00:00:00 2001 From: Sylvain CALINON <sylvain.calinon@idiap.ch> Date: Fri, 4 Apr 2025 11:19:10 +0200 Subject: [PATCH] LQT_LS refined --- matlab/LQT_recursive.m | 5 ++++- matlab/LQT_recursive_LS.m | 6 +++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/matlab/LQT_recursive.m b/matlab/LQT_recursive.m index e10ca11..c396f82 100644 --- a/matlab/LQT_recursive.m +++ b/matlab/LQT_recursive.m @@ -79,7 +79,10 @@ for n=1:2 x = x + xn; %Simulated noise on the state end Ka = (B' * P(:,:,t) * B + Ra) \ B' * P(:,:,t) * A; %Feedback gain - u = -Ka * x; %Feedback control on augmented state (resulting in feedback and feedforward terms on state) + u = -Ka * x; %Feedback control on augmented state (resulting in feedforward part and feedback on state) +% u_ff = -Ka(:,end); %Feedforward part on state +% u = u_ff - Ka(:,1:end-1) * x(1:end-1); %Feedforward and feedback part on state + x = A * x + B * u; %Update of state vector r(n).x(:,t) = x; %Log data end diff --git a/matlab/LQT_recursive_LS.m b/matlab/LQT_recursive_LS.m index 9cd3db8..7a0c437 100644 --- a/matlab/LQT_recursive_LS.m +++ b/matlab/LQT_recursive_LS.m @@ -97,9 +97,9 @@ for n=1:2 if t==25 && n==2 x = x + xn; %Simulated noise on the state end -% u = -Ka(:,:,t) * x; %Feedback control on augmented state (resulting in feedforward part and feedback on state) - u_ff = -Ka(:,end,t); %Feedforward part on state - u = u_ff - Ka(:,1:end-1,t) * x(1:end-1); %Feedforward and feedback part on state + u = -Ka(:,:,t) * x; %Feedback control on augmented state (resulting in feedforward part and feedback on state) +% u_ff = -Ka(:,end,t); %Feedforward part on state +% u = u_ff - Ka(:,1:end-1,t) * x(1:end-1); %Feedforward and feedback part on state x = A * x + B * u; %Update of state vector r(n).x(:,t) = x; %Log data -- GitLab