public void SetCol(MatrixS v, int k) { for (int i = 0; i < rows; i++) { mat[i, k] = v[i, 0]; } }
public static MatrixS Parse(string ps) // Function parses the matrix from string { string s = NormalizeMatrixString(ps); string[] rows = Regex.Split(s, "\r\n"); string[] nums = rows[0].Split(' '); MatrixS matrix = new MatrixS(rows.Length, nums.Length); try { for (int i = 0; i < rows.Length; i++) { string rws = rows[i]; rws = rws.Replace('.', ','); nums = rws.Split(' '); for (int j = 0; j < nums.Length; j++) { matrix[i, j] = double.Parse(nums[j]); } } } //FormatException exc catch (FormatException exc) { throw new MException("Wrong input format!"); } return(matrix); }
private void Update() { //Update K //K = P*Ht/(H*P*Ht + R); MatrixS Ht = MatrixS.Transpose(H); MatrixS PHt = P * Ht; MatrixS HPHt = H * P * Ht; MatrixS HPHtR = HPHt + R; MatrixS HRInv = HPHtR.Invert(); MatrixS K = PHt * HRInv; //Update X //Kalman Gain //measurement update correction //X = X0 + K*(D(data) - H*X0); MatrixS HX0 = H * X0; MatrixS DHX0 = D - HX0; MatrixS KDHX0 = K * DHX0; X = X0 + KDHX0; //zdmzc z MatrixS KH = K * H; MatrixS IMinus = I - KH; P = IMinus * P; }
/* * State = data; * //time update prediction * X0 = F*State; * P0 = F*Covariance*F + Q; * //measurement update correction * double K = H*P0/(H*P0*H + R); * State = X0 + K*(data - H*X0); * Covariance = (1 - K * H) * P0; * return State; */ /* * sName = str; * I = MatrixS.Parse("1 0 0\r\n0 1 0\r\n0 0 1"); * //Q = MatrixS.Parse("0,01 0 0\r\n0 0,01 0\r\n0 0 0,01"); * //Q = MatrixS.Parse("0,5 0 0\r\n0 0,5 0\r\n0 0 0,5"); * if (isX) Q = MatrixS.Parse("1000 0 0\r\n0 1000 0\r\n0 0 1000"); // measurement noise ковариация шума процесса для X * else Q = MatrixS.Parse("5500 0 0\r\n0 5500 0\r\n0 0 5500"); // measurement noise ковариация шума процесса для Y * //else Q = MatrixS.Parse("0,1 0 0\r\n0 0,1 0\r\n0 0 0,1"); * // Q = MatrixS.Parse("17 0 0\r\n0 17 0\r\n0 0 17"); // measurement noise ковариация шума процесса * R = MatrixS.Parse("3300 0 0\r\n0 3300 0\r\n0 0 3300"); //ковариация шума измерения * //R = MatrixS.Parse("0,5 0 0\r\n0 0,5 0\r\n0 0 0,5"); * F = MatrixS.Parse("1 1 0.5\r\n0 1 1\r\n0 0 1"); * // F = MatrixS.Parse("1 1 0.5\r\n1 1 0.5\r\n0 0 1"); * H = MatrixS.Parse("1 0 0 0\r\n0 1 0 0\r\n0 0 1 0\r\n0 0 0 1"); * // factor of measured value to real value //отношение измерений и состояний * X0 = MatrixS.Parse("1\r\n1\r\n1\r\n1"); //Предсказание состояния системы * D = MatrixS.Parse("1\r\n0\r\n0\r\n0"); //полученные данные * X = MatrixS.Parse("1\r\n0\r\n0"); //Уточненные данные * P = MatrixS.Parse("0,5 0 0\r\n0 0,5 0\r\n0 0 0,5"); //Предсказание ошибки ковариации * dat = MatrixS.Parse("0\r\n0\r\n0"); * // VData = MatrixS.Parse("1\r\n1\r\n1"); * // State = MatrixS.Parse("1\r\n1\r\n1"); * //Covariance = MatrixS.Parse("0.1 0 0\r\n0 0.1 0\r\n0 0 0.1"); * */ public Kalc3Dim(string str, bool isX) { sName = str; I = MatrixS.Parse("1 0 0 0 0\r\n0 1 0 0 0\r\n0 0 1 0 0\r\n0 0 0 1 0\r\n0 0 0 0 1");; //Q = MatrixS.Parse("0,01 0 0\r\n0 0,01 0\r\n0 0 0,01"); //Q = MatrixS.Parse("0,5 0 0\r\n0 0,5 0\r\n0 0 0,5"); if (isX) { Q = MatrixS.Parse("1000 0 0 0 0\r\n0 1000 0 0 0\r\n0 0 1000 0 0\r\n0 0 0 1000 0\r\n0 0 0 0 1000"); // measurement noise ковариация шума процесса для X } else { Q = MatrixS.Parse("5000 0 0 0 0\r\n0 5000 0 0 0\r\n0 0 5000 0 0\r\n0 0 0 5000 0\r\n0 0 0 0 5000"); // measurement noise ковариация шума процесса для Y } //else Q = MatrixS.Parse("0,1 0 0\r\n0 0,1 0\r\n0 0 0,1"); // Q = MatrixS.Parse("17 0 0\r\n0 17 0\r\n0 0 17"); // measurement noise ковариация шума процесса R = MatrixS.Parse("1000 0 0 0 0\r\n0 1000 0 0 0\r\n0 0 1000 0 0\r\n0 0 0 1000 0\r\n0 0 0 0 1000"); //ковариация шума измерения //R = MatrixS.Parse("0,5 0 0\r\n0 0,5 0\r\n0 0 0,5"); double radiusProjectionX = KalmanMatrixes.getRadiusProjectionInCurrentTime(0.35, 0.05, 15); double radiusProjectionY = KalmanMatrixes.getRadiusProjectionInCurrentTime(0.35, 0.05, 15); F = KalmanMatrixes.GetMatrixA(0.35, 0.05, 5, 1, radiusProjectionX, radiusProjectionY); // F = MatrixS.Parse("1 1 0.5\r\n1 1 0.5\r\n0 0 1"); H = MatrixS.Parse("1 0 0 0 0\r\n0 1 0 0 0\r\n0 0 1 0 0\r\n0 0 0 1 0\r\n0 0 0 0 1"); // factor of measured value to real value //отношение измерений и состояний X0 = MatrixS.Parse("1\r\n1\r\n1\r\n1\r\n1"); //Предсказание состояния системы D = KalmanMatrixes.GetMatrixD(2, 0.05, 10, radiusProjectionX, radiusProjectionY); X = D; //Уточненные данные //P = MatrixS.Parse("1 0 0 0 0\r\n0 1 0 0 0\r\n0 0 1 0 0\r\n0 0 0 1 0\r\n0 0 0 0 1"); //Предсказание ошибки ковариации P = I; //Предсказание ошибки ковариации dat = MatrixS.Parse("0\r\n0\r\n0\r\n0\r\n0"); // VData = MatrixS.Parse("1\r\n1\r\n1"); // State = MatrixS.Parse("1\r\n1\r\n1"); //Covariance = MatrixS.Parse("0.1 0 0\r\n0 0.1 0\r\n0 0 0.1"); }
public MatrixS SolveWith(MatrixS v) // Function solves Ax = v in confirmity with solution vector "v" { if (rows != cols) { throw new MException("The matrix is not square!"); } if (rows != v.rows) { throw new MException("Wrong number of results in solution vector!"); } if (L == null) { MakeLU(); } MatrixS b = new MatrixS(rows, 1); for (int i = 0; i < rows; i++) { b[i, 0] = v[pi[i], 0]; // switch two items in "v" due to permutation matrix } MatrixS z = SubsForth(L, b); MatrixS x = SubsBack(U, z); return(x); }
public static MatrixS GetMatrixD(double distantionX, double focalLength, double distantionY, double radiusSignXProectionEstimate, double radiusSignYProectionEstimate) { double x = getXInCurrentTime(distantionX, focalLength, distantionY); double y = focalLength; return(MatrixS.Parse(x + "\r\n" + y + "\r\n" + 1 / radiusSignXProectionEstimate + "\r\n" + 1 / radiusSignYProectionEstimate + "\r\n" + "1\r\n")); }
public static MatrixS IdentityMatrix(int iRows, int iCols) // Function generates the identity matrix { MatrixS matrix = ZeroMatrix(iRows, iCols); for (int i = 0; i < Math.Min(iRows, iCols); i++) { matrix[i, i] = 1; } return(matrix); }
private static void ACopytoC(MatrixS A, int xa, int ya, MatrixS C, int size) { for (int i = 0; i < size; i++) // rows { for (int j = 0; j < size; j++) { C[i, j] = A[ya + i, xa + j]; } } }
public MatrixS GetCol(int k) { MatrixS m = new MatrixS(rows, 1); for (int i = 0; i < rows; i++) { m[i, 0] = mat[i, k]; } return(m); }
private static void AminusBintoC(MatrixS A, int xa, int ya, MatrixS B, int xb, int yb, MatrixS C, int size) { for (int i = 0; i < size; i++) // rows { for (int j = 0; j < size; j++) { C[i, j] = A[ya + i, xa + j] - B[yb + i, xb + j]; } } }
public static MatrixS GetMatrixA(double radiusSign, double focalLength, double vehicleSpeed, double deltaT, double radiusSignXProectionEstimate, double radiusSignYProectionEstimate) { String firstElement = (focalLength * radiusSign / (focalLength * radiusSign - vehicleSpeed * deltaT * radiusSignXProectionEstimate)).ToString(); String secondElement = (focalLength * radiusSign / (focalLength * radiusSign - vehicleSpeed * deltaT * radiusSignYProectionEstimate)).ToString(); String thirdElement = (-(vehicleSpeed * deltaT / (focalLength * radiusSign))).ToString(); return(MatrixS.Parse(firstElement + " 0 0 0 0\r\n0 " + secondElement + " 0 0 0\r\n0 0 1 0 " + thirdElement + "\r\n0 0 0 1 " + thirdElement + "\r\n0 0 0 0 1")); }
public static MatrixS ZeroMatrix(int iRows, int iCols) // Function generates the zero matrix { MatrixS matrix = new MatrixS(iRows, iCols); for (int i = 0; i < iRows; i++) { for (int j = 0; j < iCols; j++) { matrix[i, j] = 0; } } return(matrix); }
public MatrixS Duplicate() // Function returns the copy of this matrix { MatrixS matrix = new MatrixS(rows, cols); for (int i = 0; i < rows; i++) { for (int j = 0; j < cols; j++) { matrix[i, j] = mat[i, j]; } } return(matrix); }
public static MatrixS Transpose(MatrixS m) // Matrix transpose, for any rectangular matrix { MatrixS t = new MatrixS(m.cols, m.rows); for (int i = 0; i < m.rows; i++) { for (int j = 0; j < m.cols; j++) { t[j, i] = m[i, j]; } } return(t); }
public static MatrixS Multiply(double n, MatrixS m) // Multiplication by constant n { MatrixS r = new MatrixS(m.rows, m.cols); for (int i = 0; i < m.rows; i++) { for (int j = 0; j < m.cols; j++) { r[i, j] = m[i, j] * n; } } return(r); }
private static void SafeACopytoC(MatrixS A, int xa, int ya, MatrixS C, int size) { for (int i = 0; i < size; i++) // rows { for (int j = 0; j < size; j++) // cols { C[i, j] = 0; if (xa + j < A.cols && ya + i < A.rows) { C[i, j] += A[ya + i, xa + j]; } } } }
public static MatrixS RandomMatrix(int iRows, int iCols, int dispersion) // Function generates the random matrix { Random random = new Random(); MatrixS matrix = new MatrixS(iRows, iCols); for (int i = 0; i < iRows; i++) { for (int j = 0; j < iCols; j++) { matrix[i, j] = random.Next(-dispersion, dispersion); } } return(matrix); }
private void Predict() { //X0 = F * X; //P' = F * P * F + Q; if (iIndex++ == 0) { X[0, 0] = D[0, 0]; } X0 = F * X; MatrixS Ft = MatrixS.Transpose(F); MatrixS FPFt = F * P * Ft; P = FPFt + Q; }
public MatrixS GetP() // Function returns permutation matrix "P" due to permutation vector "pi" { if (L == null) { MakeLU(); } MatrixS matrix = ZeroMatrix(rows, cols); for (int i = 0; i < rows; i++) { matrix[pi[i], i] = 1; } return(matrix); }
private static MatrixS Add(MatrixS m1, MatrixS m2) // Sčítání matic { if (m1.rows != m2.rows || m1.cols != m2.cols) { throw new MException("Matrices must have the same dimensions!"); } MatrixS r = new MatrixS(m1.rows, m1.cols); for (int i = 0; i < r.rows; i++) { for (int j = 0; j < r.cols; j++) { r[i, j] = m1[i, j] + m2[i, j]; } } return(r); }
private static void SafeAminusBintoC(MatrixS A, int xa, int ya, MatrixS B, int xb, int yb, MatrixS C, int size) { for (int i = 0; i < size; i++) // rows { for (int j = 0; j < size; j++) // cols { C[i, j] = 0; if (xa + j < A.cols && ya + i < A.rows) { C[i, j] += A[ya + i, xa + j]; } if (xb + j < B.cols && yb + i < B.rows) { C[i, j] -= B[yb + i, xb + j]; } } } }
public MatrixS Invert() // Function returns the inverted matrix { if (L == null) { MakeLU(); } MatrixS inv = new MatrixS(rows, cols); for (int i = 0; i < rows; i++) { MatrixS Ei = MatrixS.ZeroMatrix(rows, 1); Ei[i, 0] = 1; MatrixS col = SolveWith(Ei); inv.SetCol(col, i); } return(inv); }
private double det(MatrixS a) { double s = 0; if (a.cols == 2) { s = a[0, 0] * a[1, 1] - a[0, 1] * a[1, 0]; return(s); } if (a.cols != 3) { return(s); } var t1 = a[0, 0] * (a[1, 1] * a[2, 2] - a[1, 2] * a[2, 1]); var t2 = a[0, 1] * (a[1, 0] * a[2, 2] - a[1, 2] * a[2, 0]); var t3 = a[0, 2] * (a[1, 0] * a[2, 1] - a[1, 1] * a[2, 0]); s = t1 - t2 + t3; return(s); }
public static MatrixS SubsForth(MatrixS A, MatrixS b) // Function solves Ax = b for A as a lower triangular matrix { if (A.L == null) { A.MakeLU(); } int n = A.rows; MatrixS x = new MatrixS(n, 1); for (int i = 0; i < n; i++) { x[i, 0] = b[i, 0]; for (int j = 0; j < i; j++) { x[i, 0] -= A[i, j] * x[j, 0]; } x[i, 0] = x[i, 0] / A[i, i]; } return(x); }
public static MatrixS StupidMultiply(MatrixS m1, MatrixS m2) // Stupid matrix multiplication { if (m1.cols != m2.rows) { throw new MException("Wrong dimensions of matrix!"); } MatrixS result = ZeroMatrix(m1.rows, m2.cols); for (int i = 0; i < result.rows; i++) { for (int j = 0; j < result.cols; j++) { for (int k = 0; k < m1.cols; k++) { result[i, j] += m1[i, k] * m2[k, j]; } } } return(result); }
public static MatrixS SubsBack(MatrixS A, MatrixS b) // Function solves Ax = b for A as an upper triangular matrix { if (A.L == null) { A.MakeLU(); } int n = A.rows; MatrixS x = new MatrixS(n, 1); for (int i = n - 1; i > -1; i--) { x[i, 0] = b[i, 0]; for (int j = n - 1; j > i; j--) { x[i, 0] -= A[i, j] * x[j, 0]; } x[i, 0] = x[i, 0] / A[i, i]; } return(x); }
public static MatrixS Power(MatrixS m, int pow) // Power matrix to exponent { if (pow == 0) { return(IdentityMatrix(m.rows, m.cols)); } if (pow == 1) { return(m.Duplicate()); } if (pow == -1) { return(m.Invert()); } MatrixS x; if (pow < 0) { x = m.Invert(); pow *= -1; } else { x = m.Duplicate(); } MatrixS ret = IdentityMatrix(m.rows, m.cols); while (pow != 0) { if ((pow & 1) == 1) { ret *= x; } x *= x; pow >>= 1; } return(ret); }
private void матрицаToolStripMenuItem_Click(object sender, EventArgs e) { /*MatrixS A2 = new MatrixS(2, 2); * A2[0, 0] = 0; * A2[0, 1] = 1; * A2[1, 0] = 1; * A2[1, 1] = 1; * det(A2); * MatrixS A3 = MatrixS.Parse("0 0 1\r\n1 1 1\r\n4 2 1"); * ; * MatrixS b3 = MatrixS.Parse("3\r\n6\r\n11"); * MatrixS x3 = A3.SolveWith(b3); * // MatrixS A3Inv = A3.Invert(); * // det(A3); * MessageBox.Show("a = " + x3[0, 0].ToString() + " b = " + x3[1, 0].ToString() + " b = " + x3[2, 0].ToString()); * List<PointCoords> listKalman = new List<PointCoords>();*/ if (listSourcePC.Count <= 0) { return; } listMatrix3 = new List <PointCoords>(); var Num = listSourcePC[0].NumFrame - 1; var x3 = new MatrixS(3, 1); var y3 = new MatrixS(3, 1); var b3x = new MatrixS(3, 1); var b3y = new MatrixS(3, 1); for (var jc = 0; jc < listSourcePC.Count; jc++) { var pt = new Point(listSourcePC[jc].X, listSourcePC[jc].Y); //PointCoords ptC = new PointCoords(); var ptC = pt + listSourcePC[jc]; ; listMatrix3.Add(ptC); if (jc < 2) { if (jc == 1) { b3x[0, 0] = listSourcePC[0].X; b3x[1, 0] = listSourcePC[1].X; b3y[0, 0] = listSourcePC[0].Y; b3y[1, 0] = listSourcePC[1].Y; } continue; } if (jc > 2) { ptC.X = (int)(x3[0, 0] * 3.0 * 3.0 + x3[1, 0] * 3.0 + x3[2, 0]); ptC.Y = (int)(y3[0, 0] * 3.0 * 3.0 + y3[1, 0] * 3.0 + y3[2, 0]); b3x[0, 0] = b3x[1, 0]; b3x[1, 0] = b3x[2, 0]; b3y[0, 0] = b3y[1, 0]; b3y[1, 0] = b3y[2, 0]; } b3x[2, 0] = ptC.X; b3y[2, 0] = ptC.Y; var A3 = new MatrixS(3, 3); // t 0,1,2 y - listSourcePC[jc - 2] listSourcePC[jc - 1] listSourcePC[jc ] A3[0, 0] = 0; //x~2a A3[0, 1] = 0; //xb A3[0, 2] = 1; //c /// A3[1, 0] = 1; //x~2a A3[1, 1] = 1; //xb A3[1, 2] = 1; //c // A3[2, 0] = 4; //x~2a A3[2, 1] = 2; //xb A3[2, 2] = 1; //c if (listSourcePC[jc].X != -1) { b3x[2, 0] = listSourcePC[jc].X; b3y[2, 0] = listSourcePC[jc].Y; } x3 = A3.SolveWith(b3x); y3 = A3.SolveWith(b3y); ptC.col = Color.Moccasin; } }
public static MatrixS operator *(double n, MatrixS m) { return(MatrixS.Multiply(n, m)); }
public static MatrixS operator *(MatrixS m1, MatrixS m2) { return(MatrixS.StrassenMultiply(m1, m2)); }