function [G,R]=dric_solv(A,B,S,Q,P,N);
% DRIC_SOLV
% [G,R]=dric_solv(A,B,S,Q,P,N);
% Iterate Riccati-equation backward
% in time from k=N+1 to k=1.

R=S;
for k=N:-1:1
 G = -inv(P+B'*R*B)*B'*R*A;
 R = (A+B*G)'*R*(A+B*G)+G'*P*G+Q;  % Josehps stabilized version of Riccati eq. 
end