Ported UpdateEKF

LinearAlgebra/Matrix.cs

@@ -1,3 +1,4 @@
+using System;
 using System.Diagnostics;
 using Vector3 = UnityEngine.Vector3;
 
@@ -42,6 +43,19 @@ public class Matrix2 {
         // double checked code
     }
 
+    public static Matrix2 operator -(Matrix2 A, Matrix2 B) {
+        if (A.nRows != B.nRows || A.nCols != B.nCols)
+            throw new System.ArgumentException("Size of A must match size of B.");
+
+        float[,] result = new float[A.nRows, B.nCols];
+
+        for (int i = 0; i < A.nRows; i++) {
+            for (int j = 0; j < A.nCols; j++)
+                result[i, j] = A.data[i, j] - B.data[i, j];
+        }
+        return new Matrix2(result);
+    }
+
     public static Matrix2 operator +(Matrix2 A, Matrix2 B) {
         if (A.nRows != B.nRows || A.nCols != B.nCols)
             throw new System.ArgumentException("Size of A must match size of B.");
@@ -49,9 +63,8 @@ public class Matrix2 {
         float[,] result = new float[A.nRows, B.nCols];
 
         for (int i = 0; i < A.nRows; i++) {
-            for (int j = 0; j < A.nCols; j++) {
+            for (int j = 0; j < A.nCols; j++)
                 result[i, j] = A.data[i, j] + B.data[i, j];
-            }
         }
         return new Matrix2(result);
     }
@@ -120,6 +133,59 @@ public class Matrix2 {
 
         return new Matrix2(result);
     }
+
+    public Matrix2 Inverse() {
+        Matrix2 A = this;
+        // unchecked
+        int n = A.nRows;
+
+        // Create an identity matrix of the same size as the original matrix
+        float[,] augmentedMatrix = new float[n, 2 * n];
+        for (int i = 0; i < n; i++) {
+            for (int j = 0; j < n; j++) {
+                augmentedMatrix[i, j] = A.data[i, j];
+                augmentedMatrix[i, j + n] = (i == j) ? 1 : 0;  // Identity matrix
+            }
+        }
+
+        // Perform Gaussian elimination
+        for (int i = 0; i < n; i++) {
+            // Find the pivot row
+            float pivot = augmentedMatrix[i, i];
+            if (Math.Abs(pivot) < 1e-10) // Check for singular matrix
+                throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
+
+            // Normalize the pivot row
+            for (int j = 0; j < 2 * n; j++)
+                augmentedMatrix[i, j] /= pivot;
+
+            // Eliminate the column below the pivot
+            for (int j = i + 1; j < n; j++) {
+                float factor = augmentedMatrix[j, i];
+                for (int k = 0; k < 2 * n; k++)
+                    augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
+            }
+        }
+
+        // Back substitution
+        for (int i = n - 1; i >= 0; i--) {
+            // Eliminate the column above the pivot
+            for (int j = i - 1; j >= 0; j--) {
+                float factor = augmentedMatrix[j, i];
+                for (int k = 0; k < 2 * n; k++)
+                    augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
+            }
+        }
+
+        // Extract the inverse matrix from the augmented matrix
+        float[,] inverse = new float[n, n];
+        for (int i = 0; i < n; i++) {
+            for (int j = 0; j < n; j++)
+                inverse[i, j] = augmentedMatrix[i, j + n];
+        }
+
+        return new Matrix2(inverse);
+    }
 }
 
 public class Matrix1 {
@@ -127,10 +193,26 @@ public class Matrix1 {
 
     public int magnitude => data.GetLength(0);
 
+    public Matrix1(uint magnitude) {
+        this.data = new float[magnitude];
+    }
+
     public Matrix1(float[] data) {
         this.data = data;
     }
 
+    public static Matrix1 Zero(uint magnitude) {
+        return new Matrix1(magnitude);
+    }
+
+    public Vector3 vector3 {
+        get {
+            if (this.magnitude != 3)
+                throw new System.ArgumentException("Matrix1 must be of size 3");
+            return new Vector3(this.data[0], this.data[1], this.data[2]);
+        }
+    }
+
     public Matrix2 Transpose() {
         float[,] r = new float[1, this.magnitude];
         for (uint colIx = 0; colIx < this.magnitude; colIx++)
@@ -150,6 +232,18 @@ public class Matrix1 {
         return result;
     }
 
+    public static Matrix1 operator *(Matrix1 A, float f) {
+        float[] result = new float[A.magnitude];
+
+        for (int i = 0; i < A.magnitude; i++)
+                result[i] += A.data[i] * f;
+        
+        return new Matrix1(result);
+    }
+    public static Matrix1 operator *(float f, Matrix1 A) {
+        return A * f;
+    }
+
     public Matrix1 Slice(int from, int to) {
         if (from < 0 || to >= this.magnitude)
             throw new System.ArgumentException("Slice index out of range.");