diff --git a/main/matrix.cpp b/main/matrix.cpp
index 824946e..11466b6 100644
--- a/main/matrix.cpp
+++ b/main/matrix.cpp
@@ -51,55 +51,55 @@ float * m_add_helper(float * A, float * B, int elements) {
 }
 
 //matrix multiplication
-Matrix mMult(Matrix A, Matrix B)
+Matrix Matrix::operator*(Matrix B)
 {
-    const int m=A.rows;
+    const int m=rows;
     const int p=B.columns;
-    const int n=A.columns;
-    float * values;
-    if (A.columns==B.rows)
+    const int n=columns;
+    float * new_values;
+    if (columns==B.rows)
     {
-        values=m_mult_helper(A.values, B.values, m, n, p);
+        new_values=m_mult_helper(values, B.values, m, n, p);
     }
-    return Matrix(m, p, values);
+    return Matrix(m, p, new_values);
 }
 
 //matrix addition function
-Matrix mAdd(Matrix A, Matrix B)
+Matrix Matrix::operator+(Matrix B)
 {
-    const int m_a=A.rows;
+    const int m_a=rows;
     const int n_b=B.columns;
-    const int n_a=A.columns;
+    const int n_a=columns;
     const int m_b=B.rows;
-    float * values;
+    float * new_values;
     if ((m_a==m_b)&&(n_a==n_b))
     {
-        values=m_add_helper(A.values, B.values, m_a*n_a);
+        new_values=m_add_helper(values, B.values, m_a*n_a);
     }
-    return Matrix(m_a, n_a, values);
+    return Matrix(m_a, n_a, new_values);
 }
 
 //scalar multiplication function
-Matrix sMult(Matrix A, float k)
+Matrix Matrix :: scale(float k)
 {
-    float * values;
-    values=s_mult_helper(A.values, k, A.rows*A.columns);
-    return Matrix(A.rows, A.columns, values);
+    float * new_values;
+    new_values=s_mult_helper(values, k, rows*columns);
+    return Matrix(rows, columns, new_values);
 }
 
 //matrix subtraction function
-Matrix mSub(Matrix A, Matrix B)
+Matrix Matrix::operator-(Matrix B)
 {
-    const int m_a=A.rows;
+    const int m_a=rows;
     const int n_b=B.columns;
-    const int n_a=A.columns;
+    const int n_a=columns;
     const int m_b=B.rows;
-    float * values;
+    float * new_values;
     if ((m_a==m_b)&&(n_a==n_b))
     {
-        values=m_sub_helper(A.values, B.values, m_a*n_a);
+        new_values=m_sub_helper(values, B.values, m_a*n_a);
     }
-    return Matrix(m_a, n_a, values);
+    return Matrix(m_a, n_a, new_values);
 }
 
 void display_matrix(Matrix A)
diff --git a/main/matrix.hh b/main/matrix.hh
index 4b94bae..ac752e6 100644
--- a/main/matrix.hh
+++ b/main/matrix.hh
@@ -23,12 +23,16 @@ class Matrix
         {
             return values[(row-1)*columns+(column-1)];
         }
+        Matrix operator+(Matrix);
+        Matrix operator-(Matrix);
+        Matrix scale (float k);
+        Matrix operator*(Matrix);
 };
 
-Matrix mMult(Matrix, Matrix);
-Matrix mAdd(Matrix, Matrix);
-Matrix sMult(Matrix, float);
-Matrix mSub(Matrix, Matrix);
+//Matrix mMult(Matrix, Matrix);
+//Matrix mAdd(Matrix, Matrix);
+//Matrix sMult(Matrix, float);
+//Matrix mSub(Matrix, Matrix);
 void display_matrix(Matrix);
 
 #endif  // __MODING_HH__
\ No newline at end of file
diff --git a/main/mission_constants.hh b/main/mission_constants.hh
index 56d87a0..9c3292a 100644
--- a/main/mission_constants.hh
+++ b/main/mission_constants.hh
@@ -111,8 +111,8 @@ float A_VALUES [36] =   {0,0,0,0,0,0,
                         0,0,0,0,0,0,
                         0,0,0,0,0,0,
                         0,0,0,0,0,0,
-                        0,0,0,0,0,0,
                         0,0,0,0,0,0}; //dynamics matrix
+Matrix A=Matrix(6, 6, A_VALUES);        
  
  float B_VALUES [12] =  {0, 0, 
                         0, 0,
@@ -120,19 +120,25 @@ float A_VALUES [36] =   {0,0,0,0,0,0,
                         0, 0,
                         0, 0,
                         0, 0}; //input matrix
+Matrix B=Matrix(6, 2, B_VALUES);
  
  float K_VALUES [12] =  {0,0,0,0,0,0,
                         0,0,0,0,0,0}; //controller gain
+Matrix K=Matrix(2, 6, K_VALUES);
 
  float C_VALUES [24] =  {0,0,0,0,0,0,
                         0,0,0,0,0,0,
                         0,0,0,0,0,0,
                         0,0,0,0,0,0}; //sensor matrix
+Matrix C=Matrix(4, 6, C_VALUES);
  
  float L_VALUES [24] =  {0, 0, 0, 0,
+                        0, 0, 0, 0,
+                        0, 0, 0, 0,
                         0, 0, 0, 0,
                         0, 0, 0, 0,
                         0, 0, 0, 0}; //kalman gain
+Matrix L=Matrix(6, 4, L_VALUES);
 
 
 #endif  // __MISSION_CONSTANTS_HH__
\ No newline at end of file