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