function [ S ] = equilibrioS( A,C,Q,R,lambda,tol )
%EQUILIBRIOS Summary of this function goes here
%   Detailed explanation goes here
S = zeros(4,4);
for j = 1:10000
    S_ = A*S*A' + Q - lambda*A*S*C'*pinv(C*S*C' + R)*C*S*A';
    if norm(S-S_) < tol
        S = S_;
        break;
    end
    S = S_;
end
