From b991153b8b955313d928b31860cf7fa1825dbd6a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 12 Feb 2025 11:38:15 +0100 Subject: [PATCH 1/5] Add ChiSquareTest --- LinearAlgebra/Matrix.cs | 122 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 122 insertions(+) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index 60d260b..1960109 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -1,5 +1,127 @@ +using System.Diagnostics; using Vector3 = UnityEngine.Vector3; +public class Matrix2 { + public float[,] data { get; } + + public int nRows => data.GetLength(0); + public int nCols => data.GetLength(1); + + public Matrix2(float[,] data) { + this.data = data; + } + + public static Matrix2 Identity(int size) { + float[,] I = new float[size, size]; + for (int i = 0; i < size; i++) I[i, i] = 1.0f; + return new Matrix2(I); + } + + public Matrix2 Transpose() { + float[,] r = new float[this.nCols, this.nRows]; + for (uint rowIx = 0; rowIx < this.nRows; rowIx++) { + for (uint colIx = 0; colIx < this.nCols; colIx++) + r[colIx, rowIx] = this.data[rowIx, colIx]; + } + return new Matrix2(r); + // 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.nCols != B.nRows) + throw new System.ArgumentException("Number of columns in A must match number of rows in B."); + + float[,] result = new float[A.nRows, B.nCols]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < B.nCols; j++) { + float sum = 0.0f; + for (int k = 0; k < A.nCols; k++) + sum += A.data[i, k] * B.data[k, j]; + + result[i, j] = sum; + } + } + + return new Matrix2(result); + // double checked code + } + + public static Matrix1 operator *(Matrix2 A, Matrix1 v) { + // int rows = A.GetLength(0); + // int cols = A.GetLength(1); + float[] result = new float[A.nRows]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) { + result[i] += A.data[i, j] * v.data[j]; + } + } + + return new Matrix1(result); + } + + + public static Matrix2 operator *(Matrix2 A, float s) { + float[,] result = new float[A.nRows, A.nCols]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) + result[i, j] += A.data[i, j] * s; + } + + return new Matrix2(result); + } + public static Matrix2 operator *(float scalar, Matrix2 A) { + return A * scalar; + } + +} + +public class Matrix1 { + public float[] data { get; } + + public int size => data.GetLength(0); + + public Matrix1(float[] data) { + this.data = data; + } + + public Matrix2 Transpose() { + float[,] r = new float[1, this.size]; + for (uint colIx = 0; colIx < this.size; colIx++) + r[1, colIx] = this.data[colIx]; + + return new Matrix2(r); + } + + public static float Dot(Matrix1 a, Matrix1 b) { + if (a.size != b.size) + throw new System.ArgumentException("Vectors must be of the same length."); + + float result = 0.0f; + for (int i = 0; i < a.size; i++) { + result += a.data[i] * b.data[i]; + } + return result; + } + +} + public class Matrix { private readonly uint rows = 0; private readonly uint cols = 0; From d337fba6fdf19b6b8aa0e1f1e8208cddae4e5c74 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 12 Feb 2025 11:57:46 +0100 Subject: [PATCH 2/5] completed UpdateWithGoodIds --- LinearAlgebra/Matrix.cs | 63 ++++++++++++++++++++++++++++++++++------- 1 file changed, 52 insertions(+), 11 deletions(-) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index 1960109..2a05773 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -12,18 +12,33 @@ public class Matrix2 { } public static Matrix2 Identity(int size) { - float[,] I = new float[size, size]; - for (int i = 0; i < size; i++) I[i, i] = 1.0f; - return new Matrix2(I); + return Diagonal(1, size); + // float[,] resultData = new float[size, size]; + // for (int i = 0; i < size; i++) + // resultData[i, i] = 1.0f; + // return new Matrix2(resultData); + } + + public static Matrix2 Diagonal(Matrix1 v) { + float[,] resultData = new float[v.magnitude, v.magnitude]; + for (int ix = 0; ix < v.magnitude; ix++) + resultData[ix, ix] = v.data[ix]; + return new Matrix2(resultData); + } + public static Matrix2 Diagonal(float f, int size) { + float[,] resultData = new float[size, size]; + for (int ix = 0; ix < size; ix++) + resultData[ix, ix] = f; + return new Matrix2(resultData); } public Matrix2 Transpose() { - float[,] r = new float[this.nCols, this.nRows]; + float[,] resultData = new float[this.nCols, this.nRows]; for (uint rowIx = 0; rowIx < this.nRows; rowIx++) { for (uint colIx = 0; colIx < this.nCols; colIx++) - r[colIx, rowIx] = this.data[rowIx, colIx]; + resultData[colIx, rowIx] = this.data[rowIx, colIx]; } - return new Matrix2(r); + return new Matrix2(resultData); // double checked code } @@ -90,36 +105,62 @@ public class Matrix2 { return A * scalar; } + public Matrix2 Slice(int from, int to) { + if (from < 0 || to >= this.nRows) + throw new System.ArgumentException("Slice index out of range."); + + float[,] result = new float[to - from, this.nCols]; + int resultRowIx = 0; + for (int rowIx = from; rowIx < to; rowIx++) { + for (int colIx = 0; colIx < this.nCols; colIx++) { + result[resultRowIx, colIx] = this.data[rowIx, colIx]; + } + resultRowIx++; + } + + return new Matrix2(result); + } } public class Matrix1 { public float[] data { get; } - public int size => data.GetLength(0); + public int magnitude => data.GetLength(0); public Matrix1(float[] data) { this.data = data; } public Matrix2 Transpose() { - float[,] r = new float[1, this.size]; - for (uint colIx = 0; colIx < this.size; colIx++) + float[,] r = new float[1, this.magnitude]; + for (uint colIx = 0; colIx < this.magnitude; colIx++) r[1, colIx] = this.data[colIx]; return new Matrix2(r); } public static float Dot(Matrix1 a, Matrix1 b) { - if (a.size != b.size) + if (a.magnitude != b.magnitude) throw new System.ArgumentException("Vectors must be of the same length."); float result = 0.0f; - for (int i = 0; i < a.size; i++) { + for (int i = 0; i < a.magnitude; i++) { result += a.data[i] * b.data[i]; } return result; } + public Matrix1 Slice(int from, int to) { + if (from < 0 || to >= this.magnitude) + throw new System.ArgumentException("Slice index out of range."); + + float[] result = new float[to - from]; + int resultIx = 0; + for (int ix = from; ix < to; ix++) + result[resultIx++] = this.data[ix]; + + return new Matrix1(result); + } } public class Matrix { From 8ea28beb42412bf15bc9634e1ee0804b603a13c6 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 12 Feb 2025 12:50:53 +0100 Subject: [PATCH 3/5] Ported UpdateEKF --- LinearAlgebra/Matrix.cs | 98 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 96 insertions(+), 2 deletions(-) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index 2a05773..b7dd7c3 100644 --- a/LinearAlgebra/Matrix.cs +++ b/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."); From dd23355554624c15f36c7ad1ad5efe2ab7cdc9ed Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 14 Feb 2025 11:07:30 +0100 Subject: [PATCH 4/5] Residual & Jacobian progress --- LinearAlgebra/Matrix.cs | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index b7dd7c3..9c2e84e 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -5,14 +5,21 @@ using Vector3 = UnityEngine.Vector3; public class Matrix2 { public float[,] data { get; } - public int nRows => data.GetLength(0); - public int nCols => data.GetLength(1); + public uint nRows => (uint)data.GetLength(0); + public uint nCols => (uint)data.GetLength(1); + public Matrix2(uint nRows, uint nCols) { + this.data = new float[nRows, nCols]; + } public Matrix2(float[,] data) { this.data = data; } - public static Matrix2 Identity(int size) { + public static Matrix2 Zero(uint nRows, uint nCols) { + return new Matrix2(nRows, nCols); + } + + public static Matrix2 Identity(uint size) { return Diagonal(1, size); // float[,] resultData = new float[size, size]; // for (int i = 0; i < size; i++) @@ -26,7 +33,7 @@ public class Matrix2 { resultData[ix, ix] = v.data[ix]; return new Matrix2(resultData); } - public static Matrix2 Diagonal(float f, int size) { + public static Matrix2 Diagonal(float f, uint size) { float[,] resultData = new float[size, size]; for (int ix = 0; ix < size; ix++) resultData[ix, ix] = f; @@ -137,7 +144,7 @@ public class Matrix2 { public Matrix2 Inverse() { Matrix2 A = this; // unchecked - int n = A.nRows; + uint n = A.nRows; // Create an identity matrix of the same size as the original matrix float[,] augmentedMatrix = new float[n, 2 * n]; @@ -168,9 +175,9 @@ public class Matrix2 { } // Back substitution - for (int i = n - 1; i >= 0; i--) { + for (uint i = n - 1; i >= 0; i--) { // Eliminate the column above the pivot - for (int j = i - 1; j >= 0; j--) { + for (uint 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]; @@ -191,7 +198,7 @@ public class Matrix2 { public class Matrix1 { public float[] data { get; } - public int magnitude => data.GetLength(0); + public uint magnitude => (uint)data.GetLength(0); public Matrix1(uint magnitude) { this.data = new float[magnitude]; From 8dab67f620ebd8597b633cae4ce55b3a98f6a922 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 14 Feb 2025 12:29:38 +0100 Subject: [PATCH 5/5] completed ComputeResidualAndJacobian --- LinearAlgebra/Matrix.cs | 89 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 85 insertions(+), 4 deletions(-) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index 9c2e84e..c2e6334 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -1,6 +1,16 @@ using System; using System.Diagnostics; using Vector3 = UnityEngine.Vector3; +using Vector2 = UnityEngine.Vector2; + +public readonly struct Slice { + public int start {get;} + public int stop {get;} + public Slice(int start, int stop) { + this.start = start; + this.stop = stop; + } +} public class Matrix2 { public float[,] data { get; } @@ -40,6 +50,15 @@ public class Matrix2 { return new Matrix2(resultData); } + public static Matrix2 SkewMatrix(Vector3 v) { + float[,] result = new float[3, 3] { + {0, -v.z, v.y}, + {v.z, 0, -v.x}, + {-v.y, v.x, 0} + }; + return new Matrix2(result); + } + public Matrix2 Transpose() { float[,] resultData = new float[this.nCols, this.nRows]; for (uint rowIx = 0; rowIx < this.nRows; rowIx++) { @@ -50,6 +69,16 @@ public class Matrix2 { // double checked code } + public static Matrix2 operator -(Matrix2 m) { + float[,] result = new float[m.nRows, m.nCols]; + + for (int i = 0; i < m.nRows; i++) { + for (int j = 0; j < m.nCols; j++) + result[i, j] = -m.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."); @@ -97,8 +126,6 @@ public class Matrix2 { } public static Matrix1 operator *(Matrix2 A, Matrix1 v) { - // int rows = A.GetLength(0); - // int cols = A.GetLength(1); float[] result = new float[A.nRows]; for (int i = 0; i < A.nRows; i++) { @@ -110,6 +137,14 @@ public class Matrix2 { return new Matrix1(result); } + public static Vector3 operator *(Matrix2 A, Vector3 v) { + return new Vector3() { + x = A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z, + y = A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z, + z = A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z + }; + } + public static Matrix2 operator *(Matrix2 A, float s) { float[,] result = new float[A.nRows, A.nCols]; @@ -125,6 +160,9 @@ public class Matrix2 { return A * scalar; } + public Matrix2 Slice(Slice slice) { + return Slice(slice.start, slice.stop); + } public Matrix2 Slice(int from, int to) { if (from < 0 || to >= this.nRows) throw new System.ArgumentException("Slice index out of range."); @@ -140,6 +178,19 @@ public class Matrix2 { return new Matrix2(result); } + public void UpdateSlice(Slice slice, Matrix2 m) { + int mRowIx = 0; + for (int rowIx = slice.start; rowIx < slice.stop; rowIx++) { + for (int colIx = 0; colIx < this.nCols; colIx++) + this.data[rowIx, colIx] = m.data[mRowIx, colIx]; + } + } + public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m) { + for (int i = rowRange.start; i < rowRange.stop; i++) { + for (int j = colRange.start; j < colRange.stop; j++) + this.data[i, j] = m.data[i - rowRange.start, j - colRange.stop]; + } + } public Matrix2 Inverse() { Matrix2 A = this; @@ -212,6 +263,28 @@ public class Matrix1 { return new Matrix1(magnitude); } + public static Matrix1 FromVector2(Vector2 v) { + float[] result = new float[2]; + result[0] = v.x; + result[1] = v.y; + return new Matrix1(result); + } + + public static Matrix1 FromVector3(Vector3 v) { + float[] result = new float[3]; + result[0] = v.x; + result[1] = v.y; + result[2] = v.z; + return new Matrix1(result); + } + + public Vector2 vector2 { + get { + if (this.magnitude != 2) + throw new System.ArgumentException("Matrix1 must be of size 2"); + return new Vector2(this.data[0], this.data[1]); + } + } public Vector3 vector3 { get { if (this.magnitude != 3) @@ -243,14 +316,17 @@ public class Matrix1 { float[] result = new float[A.magnitude]; for (int i = 0; i < A.magnitude; i++) - result[i] += A.data[i] * f; - + result[i] += A.data[i] * f; + return new Matrix1(result); } public static Matrix1 operator *(float f, Matrix1 A) { return A * f; } + public Matrix1 Slice(Slice range) { + return Slice(range.start, range.stop); + } public Matrix1 Slice(int from, int to) { if (from < 0 || to >= this.magnitude) throw new System.ArgumentException("Slice index out of range."); @@ -262,6 +338,11 @@ public class Matrix1 { return new Matrix1(result); } + public void UpdateSlice(Slice slice, Matrix1 v) { + int vIx = 0; + for (int ix = slice.start; ix < slice.stop; ix++, vIx++) + this.data[ix] = v.data[vIx]; + } } public class Matrix {