diff --git a/code/eigenvalue_methods.py b/code/eigenvalue_methods.py
index 5f218d7d..8c7dc2c6 100644
--- a/code/eigenvalue_methods.py
+++ b/code/eigenvalue_methods.py
@@ -10,7 +10,7 @@ class EigenvalueMethods:
         for _ in range(max_iter):
             x = LinearAlgebraUtils.matrix_vector_multiply(A, x)
             lambda_new = LinearAlgebraUtils.vector_norm(x)
-            x = LinearAlgebraUtils.scalar_divide(x, lambda_new)
+            x = LinearAlgebraUtils.vector_scalar_divide(x, lambda_new)
             if abs(lambda_new - lambda_old) < tol:
                 break
             lambda_old = lambda_new
diff --git a/code/linear_algebra_utils.py b/code/linear_algebra_utils.py
index 9b3647fa..c6175668 100644
--- a/code/linear_algebra_utils.py
+++ b/code/linear_algebra_utils.py
@@ -1,3 +1,5 @@
+import math
+
 class LinearAlgebraUtils:
     @staticmethod
     def dot_product(v1, v2):
@@ -8,15 +10,19 @@ class LinearAlgebraUtils:
         return [LinearAlgebraUtils.dot_product(row, x) for row in A]
 
     @staticmethod
-    def vector_subtraction(v1, v2):
+    def matrix_scalar_multiply(A, w):
+        return [[w * A[i][j] for j in range(len(A[0]))] for i in range(len(A))]
+
+    @staticmethod
+    def vector_vector_subtraction(v1, v2):
         return [x-y for x, y in zip(v1, v2)]
 
     @staticmethod
-    def vector_addition(v1, v2):
+    def vector_vector_addition(v1, v2):
         return [x+y for x, y in zip(v1, v2)]
 
     @staticmethod
-    def scalar_multiply(omega, vector):
+    def scalar_matrix_multiply(omega, vector):
         return [omega * x for x in vector]
 
     @staticmethod
@@ -24,5 +30,13 @@ class LinearAlgebraUtils:
         return sum(x*x for x in v)**0.5
 
     @staticmethod
-    def scalar_divide(x, scalar):
+    def matrix_norm(A):
+        return math.sqrt(sum(sum(element ** 2 for element in row) for row in A))
+
+    @staticmethod
+    def vector_scalar_divide(x, scalar):
         return [xi / scalar for xi in x]
+
+    @staticmethod
+    def matrix_matrix_subtraction(A, B):
+        return [[A[i][j] - B[i][j] for j in range(len(A[0]))] for i in range(len(A))]
diff --git a/code/matrix_generator.py b/code/matrix_generator.py
index ab4addd2..0b99273b 100644
--- a/code/matrix_generator.py
+++ b/code/matrix_generator.py
@@ -7,3 +7,6 @@ class MatrixGenerator:
         A = np.dot(A.T, A) + np.eye(size) * size # dodanie `size` do przekątnej zwiększa wartości własne -> obejście problemu z overflow
         b = np.random.uniform(-1, 1, size)
         return A, b
+
+    def generate_identity_matrix(size):
+        return np.eye(size)
diff --git a/code/richardson_method.py b/code/richardson_method.py
index 86d07927..0dfc753b 100644
--- a/code/richardson_method.py
+++ b/code/richardson_method.py
@@ -1,13 +1,15 @@
 from linear_algebra_utils import LinearAlgebraUtils
 from eigenvalue_methods import EigenvalueMethods
+from matrix_generator import MatrixGenerator
 
 class RichardsonMethod:
-    def __init__(self, A, b, x0=None, max_iterations=1000, tol=1e-5):
+    def __init__(self, A, b, size: int, x0=None, max_iterations=1000, tol=1e-5):
         self.A = A
         self.b = b
         self.x0 = x0 if x0 is not None else [0.0] * len(b)
         self.max_iterations = max_iterations
         self.tol = tol
+        self.I = MatrixGenerator.generate_identity_matrix(size)
 
     def solve(self):
         x = self.x0[:]
@@ -21,13 +23,14 @@ class RichardsonMethod:
 
         for iteration in range(self.max_iterations):
             Ax = LinearAlgebraUtils.matrix_vector_multiply(self.A, x)
-            residual = LinearAlgebraUtils.vector_subtraction(self.b, Ax)
-
-            if LinearAlgebraUtils.vector_norm(residual) < self.tol:
+            residual = LinearAlgebraUtils.vector_vector_subtraction(self.b, Ax)
+            wA = LinearAlgebraUtils.matrix_scalar_multiply(self.A, omega)
+            IMinuswA = LinearAlgebraUtils.matrix_matrix_subtraction(self.I, wA)
+            if LinearAlgebraUtils.matrix_norm(IMinuswA) < 1:
                 print('Convergence achieved.')
                 return x
             
-            x = LinearAlgebraUtils.vector_addition(x, LinearAlgebraUtils.scalar_multiply(omega, residual))
+            x = LinearAlgebraUtils.vector_vector_addition(x, LinearAlgebraUtils.scalar_matrix_multiply(omega, residual))
 
         print('Maximum number of iterations reached without convergence.')
         return x
diff --git a/code/tests.py b/code/tests.py
index dcc6b654..0dc23ccc 100644
--- a/code/tests.py
+++ b/code/tests.py
@@ -12,7 +12,7 @@ def run_tests():
         
         A, b = MatrixGenerator.generate_random_matrix_and_vector(n)
 
-        richardson_solver = RichardsonMethod(A, b, max_iterations=10000000, tol=1e-7)
+        richardson_solver = RichardsonMethod(A, b, size=n, max_iterations=10000000, tol=1e-7)
         solution_richardson = richardson_solver.solve()
         print("Richardson Method Solution:", solution_richardson)