diff --git a/libs/kimath/include/math/matrix3x3.h b/libs/kimath/include/math/matrix3x3.h
index b8bc98763a..7e6ec11a72 100644
--- a/libs/kimath/include/math/matrix3x3.h
+++ b/libs/kimath/include/math/matrix3x3.h
@@ -164,6 +164,11 @@ public:
      */
     friend std::ostream& operator<<<T>( std::ostream& aStream, const MATRIX3x3<T>& aMatrix );
 
+    ///< Equality operator
+    bool operator==( const MATRIX3x3<T>& aOtherMatrix ) const;
+
+    ///< Not equality operator
+    bool operator!=( const MATRIX3x3<T>& aOtherMatrix ) const;
 };
 
 // Operators
@@ -335,7 +340,7 @@ VECTOR3<T> const operator*( MATRIX3x3<T> const& aMatrix, VECTOR3<T> const& aVect
                + aMatrix.m_data[0][2] * aVector.z;
     result.y = aMatrix.m_data[1][0] * aVector.x + aMatrix.m_data[1][1] * aVector.y
                + aMatrix.m_data[1][2] * aVector.z;
-    result.y = aMatrix.m_data[2][0] * aVector.x + aMatrix.m_data[2][1] * aVector.y
+    result.z = aMatrix.m_data[2][0] * aVector.x + aMatrix.m_data[2][1] * aVector.y
                + aMatrix.m_data[2][2] * aVector.z;
 
     return result;
@@ -434,6 +439,36 @@ std::ostream& operator<<( std::ostream& aStream, const MATRIX3x3<T>& aMatrix )
 }
 
 
+template <class T>
+bool MATRIX3x3<T>::operator==( MATRIX3x3<T> const& aOtherMatrix ) const
+{
+    return aOtherMatrix.m_data[0][0] == m_data[0][0] &&
+        aOtherMatrix.m_data[0][1] == m_data[0][1] &&
+        aOtherMatrix.m_data[0][2] == m_data[0][2] &&
+        aOtherMatrix.m_data[1][0] == m_data[1][0] &&
+        aOtherMatrix.m_data[1][1] == m_data[1][1] &&
+        aOtherMatrix.m_data[1][2] == m_data[1][2] &&
+        aOtherMatrix.m_data[2][0] == m_data[2][0] &&
+        aOtherMatrix.m_data[2][1] == m_data[2][1] &&
+        aOtherMatrix.m_data[2][2] == m_data[2][2];
+}
+
+
+template <class T>
+bool MATRIX3x3<T>::operator!=( MATRIX3x3<T> const& aOtherMatrix ) const
+{
+    return aOtherMatrix.m_data[0][0] != m_data[0][0] ||
+        aOtherMatrix.m_data[0][1] != m_data[0][1] ||
+        aOtherMatrix.m_data[0][2] != m_data[0][2] ||
+        aOtherMatrix.m_data[1][0] != m_data[1][0] ||
+        aOtherMatrix.m_data[1][1] != m_data[1][1] ||
+        aOtherMatrix.m_data[1][2] != m_data[1][2] ||
+        aOtherMatrix.m_data[2][0] != m_data[2][0] ||
+        aOtherMatrix.m_data[2][1] != m_data[2][1] ||
+        aOtherMatrix.m_data[2][2] != m_data[2][2];
+}
+
+
 /* Default specializations */
 typedef MATRIX3x3<double> MATRIX3x3D;
 
diff --git a/qa/unittests/libs/kimath/CMakeLists.txt b/qa/unittests/libs/kimath/CMakeLists.txt
index 7c61a5ecdf..e4fd1a0e95 100644
--- a/qa/unittests/libs/kimath/CMakeLists.txt
+++ b/qa/unittests/libs/kimath/CMakeLists.txt
@@ -41,6 +41,7 @@ set( QA_KIMATH_SRCS
     geometry/test_shape_line_chain.cpp
 
     math/test_box2.cpp
+    math/test_matrix3x3.cpp
     math/test_vector2.cpp
     math/test_vector3.cpp
     math/test_util.cpp
@@ -60,7 +61,7 @@ endif()
 add_executable( qa_kimath
     ${QA_KIMATH_SRCS}
     ${QA_KIMATH_RESOURCES}
-    
+
     # Mock Pgm needed for advanced_config
     ${CMAKE_SOURCE_DIR}/qa/mocks/kicad/common_mocks.cpp
 )
diff --git a/qa/unittests/libs/kimath/math/test_matrix3x3.cpp b/qa/unittests/libs/kimath/math/test_matrix3x3.cpp
new file mode 100644
index 0000000000..8db24c134c
--- /dev/null
+++ b/qa/unittests/libs/kimath/math/test_matrix3x3.cpp
@@ -0,0 +1,74 @@
+/*
+ * This program source code file is part of KiCad, a free EDA CAD application.
+ *
+ * Copyright (C) 2023 KiCad Developers, see AUTHORS.TXT for contributors.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, you may find one here:
+ * http://www.gnu.org/licenses/old-licenses/gpl-2.0.html
+ * or you may search the http://www.gnu.org website for the version 2 license,
+ * or you may write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
+ */
+
+/**
+ * Test suite for KiCad math code.
+ */
+
+#include <qa_utils/wx_utils/unit_test_utils.h>
+
+// Code under test
+#include <math/matrix3x3.h>
+
+/**
+ * Declare the test suite
+ */
+BOOST_AUTO_TEST_SUITE( MATRIX3X3TESTS )
+
+
+BOOST_AUTO_TEST_CASE( test_equality_ops, *boost::unit_test::tolerance( 0.000001 ) )
+{
+    MATRIX3x3D m1( VECTOR3I{ 1, 1, 1 }, { 2, 2, 2 }, { 3, 3, 3 } );
+    MATRIX3x3D m2( VECTOR3I{ 6, 6, 6 }, { 1, 1, 1 }, { 3, 3, 3 } );
+    MATRIX3x3D m3( VECTOR3I{ 1, 1, 1 }, { 2, 2, 2 }, { 3, 3, 3 } );
+
+    BOOST_CHECK( m1 == m3 );
+    BOOST_CHECK( m2 != m1 );
+}
+
+BOOST_AUTO_TEST_CASE( test_matrix_multiply_vector, *boost::unit_test::tolerance( 0.000001 ) )
+{
+    MATRIX3x3 m1( VECTOR3I{ 1, 1, 1 }, { 2, 2, 2 }, { 3, 3, 3 } );
+    VECTOR3I v1( 5, 5, 5 );
+
+    VECTOR3I res = m1 * v1;
+
+    VECTOR3I expected( 15, 30, 45 );
+
+    BOOST_CHECK( res == expected );
+}
+
+
+BOOST_AUTO_TEST_CASE( test_matrix_multiply_scalar, *boost::unit_test::tolerance( 0.000001 ) )
+{
+    MATRIX3x3 m1( VECTOR3I{ 1, 1, 1 }, { 2, 2, 2 }, { 3, 3, 3 } );
+
+    MATRIX3x3 res = m1 * 5;
+
+    MATRIX3x3 expected( VECTOR3I{ 5, 5, 5 }, { 10, 10, 10 }, { 15, 15, 15 } );
+
+    BOOST_CHECK( res == expected );
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()