/*
* 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 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 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"));
}