Matrix eqn

Added solve_discrete_lyapunov and solve_discrete_riccati.
use `f_gamma = max(f1, f2, f3)`