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));
}
