function X = sda_dare(A,B,C,Q,R,s)
% X=SDA_DARE(A,B,C,Q,R,g) solves the DARE
% A'XA + Q - (C + B'XA)'(R + B'XB)^{-1}(C + B'XA) - X = 0
% by means of SDA
%    A, B, C, Q, R, E: matrix coefficients
%    s: a real positive number such that R+s B'B is nonsingular
%    X : solution of the DARE
n = size(A,1);
m = size(R,1);
tol = 1e-13;
kmax = 30;
% Initialization
W = s*eye(n);
HR = R + B'*W*B;
HRI = inv(HR);
S = B*HRI;
G = -S*B';
S = S*C;
E = (eye(n) + G*W)*A - S;
P = Q - W - C'*HRI*(C+B'*W*A) + A'*W*E;
% SDA step
err = 1;
k = 0;
while err > tol && k < kmax
    IGP = eye(n) - G*P;
    Z = [E;P']/IGP;
    E1 = Z(1:n,:);
    P1 = Z(n+1:end,:);
    G = G + E1*G*E';
    P = P + E'*P1'*E;
    E = E1*E;
    err = norm(E,1);
    k = k + 1;
end
X = P + W;
if k == kmax
    disp('Warning: reached the maximum number of iterations')
end
