Matrix(CSharp).rar_CSharp 滤波_Matrix 卡尔曼_c#中Matrix_csharp matrix_

using System; using System.Collections.Generic; using System.Linq; using System.Text; namespace 平差 { class Matrix { public static double[,] Plus(double[,] Matrix_A, double[,] Matrix_B, int row, int col) //矩阵加法 { double[,] dMatrix_plus = new double[row, col]; for (int i = 0; i < row; i++) { for (int j = 0; j < col; j++) { dMatrix_plus[i, j] = Matrix_A[i, j] + Matrix_B[i, j]; } } return dMatrix_plus; } public static double[,] Multiply(double[,] Matrix_A, double[,] Matrix_B, int A_row, int common, int B_col) //矩阵乘法 { double[,] dMatrix_multiply = new double[A_row, B_col]; for (int i = 0; i < A_row; i++) { for (int j = 0; j < B_col; j++) { for (int k = 0; k < common; k++) { dMatrix_multiply[i, j] += Matrix_A[i, k] * Matrix_B[k, j]; } } } return dMatrix_multiply; } public static double[,] Transfer(double[,] Matrix_A, int row, int col) //矩阵转置 { double[,] dMatrix_transfer = new double[col, row]; for (int i = 0; i < col; i++) { for (int j = 0; j < row; j++) { dMatrix_transfer[i, j] = Matrix_A[j, i]; } } return dMatrix_transfer; } public static double Value_hl(double[,] MatrixList, int level) //矩阵行列式 { double[,] dMatrix = new double[level, level]; for (int i = 0; i < level; i++) { for (int j = 0; j < level; j++) { dMatrix[i, j] = MatrixList[i, j]; } } double c, x; int k = 1; for (int i = 0, j = 0; i < level && j < level; i++, j++) { if (dMatrix[i, j] == 0) { int m = i; for (; dMatrix[m, j] == 0; m++) ; if (m == level) { return 0; } else { for (int n = j; n < level; n++) { c = dMatrix[i, n]; dMatrix[i, n] = dMatrix[m, n]; dMatrix[m, n] = c; } k *= (-1); } } for (int s = level - 1; s > i; s--) { x = dMatrix[s, j]; for (int t = j; t < level; t++) { dMatrix[s, t] -= dMatrix[i, t] * (x / dMatrix[i, j]); } } } double sn = 1; for (int i = 0; i < level; i++) { if (dMatrix[i, i] != 0) { sn *= dMatrix[i, i]; } else { return (0); } } return k * sn; } public static double[,] Reverse(double[,] dMatrix, int Level) //矩阵求逆 { double dMatrixValue = Value_hl(dMatrix, Level); if (dMatrixValue == 0) { return null; } double[,] dReverseMatrix = new double[Level, 2 * Level]; double x, c; for (int i = 0; i < Level; i++) { for (int j = 0; j < 2 * Level; j++) { if (j < Level) { dReverseMatrix[i, j] = dMatrix[i, j]; } else { dReverseMatrix[i, j] = 0; } } dReverseMatrix[i, Level + i] = 1; } for (int i = 0, j = 0; i < Level && j < Level; i++, j++) { if (dReverseMatrix[i, j] == 0) { int m = i; for (; dMatrix[m, j] == 0; m++) ; if (m == Level) { return null; } else { for (int n = j; n < 2 * Level; n++) { dReverseMatrix[i, n] += dReverseMatrix[m, n]; } } } x = dReverseMatrix[i, j]; if (x != 1) { for (int n = j; n < 2 * Level; n++) { if (dReverseMatrix[i, n] != 0) { dReverseMatrix[i, n] /= x; } } } for (int s = Level - 1; s > i; s--) { x = dReverseMatrix[s, j]; for (int t = j; t < 2 * Level; t++) { dReverseMatrix[s, t] -= (dReverseMatrix[i, t] * x); } } } for (int i = Level - 2; i >= 0; i--) { for (int j = i + 1; j < Level; j++) { if (dReverseMatrix[i, j] != 0) { c = dReverseMatrix[i, j]; for (int n = j; n < 2 * Level; n++) { dReverseMatrix[i, n] -= (c * dReverseMatrix[j, n]); } } } } double[,] dReturn = new double[Level, Level]; for (int i = 0; i < Level; i++) { for (int j = 0; j < Level; j++) { dReturn[i, j] = dReverseMatrix[i, j + Level]; } } return dReturn; } }