QUYVN/Denso
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:31:37 +07:00
commit 899c7c637d
1939 changed files with 641750 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

236
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Motion/MatrixHelper.cs Normal file
View File

@@ -0,0 +1,236 @@
namespace RobotNet10.RobotApp.Motion;
/// <summary>
/// Helper class for matrix operations used in Extended Kalman Filter
/// Implements basic operations for small matrices (up to 6x6)
/// </summary>
public static class MatrixHelper
{
/// <summary>
/// Create identity matrix of size n x n
/// </summary>
public static double[,] CreateIdentity(int n)
{
var result = new double[n, n];
for (int i = 0; i < n; i++)
result[i, i] = 1.0;
return result;
}
/// <summary>
/// Create zero matrix of size rows x cols
/// </summary>
public static double[,] CreateZeros(int rows, int cols)
{
return new double[rows, cols];
}
/// <summary>
/// Create diagonal matrix from array of diagonal values
/// </summary>
public static double[,] CreateDiagonal(double[] diagonal)
{
int n = diagonal.Length;
var result = new double[n, n];
for (int i = 0; i < n; i++)
result[i, i] = diagonal[i];
return result;
}
/// <summary>
/// Matrix multiplication: C = A * B
/// </summary>
public static double[,] Multiply(double[,] a, double[,] b)
{
int aRows = a.GetLength(0);
int aCols = a.GetLength(1);
int bRows = b.GetLength(0);
int bCols = b.GetLength(1);
if (aCols != bRows)
throw new ArgumentException($"Matrix dimensions incompatible for multiplication: ({aRows}x{aCols}) * ({bRows}x{bCols})");
var result = new double[aRows, bCols];
for (int i = 0; i < aRows; i++)
{
for (int j = 0; j < bCols; j++)
{
double sum = 0.0;
for (int k = 0; k < aCols; k++)
sum += a[i, k] * b[k, j];
result[i, j] = sum;
}
}
return result;
}
/// <summary>
/// Matrix addition: C = A + B
/// </summary>
public static double[,] Add(double[,] a, double[,] b)
{
int rows = a.GetLength(0);
int cols = a.GetLength(1);
if (rows != b.GetLength(0) || cols != b.GetLength(1))
throw new ArgumentException("Matrix dimensions must match for addition");
var result = new double[rows, cols];
for (int i = 0; i < rows; i++)
for (int j = 0; j < cols; j++)
result[i, j] = a[i, j] + b[i, j];
return result;
}
/// <summary>
/// Matrix subtraction: C = A - B
/// </summary>
public static double[,] Subtract(double[,] a, double[,] b)
{
int rows = a.GetLength(0);
int cols = a.GetLength(1);
if (rows != b.GetLength(0) || cols != b.GetLength(1))
throw new ArgumentException("Matrix dimensions must match for subtraction");
var result = new double[rows, cols];
for (int i = 0; i < rows; i++)
for (int j = 0; j < cols; j++)
result[i, j] = a[i, j] - b[i, j];
return result;
}
/// <summary>
/// Matrix transpose: B = A^T
/// </summary>
public static double[,] Transpose(double[,] a)
{
int rows = a.GetLength(0);
int cols = a.GetLength(1);
var result = new double[cols, rows];
for (int i = 0; i < rows; i++)
for (int j = 0; j < cols; j++)
result[j, i] = a[i, j];
return result;
}
/// <summary>
/// Matrix scalar multiplication: B = scalar * A
/// </summary>
public static double[,] ScalarMultiply(double scalar, double[,] a)
{
int rows = a.GetLength(0);
int cols = a.GetLength(1);
var result = new double[rows, cols];
for (int i = 0; i < rows; i++)
for (int j = 0; j < cols; j++)
result[i, j] = scalar * a[i, j];
return result;
}
/// <summary>
/// Matrix inverse using Gauss-Jordan elimination (for small matrices)
/// </summary>
public static double[,] Inverse(double[,] a)
{
int n = a.GetLength(0);
if (n != a.GetLength(1))
throw new ArgumentException("Matrix must be square for inversion");
// Create augmented matrix [A | I]
var augmented = new double[n, 2 * n];
for (int i = 0; i < n; i++)
{
for (int j = 0; j < n; j++)
augmented[i, j] = a[i, j];
augmented[i, n + i] = 1.0;
}
// Gauss-Jordan elimination
for (int i = 0; i < n; i++)
{
// Find pivot
int maxRow = i;
for (int k = i + 1; k < n; k++)
{
if (Math.Abs(augmented[k, i]) > Math.Abs(augmented[maxRow, i]))
maxRow = k;
}
// Swap rows
if (maxRow != i)
{
for (int k = 0; k < 2 * n; k++)
(augmented[i, k], augmented[maxRow, k]) = (augmented[maxRow, k], augmented[i, k]);
}
// Check for singular matrix
if (Math.Abs(augmented[i, i]) < 1e-10)
throw new InvalidOperationException("Matrix is singular and cannot be inverted");
// Scale pivot row
double pivot = augmented[i, i];
for (int j = 0; j < 2 * n; j++)
augmented[i, j] /= pivot;
// Eliminate column
for (int k = 0; k < n; k++)
{
if (k != i)
{
double factor = augmented[k, i];
for (int j = 0; j < 2 * n; j++)
augmented[k, j] -= factor * augmented[i, j];
}
}
}
// Extract inverse from augmented matrix
var result = new double[n, n];
for (int i = 0; i < n; i++)
for (int j = 0; j < n; j++)
result[i, j] = augmented[i, n + j];
return result;
}
/// <summary>
/// Copy matrix
/// </summary>
public static double[,] Copy(double[,] a)
{
int rows = a.GetLength(0);
int cols = a.GetLength(1);
var result = new double[rows, cols];
for (int i = 0; i < rows; i++)
for (int j = 0; j < cols; j++)
result[i, j] = a[i, j];
return result;
}
/// <summary>
/// Print matrix (for debugging)
/// </summary>
public static string ToString(double[,] a, string format = "F4")
{
int rows = a.GetLength(0);
int cols = a.GetLength(1);
var sb = new System.Text.StringBuilder();
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
sb.Append(a[i, j].ToString(format));
if (j < cols - 1)
sb.Append(" ");
}
if (i < rows - 1)
sb.AppendLine();
}
return sb.ToString();
}
}