public static void StateIntegration_AT(SINS_State SINSstate, Kalman_Vars KalmanVars, SINS_State SINSstate_OdoMod) { double[] fz = new double[3], Wz = new double[3], u = new double[3], tempV = new double[3], Wz_avg = new double[3]; double[] Vx_0 = new double[3], Vx_0_prev = new double[3]; Matrix AT_z_xi = new Matrix(3, 3); Matrix B_x_eta = new Matrix(3, 3); Matrix dAT = new Matrix(3, 3); Matrix D_x_z = new Matrix(3, 3); Matrix W_x_xi = new Matrix(3, 3); Matrix C_eta_xi = new Matrix(3, 3); Matrix Hat1 = new Matrix(3, 3); Matrix Hat2 = new Matrix(3, 3); Matrix E = Matrix.UnitMatrix(3); Matrix dMatrix = new Matrix(3, 3); double W_z_abs, Omega_x_abs, dlt, dlt2, Altitude, Altitude_prev, dh, dVx, dVy, dVh, Azimth; CopyMatrix(AT_z_xi, SINSstate.AT); CopyMatrix(B_x_eta, SINSstate.A_x0n); SINSstate.A_nxi = SimpleOperations.A_ne(SINSstate.Time, SINSstate.Longitude_Start); Altitude = SINSstate.Height; Altitude_prev = SINSstate.Altitude_prev; fz[1] = SINSstate.F_z[1]; fz[2] = SINSstate.F_z[2]; fz[0] = SINSstate.F_z[0]; Wz[1] = SINSstate.W_z[1]; Wz[2] = SINSstate.W_z[2]; Wz[0] = SINSstate.W_z[0]; // --- в обратной связи используется накопленные оценки ошибок ньютонометров в горизонте fz[0] -= SINSstate.Cumulative_KalmanErrorVector[(SINSstate.value_iMx_f0_12 + 0)]; fz[1] -= SINSstate.Cumulative_KalmanErrorVector[(SINSstate.value_iMx_f0_12 + 1)]; // --- в обратной связи используется накопленные оценка ошибки ньютонометра f_3 в вертикальном канале fz[2] -= SINSstate.Vertical_Cumulative_KalmanErrorVector[SINSstate.Vertical_f0_3]; // --- то же самое для ДУС, только все с горизонтального канала for (int i = 0; i < 3; i++) { Wz[i] += SINSstate.Cumulative_KalmanErrorVector[SINSstate.value_iMx_Nu0 + i]; } // --- вычитание из показаний датчиков значений нулей, полученных алгебраической выставкой на начальной выставке for (int i = 0; i < 3; i++) { fz[i] = fz[i] - SINSstate.AlignAlgebraZeroF[i]; } for (int i = 0; i < 3; i++) { Wz[i] = Wz[i] + SINSstate.AlignAlgebraDrifts[i]; } CopyArray(SINSstate.F_z, fz); CopyArray(SINSstate.W_z, Wz); CopyArray(Vx_0, SINSstate.Vx_0); CopyArray(Vx_0_prev, SINSstate.Vx_0_prev); SINSstate.R_e = RadiusE(SINSstate.Latitude, SINSstate.Height); SINSstate.R_n = RadiusN(SINSstate.Latitude, SINSstate.Height); SINSstate.u_x = U_x0(SINSstate.Latitude); u[0] = 0.0; u[1] = SimpleData.U * Math.Cos(SINSstate.Latitude); u[2] = SimpleData.U * Math.Sin(SINSstate.Latitude); //-------------ИНТЕГРИРОВАНИЕ МАТРИЦЫ AT_Z_XI И ПЕРВОЕ ВЫЧИСЛЕНИЕ МАТРИЦЫ D_X_Z--------- for (int i = 0; i < 3; i++) { fz[i] = (fz[i] + SINSstate.F_z_prev[i]) / 2.0; Wz[i] = (Wz[i] + SINSstate.W_z_prev[i]) / 2.0; } W_z_abs = Math.Sqrt(Wz[0] * Wz[0] + Wz[1] * Wz[1] + Wz[2] * Wz[2]); dlt = Math.Sin(W_z_abs * SINSstate.timeStep) / W_z_abs; dlt2 = (1.0 - Math.Cos(W_z_abs * SINSstate.timeStep)) / (W_z_abs * W_z_abs); Hat1 = Matrix.SkewSymmetricMatrix(Wz); Hat2 = Matrix.SkewSymmetricMatrixSquare(Wz); // --- Интегрирование матрицы ориентации AT_z_xi приборного относительно инерциального трехгранника CopyMatrix(dMatrix, (E + Hat1 * dlt + Hat2 * dlt2)); CopyMatrix(AT_z_xi, (dMatrix * AT_z_xi)); //Нормировка for (int i = 0; i < 3; i++) { tempV[i] = Math.Sqrt(AT_z_xi[i, 0] * AT_z_xi[i, 0] + AT_z_xi[i, 1] * AT_z_xi[i, 1] + AT_z_xi[i, 2] * AT_z_xi[i, 2]); for (int j = 0; j < 3; j++) { AT_z_xi[i, j] = AT_z_xi[i, j] / tempV[i]; } } CopyMatrix(SINSstate.AT, AT_z_xi); // --- Вычисление матрицы ориентации D_x_z приборного относительно географии CopyMatrix(W_x_xi, B_x_eta * SINSstate.A_nxi); CopyMatrix(D_x_z, W_x_xi * SINSstate.AT.Transpose()); //-------------------------------------------------------------------------------------- //---------------------------------ИНТЕГРИРОВАНИЕ СКОРОСТЕЙ---------------------------- CopyArray(SINSstate.F_x, D_x_z * fz); SINSstate.g = SimpleData.Gravity_Normal * (1.0 + 0.0053020 * Math.Pow(Math.Sin(SINSstate.Latitude), 2) - 0.000007 * Math.Pow(Math.Sin(2 * SINSstate.Latitude), 2)) - 0.00014 - 2 * 0.000001538 * Altitude; dVx = SINSstate.F_x[0] + Vx_0[1] * (2.0 * u[2] + SINSstate.Omega_x[2]) - Vx_0[2] * (2.0 * u[1] + SINSstate.Omega_x[1]); dVy = SINSstate.F_x[1] - Vx_0[0] * (2.0 * u[2] + SINSstate.Omega_x[2]) + Vx_0[2] * (2.0 * u[0] + SINSstate.Omega_x[0]); Vx_0[0] += dVx * SINSstate.timeStep; Vx_0[1] += dVy * SINSstate.timeStep; if (SimpleOperations.AbsoluteVectorValue(Vx_0) > 0.1) { double[] Vs = new double[3], Vx0 = new double[3]; Vx0[0] = Vx_0[0]; Vx0[1] = Vx_0[1]; SimpleOperations.CopyArray(Vs, SINSstate.A_sx0 * Vx0); SINSstate.distance_by_SINS += Vs[1] * SINSstate.timeStep; } //-------------------------------------------------------------------------------------- //--- Интегрируем вертикальную скорость ---// if (SINSstate.flag_AutonomouseSolution == false) { dVh = SINSstate.F_x[2] - SINSstate.g + (Vx_0[0] + Vx_0_prev[0]) / 2.0 * (2 * u[1] + SINSstate.Omega_x[1]) - (Vx_0[1] + Vx_0_prev[1]) / 2.0 * (2 * u[0] + SINSstate.Omega_x[0]); Vx_0[2] += dVh * SINSstate.timeStep; dh = (Vx_0[2] + Vx_0_prev[2]) / 2.0; Altitude += dh * SINSstate.timeStep; } //---------ИНТЕГРИРОВАНИЕ МАТРИЦЫ B_X_ETA И ВТОРОЕ ВЫЧИСЛЕНИЕ МАТРИЦЫ D_X_Z-------------- SINSstate.Omega_x[0] = -(Vx_0[1] + Vx_0_prev[1]) / 2.0 / SINSstate.R_n; SINSstate.Omega_x[1] = (Vx_0[0] + Vx_0_prev[0]) / 2.0 / SINSstate.R_e; SINSstate.Omega_x[2] = Math.Tan(SINSstate.Latitude) * SINSstate.Omega_x[1]; Omega_x_abs = Math.Sqrt(SINSstate.Omega_x[0] * SINSstate.Omega_x[0] + SINSstate.Omega_x[1] * SINSstate.Omega_x[1] + SINSstate.Omega_x[2] * SINSstate.Omega_x[2]); if (Omega_x_abs != 0) { dlt = Math.Sin(Omega_x_abs * SINSstate.timeStep) / Omega_x_abs; dlt2 = (1.0 - Math.Cos(Omega_x_abs * SINSstate.timeStep)) / (Omega_x_abs * Omega_x_abs); } else { dlt = 1.0; dlt2 = 0.0; } Hat1 = Matrix.SkewSymmetricMatrix(SINSstate.Omega_x); Hat2 = Matrix.SkewSymmetricMatrixSquare(SINSstate.Omega_x); CopyMatrix(dMatrix, E + Hat1 * dlt + Hat2 * dlt2); CopyMatrix(B_x_eta, dMatrix * B_x_eta); //Нормировка for (int i = 0; i < 3; i++) { tempV[i] = Math.Sqrt(B_x_eta[i, 0] * B_x_eta[i, 0] + B_x_eta[i, 1] * B_x_eta[i, 1] + B_x_eta[i, 2] * B_x_eta[i, 2]); for (int j = 0; j < 3; j++) { B_x_eta[i, j] = B_x_eta[i, j] / tempV[i]; } } // --- Повторное вычисление матрицы ориентации D_x_z приборного относительно географии CopyMatrix(W_x_xi, B_x_eta * SINSstate.A_nxi); CopyMatrix(D_x_z, W_x_xi * SINSstate.AT.Transpose()); //----------------Вычисление углов и переприсвоение матриц--------------------------- CopyMatrix(SINSstate.A_sx0, D_x_z.Transpose()); CopyMatrix(SINSstate.A_x0s, D_x_z); CopyMatrix(SINSstate.A_x0n, B_x_eta); CopyMatrix(SINSstate.A_nx0, B_x_eta.Transpose()); //---ОПРЕДЕЛЕНИЕ ГЕОГРАФИЧЕСКИХ КООРДИНАТ--- SINSstate.Longitude = Math.Atan2(SINSstate.A_x0n[2, 1], SINSstate.A_x0n[2, 0]); SINSstate.Latitude = Math.Atan2(SINSstate.A_x0n[2, 2], Math.Sqrt(SINSstate.A_x0n[0, 2] * SINSstate.A_x0n[0, 2] + SINSstate.A_x0n[1, 2] * SINSstate.A_x0n[1, 2])); Azimth = Math.Atan2(SINSstate.A_x0n[0, 2], SINSstate.A_x0n[1, 2]); SINSstate.Altitude_prev = SINSstate.Height; SINSstate.Height = Altitude; //SINSstate.Heading = gkurs - Azimth; SINSstate.Heading = Math.Atan2(SINSstate.A_sx0[1, 0], SINSstate.A_sx0[1, 1]); SINSstate.Roll = -Math.Atan2(SINSstate.A_sx0[0, 2], SINSstate.A_sx0[2, 2]); SINSstate.Pitch = Math.Atan2(SINSstate.A_sx0[1, 2], Math.Sqrt(SINSstate.A_sx0[0, 2] * SINSstate.A_sx0[0, 2] + SINSstate.A_sx0[2, 2] * SINSstate.A_sx0[2, 2])); //SINSstate.Azimth = Azimth; SINSstate.R_e = RadiusE(SINSstate.Latitude, SINSstate.Height); SINSstate.R_n = RadiusN(SINSstate.Latitude, SINSstate.Height); SINSstate.u_x = U_x0(SINSstate.Latitude); CopyArray(SINSstate.Vx_0_prev, SINSstate.Vx_0); CopyArray(SINSstate.Vx_0, Vx_0); CopyArray(SINSstate.F_z_prev, SINSstate.F_z); CopyArray(SINSstate.W_z_prev, SINSstate.W_z); CopyArray(SINSstate.W_x, SINSstate.A_x0s * Wz); //-------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------- //--------- ДЛЯ SINSstate_OdoModel - одометрического счисления ---------// SimpleOperations.CopyArray(SINSstate_OdoMod.Vx_0_prev, SINSstate_OdoMod.Vx_0); SimpleOperations.CopyArray(SINSstate.OdoSpeed_x0, SINSstate.A_x0s * SINSstate.OdoSpeed_s); SimpleOperations.CopyArray(SINSstate_OdoMod.OdoSpeed_s, SINSstate.OdoSpeed_s); //--- считаем для одометрического счисления свою матрицу ориентации SINSstate_OdoMod.A_x0s ---// SimpleOperations.CopyMatrix(W_x_xi, SINSstate_OdoMod.A_x0n * SINSstate.A_nxi); SimpleOperations.CopyMatrix(SINSstate_OdoMod.A_x0s, W_x_xi * SINSstate.AT.Transpose()); SimpleOperations.CopyArray(SINSstate_OdoMod.OdoSpeed_x0, SINSstate_OdoMod.A_x0s * SINSstate.OdoSpeed_s); //---------ВЫЧИСЛЕНИЕ МАТРИЦЫ B_X_ETA И ВТОРОЕ ВЫЧИСЛЕНИЕ МАТРИЦЫ D_X_Z для одометрического счисления-------------- { SimpleOperations.CopyArray(SINSstate_OdoMod.Vx_0, SINSstate_OdoMod.OdoSpeed_x0); SINSstate_OdoMod.Omega_x[0] = -(SINSstate_OdoMod.Vx_0[1] + SINSstate_OdoMod.Vx_0_prev[1]) / 2.0 / SINSstate_OdoMod.R_n; SINSstate_OdoMod.Omega_x[1] = (SINSstate_OdoMod.Vx_0[0] + SINSstate_OdoMod.Vx_0_prev[0]) / 2.0 / SINSstate_OdoMod.R_e; SINSstate_OdoMod.Omega_x[2] = Math.Tan(SINSstate_OdoMod.Latitude) * SINSstate_OdoMod.Omega_x[1]; //--- Производим одометрическое счисление координат, если пришло обновление показаний одометра ---// if (SINSstate.OdometerData.odometer_left.isReady == 1) { double[] dS_x = new double[3]; SimpleOperations.CopyArray(dS_x, SINSstate_OdoMod.A_x0s * SINSstate.OdometerVector); SINSstate_OdoMod.Latitude = SINSstate_OdoMod.Latitude + dS_x[1] / SimpleOperations.RadiusN(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Height); SINSstate_OdoMod.Longitude = SINSstate_OdoMod.Longitude + dS_x[0] / SimpleOperations.RadiusE(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Height) / Math.Cos(SINSstate_OdoMod.Latitude); SINSstate_OdoMod.Height = SINSstate_OdoMod.Height + dS_x[2]; } //----------------Вычисление углов и переприсвоение матриц--------------------------- SINSstate_OdoMod.A_x0n = SimpleOperations.A_x0n(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Longitude); SimpleOperations.CopyMatrix(SINSstate_OdoMod.A_nx0, SINSstate_OdoMod.A_x0n.Transpose()); SimpleOperations.CopyMatrix(W_x_xi, SINSstate_OdoMod.A_x0n * SINSstate.A_nxi); SimpleOperations.CopyMatrix(SINSstate_OdoMod.A_x0s, W_x_xi * SINSstate.AT.Transpose()); SimpleOperations.CopyMatrix(SINSstate_OdoMod.A_sx0, SINSstate_OdoMod.A_x0s.Transpose()); } SINSstate_OdoMod.R_e = RadiusE(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Height); SINSstate_OdoMod.R_n = RadiusN(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Height); //-------------------------------------------------------------------------------------- }
public static void MatrixNoise_ReDef(SINS_State SINSstate, Kalman_Vars KalmanVars) { int iMx = SimpleData.iMx, iMz = SimpleData.iMz, iMq = SimpleData.iMq, iMx_r12_odo = SINSstate.value_iMx_r_odo_12, iMx_kappa_3_ds = SINSstate.value_iMx_kappa_3_ds; int iMx_dV_12 = SINSstate.value_iMx_dV_12, iMx_alphaBeta = SINSstate.value_iMx_alphaBeta, iMx_Nu0 = SINSstate.value_iMx_Nu0; double sqrt_freq_vert = Math.Sqrt(Math.Abs(SINSstate.Freq)); double sqrt_freq = Math.Sqrt(Math.Abs(SINSstate.Freq)); double Noise_Pos = KalmanVars.Noise_Pos, Noise_Pos_Vertical = KalmanVars.Noise_Pos_Vertical; if (Math.Abs(SimpleOperations.AbsoluteVectorValue(SINSstate.Vx_0)) < 0.2) { //sqrt_freq = 1.0; sqrt_freq_vert = 1.0; Noise_Pos = 0.1; Noise_Pos_Vertical = 0.0; } double[] Noise_Vel_in_Mx = new double[3], Noise_Angl_in_Mx = new double[3]; for (int i = 0; i < iMx * iMq; i++) { KalmanVars.CovarianceMatrixNoise[i] = 0.0; } // --- На основе шумовых параметров, полученных на выставке в приборной системе координат, формируем шумовые параметры в проекции на географическую СК for (int j = 0; j < 3; j++) { Noise_Vel_in_Mx[j] = Math.Sqrt(Math.Pow(SINSstate.A_x0s[j, 0] * KalmanVars.Noise_Vel[0], 2) + Math.Pow(SINSstate.A_x0s[j, 1] * KalmanVars.Noise_Vel[1], 2) + Math.Pow(SINSstate.A_x0s[j, 2] * KalmanVars.Noise_Vel[2], 2)); Noise_Angl_in_Mx[j] = Math.Sqrt(Math.Pow(SINSstate.A_x0s[j, 0] * KalmanVars.Noise_Angl[0], 2) + Math.Pow(SINSstate.A_x0s[j, 1] * KalmanVars.Noise_Angl[1], 2) + Math.Pow(SINSstate.A_x0s[j, 2] * KalmanVars.Noise_Angl[2], 2)); } // --- шумы по горизонтальному каналу БИНС KalmanVars.CovarianceMatrixNoise[0 * iMq + 0] = Noise_Pos * sqrt_freq; KalmanVars.CovarianceMatrixNoise[1 * iMq + 1] = Noise_Pos * sqrt_freq; // --- Проставляются параметры шумов датчиков в матриц Q // KalmanVars.CovarianceMatrixNoise[(iMx_dV_12 + 0) * iMq + iMx_dV_12 + 0] = Noise_Vel_in_Mx[0] * sqrt_freq; //KalmanVars.CovarianceMatrixNoise[(iMx_dV_12 + 0) * iMq + iMx_alphaBeta + 0] = SINSstate.Vx_0[1] * Noise_Angl_in_Mx[0] * sqrt_freq; KalmanVars.CovarianceMatrixNoise[(iMx_dV_12 + 1) * iMq + iMx_dV_12 + 1] = Noise_Vel_in_Mx[1] * sqrt_freq; //KalmanVars.CovarianceMatrixNoise[(iMx_dV_12 + 1) * iMq + iMx_alphaBeta + 1] = SINSstate.Vx_0[0] * Noise_Angl_in_Mx[1] * sqrt_freq; KalmanVars.CovarianceMatrixNoise[(iMx_alphaBeta + 0) * iMq + iMx_alphaBeta + 0] = Noise_Angl_in_Mx[0] * sqrt_freq; KalmanVars.CovarianceMatrixNoise[(iMx_alphaBeta + 1) * iMq + iMx_alphaBeta + 1] = Noise_Angl_in_Mx[1] * sqrt_freq; KalmanVars.CovarianceMatrixNoise[(iMx_alphaBeta + 2) * iMq + iMx_alphaBeta + 2] = Noise_Angl_in_Mx[2] * sqrt_freq; // --- шумы по горизонтальному одометрическому решению KalmanVars.CovarianceMatrixNoise[(iMx_r12_odo + 0) * iMq + iMx_r12_odo + 0] = Noise_Pos * sqrt_freq; KalmanVars.CovarianceMatrixNoise[(iMx_r12_odo + 1) * iMq + iMx_r12_odo + 1] = Noise_Pos * sqrt_freq; //KalmanVars.CovarianceMatrixNoise[(iMx_Nu0 + 0) * iMq + iMx_Nu0 + 0] = 0.001 * SimpleData.ToRadian / 3600.0; //KalmanVars.CovarianceMatrixNoise[(iMx_Nu0 + 1) * iMq + iMx_Nu0 + 1] = 0.001 * SimpleData.ToRadian / 3600.0; //KalmanVars.CovarianceMatrixNoise[(iMx_Nu0 + 2) * iMq + iMx_Nu0 + 2] = 0.001 * SimpleData.ToRadian / 3600.0; //SimpleOperations.PrintMatrixToFile(KalmanVars.CovarianceMatrixNoise, SimpleData.iMx, SimpleData.iMx, "CovarianceMatrixNoise"); // ----------------------------------------------------------// // --- Матрица шумов для вертикального канала --- SimpleOperations.NullingOfArray(KalmanVars.Vertical_CovarianceMatrixNoise); KalmanVars.Vertical_CovarianceMatrixNoise[0 * SimpleData.iMq_Vertical + 0] = Noise_Pos_Vertical * sqrt_freq_vert; KalmanVars.Vertical_CovarianceMatrixNoise[1 * SimpleData.iMq_Vertical + 1] = Noise_Vel_in_Mx[2] * sqrt_freq_vert; KalmanVars.Vertical_CovarianceMatrixNoise[SINSstate.Vertical_rOdo3 * SimpleData.iMq_Vertical + SINSstate.Vertical_rOdo3] = Noise_Pos_Vertical * sqrt_freq_vert; }
public static void Make_A(SINS_State SINSstate, Kalman_Vars KalmanVars, SINS_State SINSstate_OdoMod) { int iMx = SimpleData.iMx, iMz = SimpleData.iMz, iMq = SimpleData.iMq, iMx_r3_dV3 = SINSstate.iMx_r3_dV3, iMx_odo_model = SINSstate.iMx_odo_model, iMx_r12_odo = SINSstate.iMx_r12_odo; SINSprocessing.Make_A(SINSstate, KalmanVars, SINSstate_OdoMod); /*-----------МОДИФИЦИРОВАННЫЙ СЛАБОСВЗАННЫЙ ВАРИАНТ----------------*/ if (SINSstate.flag_OdoSINSWeakConnect_MODIF) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + 6] = SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 1] = SINSstate_OdoMod.Omega_x[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 6] = -SINSstate_OdoMod.Vx_0[0]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 0] = -SINSstate_OdoMod.Omega_x[2]; if (SINSstate.flag_iMx_r3_dV3) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + 0] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_e; KalmanVars.Matrix_A[iMx_r12_odo * iMx + 5] += -SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 2] = -SINSstate_OdoMod.Omega_x[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 1] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_n; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 4] += SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 2] = SINSstate_OdoMod.Omega_x[0]; } if (SINSstate.flag_Using_iMx_r_odo_3) { KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 0] = -SINSstate_OdoMod.Vx_0[0] / SINSstate_OdoMod.R_e; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 1] = -SINSstate_OdoMod.Vx_0[1] / SINSstate_OdoMod.R_n; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 4] = -SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 5] = SINSstate_OdoMod.Vx_0[0]; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_r12_odo + 0] = SINSstate_OdoMod.Omega_x[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_r12_odo + 1] = -SINSstate_OdoMod.Omega_x[0]; } } /*-----------СЛАБОСВЗАННЫЙ ВАРИАНТ----------------*/ else if (SINSstate.flag_OdoSINSWeakConnect) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + 6] = SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + 0] = -SINSstate_OdoMod.Omega_x[0] * Math.Tan(SINSstate_OdoMod.Latitude); KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 0] = SINSstate_OdoMod.Omega_x[0] * Math.Tan(SINSstate_OdoMod.Latitude); KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 1] = SINSstate_OdoMod.Omega_x[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 6] = -SINSstate_OdoMod.Vx_0[0]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 0] = -SINSstate_OdoMod.Omega_x[2]; if (SINSstate.flag_iMx_r3_dV3) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + 5] += -SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 0] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_e; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 2] = -SINSstate_OdoMod.Omega_x[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 4] += SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 1] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_n; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 2] = SINSstate_OdoMod.Omega_x[0]; } if (SINSstate.flag_Using_iMx_r_odo_3) { KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 4] = -SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 5] = SINSstate_OdoMod.Vx_0[0]; } } if (SINSstate.flag_iMx_kappa_13_ds) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_odo_model + 0] = -SINSstate_OdoMod.A_x0s[0, 2] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_odo_model + 1] = SINSstate_OdoMod.A_x0s[0, 0] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_odo_model + 2] = SINSstate_OdoMod.A_x0s[0, 1] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_odo_model + 0] = -SINSstate_OdoMod.A_x0s[1, 2] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_odo_model + 1] = SINSstate_OdoMod.A_x0s[1, 0] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_odo_model + 2] = SINSstate_OdoMod.A_x0s[1, 1] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); if (SINSstate.flag_Using_iMx_r_odo_3) { KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_odo_model + 0] = -SINSstate_OdoMod.A_x0s[2, 2] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_odo_model + 1] = SINSstate_OdoMod.A_x0s[2, 0] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_odo_model + 2] = SINSstate_OdoMod.A_x0s[2, 1] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); } } //if (SINSstate.Count % 1000 == 0) //{ // SimpleOperations.PrintMatrixToFile(KalmanVars.Matrix_A, SimpleData.iMx, SimpleData.iMx); // SimpleOperations.PrintMatrixToFile(KalmanVars.CovarianceMatrixS_m, SimpleData.iMx, SimpleData.iMx); //} }
public static int RougthAlignment(Proc_Help ProcHelp, SINS_State SINSstate, StreamReader myFile, Kalman_Vars KalmanVars, SINS_State SINSstate_OdoMod, StreamWriter GRTV_output) { int k = 0, i = 0; double[] f_avg = new double[3]; double[] w_avg = new double[3]; double[] w_avg_x = new double[3]; double[] U_s = new double[3]; Matrix A_xs = new Matrix(3, 3); StreamWriter Alignment_avg_rougth = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_avg_rougth.txt"); StreamWriter Alignment_avg_rougthMovingAVG = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_avg_rougth_MovingAVG.txt"); StreamWriter myFileWithSmoothedCoord = new StreamWriter(SimpleData.PathInputString + "DataWithSmoothedCoord//Align_InputDataWithSmoothedCoordinates.txt"); // --- вспомогательные массивы для определения сигмы шумов double[] array_f_1 = new double[200000], array_sigma_f_1 = new double[200000]; double[] array_f_2 = new double[200000], array_sigma_f_2 = new double[200000]; double[] array_f_3 = new double[200000], array_sigma_f_3 = new double[200000]; double[] array_w_1 = new double[200000], array_sigma_w_1 = new double[200000]; double[] array_w_2 = new double[200000], array_sigma_w_2 = new double[200000]; double[] array_w_3 = new double[200000], array_sigma_w_3 = new double[200000]; double[] sigma_f = new double[3]; double[] sigma_w = new double[3]; Alignment_avg_rougth.WriteLine("count f_1 f_2 f_3 w_1 w_2 w_3 heading roll pitch Latitude"); Alignment_avg_rougthMovingAVG.WriteLine("count MA_f_1 MA_f_2 MA_f_3 MA_w_1 MA_w_2 MA_w_3"); while (true) { i++; if (i < 1) { myFile.ReadLine(); continue; } if (SINSstate.FLG_Stop == 0 && false) { ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, myFileWithSmoothedCoord, SINSstate, SINSstate_OdoMod, true); } else { i--; break; } } int t = i; double Latitude = 0.0, Pitch = 0.0, Roll = 0.0, Heading = 0.0; int MovingWindow = 500; double[] MovingAverageAccGyro = new double[6]; // --- k_f, k_nu - отдельные счетчики сколько обновлений соответствующих датчиков были использованы для осреднения (в некоторых заездах // --- почему-то ньютонометры на начальной выставке поставляют константное значение) int k_f = 0, k_nu = 0; for (i = t; ; i++) { ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, myFileWithSmoothedCoord, SINSstate, SINSstate_OdoMod, true); if (/*SINSstate.FLG_Stop == 0 || */ (ProcHelp.AlignmentCounts != 0 && i == ProcHelp.AlignmentCounts)) { break; } //if (i == 1000) // break; array_f_1[k] = SINSstate.F_z[0]; array_f_2[k] = SINSstate.F_z[1]; array_f_3[k] = SINSstate.F_z[2]; array_w_1[k] = SINSstate.W_z[0]; array_w_2[k] = SINSstate.W_z[1]; array_w_3[k] = SINSstate.W_z[2]; double array_sigma_f_1_tmp_sum = 0.0, array_sigma_w_1_tmp_sum = 0.0; int u = 0; for (u = 1; u <= Math.Min(i, 50); u++) { array_sigma_f_1_tmp_sum += array_f_1[i - u]; } array_sigma_f_1_tmp_sum /= (u - 1); if (Math.Abs(array_sigma_f_1_tmp_sum - array_f_1[i]) > 1E-9) { array_sigma_f_1[k_f] = SINSstate.F_z[0]; array_sigma_f_2[k_f] = SINSstate.F_z[1]; array_sigma_f_3[k_f] = SINSstate.F_z[2]; f_avg[0] += SINSstate.F_z[0]; f_avg[1] += SINSstate.F_z[1]; f_avg[2] += SINSstate.F_z[2]; k_f++; } u = 0; for (u = 1; u <= Math.Min(i, 50); u++) { array_sigma_w_1_tmp_sum += array_w_1[i - u]; } array_sigma_w_1_tmp_sum /= (u - 1); if (Math.Abs(array_sigma_w_1_tmp_sum - array_w_1[i]) > 1E-9) { array_sigma_w_1[k_nu] = SINSstate.W_z[0]; array_sigma_w_2[k_nu] = SINSstate.W_z[1]; array_sigma_w_3[k_nu] = SINSstate.W_z[2]; w_avg[0] += SINSstate.W_z[0]; w_avg[1] += SINSstate.W_z[1]; w_avg[2] += SINSstate.W_z[2]; k_nu++; } k++; for (int u1 = 1; u1 <= Math.Min(k, MovingWindow); u1++) { MovingAverageAccGyro[0] += array_f_1[k - u1]; MovingAverageAccGyro[1] += array_f_2[k - u1]; MovingAverageAccGyro[2] += array_f_3[k - u1]; MovingAverageAccGyro[3] += array_w_1[k - u1]; MovingAverageAccGyro[4] += array_w_2[k - u1]; MovingAverageAccGyro[5] += array_w_3[k - u1]; } for (int u1 = 0; u1 < 6; u1++) { MovingAverageAccGyro[u1] = MovingAverageAccGyro[u1] / MovingWindow; } //------- Pitch = Math.Atan2(f_avg[1], Math.Sqrt(f_avg[0] * f_avg[0] + f_avg[2] * f_avg[2])); Roll = -Math.Atan2(f_avg[0], f_avg[2]); A_xs = SimpleOperations.A_xs(Heading, Roll, Pitch); w_avg_x = Matrix.Multiply(A_xs, w_avg); Heading = -Math.Atan2(w_avg_x[0], w_avg_x[1]); Latitude = Math.Atan2(w_avg_x[2], Math.Sqrt(w_avg_x[1] * w_avg_x[1] + w_avg_x[0] * w_avg_x[0])); SINSstate.A_sx0 = SimpleOperations.A_sx0(Heading, Roll, Pitch); U_s = SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude); //FinalAlignment.WriteLine(k + " " + SINSstate.Count + " " + Heading + " " + Roll + " " + Pitch + " " + Latitude // + " " + SINSstate.F_z[0] + " " + SINSstate.F_z[1] + " " + SINSstate.F_z[2] // + " " + SINSstate.W_z[0] + " " + SINSstate.W_z[1] + " " + SINSstate.W_z[2] // + " " + U_s[0] + " " + U_s[1] + " " + U_s[2]); if (Math.Abs(w_avg[0] / k - U_s[0]) < 0.000005) { } else { Heading = Heading - Math.PI; SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate); U_s = SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude); } for (int j = 0; j < 3; j++) { SINSstate.AlignAlgebraDrifts[j] = w_avg[j] / k_nu - U_s[j]; } if (k > MovingWindow && k % 10 == 0) { Alignment_avg_rougth.WriteLine(SINSstate.Count.ToString() + " " + (f_avg[0] / k_f).ToString() + " " + (f_avg[1] / k_f).ToString() + " " + (f_avg[2] / k_f).ToString() + " " + (w_avg[0] / k_nu).ToString() + " " + (w_avg[1] / k_nu).ToString() + " " + (w_avg[2] / k_nu).ToString() + " " + (Heading * SimpleData.ToDegree).ToString() + " " + (Roll * SimpleData.ToDegree).ToString() + " " + (Pitch * SimpleData.ToDegree).ToString() + " " + Latitude.ToString() + " " + (w_avg_x[0] / k_nu).ToString() + " " + (w_avg_x[1] / k_nu).ToString() + " " + (w_avg_x[2] / k_nu).ToString() ); Alignment_avg_rougthMovingAVG.WriteLine(SINSstate.Time.ToString() + " " + MovingAverageAccGyro[0] + " " + MovingAverageAccGyro[1] + " " + MovingAverageAccGyro[2] + " " + MovingAverageAccGyro[3] + " " + MovingAverageAccGyro[4] + " " + MovingAverageAccGyro[5]); } // --- Вывод данных для формирования GRTV файла --- // if (SINSstate.flag_GRTV_output) { GRTV_output.WriteLine( SINSstate.Count + " " + "4" + " " + " " + SINSstate.F_z_orig[1] + " " + SINSstate.F_z_orig[2] + " " + SINSstate.F_z_orig[0] + " " + SINSstate.W_z_orig[1] + " " + SINSstate.W_z_orig[2] + " " + SINSstate.W_z_orig[0] + " " + SINSstate.Latitude + " " + SINSstate.Longitude + " " + SINSstate.Height + " " + SINSstate.Vx_0[1] + " " + SINSstate.Vx_0[0] + " " + SINSstate.Vx_0[2] + " " + SINSstate.Heading + " " + SINSstate.Pitch + " " + SINSstate.Roll + " " + SINSstate.Latitude + " 1 " + SINSstate.Longitude + " 1 " + SINSstate.Height + " 1" + " " + SINSstate.Vx_0[1] + " 1 " + SINSstate.Vx_0[0] + " 1 " + SINSstate.Vx_0[2] + " 1" + " " + SINSstate.OdometerData.odometer_left.Value_orig + " " + SINSstate.OdometerData.odometer_left.isReady_orig //метка времени - отмечает момент времени формирования пакета СНС-данных + " " + SINSstate.GPS_Data.gps_Latitude.isReady_orig + " " + SINSstate.GPS_Data.gps_Latitude.Value_orig + " " + SINSstate.GPS_Data.gps_Latitude.isReady_orig + " " + SINSstate.GPS_Data.gps_Longitude.Value_orig + " " + SINSstate.GPS_Data.gps_Longitude.isReady_orig + " " + SINSstate.GPS_Data.gps_Altitude.Value_orig + " " + SINSstate.GPS_Data.gps_Altitude.isReady_orig + " " + SINSstate.GPS_Data.gps_Vn.Value_orig + " " + SINSstate.GPS_Data.gps_Vn.isReady_orig + " " + SINSstate.GPS_Data.gps_Ve.Value_orig + " " + SINSstate.GPS_Data.gps_Vn.isReady_orig + " " + " 0 0" //Скорость GPS вертикальная ); } } f_avg[0] = f_avg[0] / k_f; w_avg[0] = w_avg[0] / k_nu; f_avg[1] = f_avg[1] / k_f; w_avg[1] = w_avg[1] / k_nu; f_avg[2] = f_avg[2] / k_f; w_avg[2] = w_avg[2] / k_nu; for (int j = 1; j < k_f; j++) { sigma_f[0] += Math.Pow((array_sigma_f_1[j] - f_avg[0]), 2); sigma_f[1] += Math.Pow((array_sigma_f_2[j] - f_avg[1]), 2); sigma_f[2] += Math.Pow((array_sigma_f_3[j] - f_avg[2]), 2); } for (int j = 1; j < k_nu; j++) { sigma_w[0] += Math.Pow((array_sigma_w_1[j] - w_avg[0]), 2); sigma_w[1] += Math.Pow((array_sigma_w_2[j] - w_avg[1]), 2); sigma_w[2] += Math.Pow((array_sigma_w_3[j] - w_avg[2]), 2); } sigma_f[0] = Math.Sqrt(sigma_f[0] / k_f); sigma_f[1] = Math.Sqrt(sigma_f[1] / k_f); sigma_f[2] = Math.Sqrt(sigma_f[2] / k_f); sigma_w[0] = Math.Sqrt(sigma_w[0] / k_nu); sigma_w[1] = Math.Sqrt(sigma_w[1] / k_nu); sigma_w[2] = Math.Sqrt(sigma_w[2] / k_nu); //шумы ньютонометров и дусов for (int j = 0; j < 3; j++) { if (SimpleOperations.AbsoluteVectorValue(sigma_f) > 1E-5) { KalmanVars.Noise_Vel[j] = sigma_f[j] / 1.0; } if (SimpleOperations.AbsoluteVectorValue(sigma_w) > 1E-9) { KalmanVars.Noise_Angl[j] = sigma_w[j] / 1.0; } } // === По вертикальному шум всегда будет меньше на выставке, поэтому мы немного сглаживаем === // if (SINSstate.flag_equalizeVertNoise == true) { KalmanVars.Noise_Vel[2] = (KalmanVars.Noise_Vel[0] + KalmanVars.Noise_Vel[1]) / 2.0; KalmanVars.Noise_Angl[2] = (KalmanVars.Noise_Angl[0] + KalmanVars.Noise_Angl[1]) / 2.0; } SINSstate.Pitch = Math.Atan2(f_avg[1], Math.Sqrt(f_avg[0] * f_avg[0] + f_avg[2] * f_avg[2])); SINSstate.Roll = -Math.Atan2(f_avg[0], f_avg[2]); //if (SINSstate.Global_file == "GRTV_Ekat_151029_1_zaezd") //{ // KalmanVars.Noise_Vel[0] = 0.007814; KalmanVars.Noise_Angl[0] = 0.0000888; // KalmanVars.Noise_Vel[1] = 0.003528; KalmanVars.Noise_Angl[1] = 0.0002505; // KalmanVars.Noise_Vel[2] = 0.005671; KalmanVars.Noise_Angl[2] = 0.0001697; //} //if (SINSstate.Global_file == "GRTVout_GCEF_format (070715выезд завод)" || SINSstate.Global_file == "GRTVout_GCEF_format (070715выезд куликовка)") //{ // KalmanVars.Noise_Vel[0] = 0.0018; KalmanVars.Noise_Angl[0] = 0.000020; // KalmanVars.Noise_Vel[1] = 0.0018; KalmanVars.Noise_Angl[1] = 0.000020; // KalmanVars.Noise_Vel[2] = 0.0018; KalmanVars.Noise_Angl[2] = 0.000020; //} A_xs = SimpleOperations.A_xs(SINSstate); w_avg_x = Matrix.Multiply(A_xs, w_avg); if (true) { SINSstate.Heading = -Math.Atan2(w_avg_x[0], w_avg_x[1]); Latitude = Math.Atan2(w_avg_x[2], Math.Sqrt(w_avg_x[1] * w_avg_x[1] + w_avg_x[0] * w_avg_x[0])); } // -- следующий способ дает то же самое с разницей в несколько секунд -- // else { double[] l1 = new double[3], l2 = new double[3], l3 = new double[3]; l3[0] = A_xs[2, 0]; l3[1] = A_xs[2, 1]; l3[2] = A_xs[2, 2]; for (int ij = 0; ij < 3; ij++) { l2[ij] = (w_avg[ij] - SimpleData.U * l3[ij] * Math.Sin(SINSstate.Latitude)) / (SimpleData.U * Math.Cos(SINSstate.Latitude)); } double l2_mod = Math.Sqrt(l2[0] * l2[0] + l2[1] * l2[1] + l2[2] * l2[2]); l2[0] = l2[0] / l2_mod; l2[1] = l2[1] / l2_mod; l2[2] = l2[2] / l2_mod; l1[0] = -l2[2] * l3[1] + l2[1] * l3[2]; l1[1] = l2[2] * l3[0] - l2[0] * l3[2]; l1[2] = -l2[1] * l3[0] + l2[0] * l3[1]; SINSstate.Heading = -Math.Atan2(w_avg_x[0], w_avg_x[1]); SINSstate.Heading = Math.Atan2(l1[1], l2[1]); } SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate); U_s = SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude); //if (Math.Abs(w_avg[0] - U_s[0]) < 0.000005) { } //else //{ // SINSstate.Heading = SINSstate.Heading - Math.PI; // SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate); // U_s = SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude); //} double[] gilmertF = new double[3]; gilmertF[2] = SimpleOperations.GilmertGravityForce(SINSstate.Latitude, SINSstate.Height); SimpleOperations.CopyArray(gilmertF, SINSstate.A_sx0 * gilmertF); for (int j = 0; j < 3; j++) { SINSstate.AlignAlgebraDrifts[j] = w_avg[j] - U_s[j]; } if (SINSstate.AlgebraicCalibration_F_Zero == true) { for (int j = 0; j < 3; j++) { SINSstate.AlignAlgebraZeroF[j] = f_avg[j] - gilmertF[j]; } } SINSstate.Time_Alignment = SINSstate.Time; // --- Если задан курс в настройках ---// if (SINSstate.Alignment_HeadingDetermined == true) { SINSstate.Heading = SINSstate.Alignment_HeadingValue + SINSstate.alpha_kappa_3; } if (SINSstate.Alignment_RollDetermined == true) { SINSstate.Roll = SINSstate.Alignment_RollValue; } if (SINSstate.Alignment_PitchDetermined == true) { SINSstate.Pitch = SINSstate.Alignment_PitchValue - SINSstate.alpha_kappa_1; } if (Math.Abs(SINSstate.WRONG_alpha_kappa_1) > 0.0) { SINSstate.Pitch = SINSstate.Pitch - SINSstate.WRONG_alpha_kappa_1; } if (Math.Abs(SINSstate.WRONG_alpha_kappa_3) > 0.0) { SINSstate.Heading = SINSstate.Heading + SINSstate.WRONG_alpha_kappa_3; } if (SINSstate.Global_file == "Saratov_run_2014_07_23") { double lat_dif_true = (49.99452656 * SimpleData.ToRadian - SINSstate.Latitude_Start) * SimpleOperations.RadiusN(49.99452656 * SimpleData.ToRadian, SINSstate.Height_Start); double long_dif_true = (46.87201806 * SimpleData.ToRadian - SINSstate.Longitude_Start) * SimpleOperations.RadiusE(49.99452656 * SimpleData.ToRadian, SINSstate.Height_Start) * Math.Cos(49.99452656 * SimpleData.ToRadian); double SettedHeading = Math.Atan2(long_dif_true, lat_dif_true); if (SINSstate.Time > 10000.0) { SettedHeading = SimpleOperations.CalculateHeadingByTwoDots(49.80892188 * SimpleData.ToRadian, 45.3817334 * SimpleData.ToRadian, SINSstate.GPS_Data.gps_Altitude_prev.Value, 49.80906066 * SimpleData.ToRadian, 45.38113053 * SimpleData.ToRadian, SINSstate.GPS_Data.gps_Altitude.Value); SINSstate.Heading = SettedHeading; SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate); SimpleOperations.CopyArray(U_s, SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude)); } else { double difHeadings = SettedHeading - SINSstate.Heading; SINSstate.Heading = SettedHeading; SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate); SimpleOperations.CopyArray(U_s, SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude)); } for (int j = 0; j < 3; j++) { SINSstate.AlignAlgebraDrifts[j] = w_avg[j] - U_s[j]; } for (int j = 0; j < 3; j++) { KalmanVars.Noise_Vel[j] = KalmanVars.Noise_Vel[j] * 5.0; KalmanVars.Noise_Angl[j] = KalmanVars.Noise_Angl[j] * 5.0; //KalmanVars.Noise_Vel[j] = KalmanVars.Noise_Vel.Max(); //KalmanVars.Noise_Angl[j] = KalmanVars.Noise_Angl.Max(); } } // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate); SINSstate.A_x0s = SINSstate.A_sx0.Transpose(); SINSstate.A_x0n = SimpleOperations.A_x0n(SINSstate.Latitude, SINSstate.Longitude); SINSstate.A_nx0 = SINSstate.A_x0n.Transpose(); SINSstate.AT = Matrix.Multiply(SINSstate.A_sx0, SINSstate.A_x0n); SINSstate.A_nxi = SimpleOperations.A_ne(SINSstate.Time - SINSstate.Time_Alignment, SINSstate.Longitude_Start); //Далее произойдет обнуление SINSstate.Time SINSstate.AT = Matrix.Multiply(SINSstate.AT, SINSstate.A_nxi); Alignment_avg_rougth.Close(); Alignment_avg_rougthMovingAVG.Close(); myFileWithSmoothedCoord.Close(); return(i); }
public static void OutPutInfo(int i, int start_i, Proc_Help ProcHelp, SINS_State SINSstate, SINS_State SINSstate_OdoMod, Kalman_Vars KalmanVars, StreamWriter Nav_FeedbackSolution, StreamWriter Nav_StateErrorsVector, StreamWriter Nav_Errors, StreamWriter GRTV_output, StreamWriter Nav_CovarianceDiagonal, StreamWriter KMLFileOut, StreamWriter KMLFileOut_PNPPK ) { double Lat = 0.0, Long = 0.0; double[] Vx_0 = new double[3]; Lat = SINSstate.Latitude; Long = SINSstate.Longitude; SimpleOperations.CopyArray(Vx_0, SINSstate.Vx_0); ProcHelp.distance = Math.Sqrt(Math.Pow((Lat - ProcHelp.LatSNS * SimpleData.ToRadian) * SimpleOperations.RadiusN(Lat, SINSstate.Height), 2) + Math.Pow((Long - ProcHelp.LongSNS * SimpleData.ToRadian) * SimpleOperations.RadiusE(Lat, SINSstate.Height) * Math.Cos(Lat), 2)); ProcHelp.distance_from_start = Math.Sqrt(Math.Pow((Lat - SINSstate.Latitude_Start) * SimpleOperations.RadiusN(Lat, SINSstate.Height), 2) + Math.Pow((Long - SINSstate.Longitude_Start) * SimpleOperations.RadiusE(Lat, SINSstate.Height) * Math.Cos(Lat), 2)); int iMx_kappa_3_ds = SINSstate.value_iMx_kappa_3_ds, iMx_alphaBeta = SINSstate.value_iMx_alphaBeta; // --- Вывод в файл данных, необходимых для формирования GRTV бинарного фалйа --- // if (SINSstate.flag_GRTV_output == true) { if (i == start_i) { StreamWriter GRTV_init_output = new StreamWriter(SimpleData.PathOutputString + "\\TXT for GRTV\\S_GRTV_init_output.txt"); GRTV_init_output.WriteLine( "fSamplingInterval " + SINSstate.timeStep + "\n" + "nInstallationModel 0\n" // 0 – продольная ось прибора совпадает с продольной осью объекта + "flLatitudeID " + SINSstate.Latitude_Start + " 1\n" + "flLongitudeID " + SINSstate.Longitude_Start + " 1\n" + "flHeightID " + SINSstate.Altitude_Start + " 1\n" + "flTrueHeadID " + SINSstate.Heading + " 1\n" //начальная долгота, заданная с пульта [рад] + "flAzimuthMisalignment 0.0 0\n" // Угол азимутального рассогласования + "flElevation 0.0 0" //начальный угол возвышения ИНС [рад] ); GRTV_init_output.Close(); } int modeGRTV = 16; if (i < ProcHelp.AlignmentCounts) { modeGRTV = 8; } GRTV_output.WriteLine( SINSstate.Count + " " + modeGRTV + " " + " " + SINSstate.F_z_orig[1] + " " + SINSstate.F_z_orig[2] + " " + SINSstate.F_z_orig[0] + " " + SINSstate.W_z_orig[1] + " " + SINSstate.W_z_orig[2] + " " + SINSstate.W_z_orig[0] + " " + SINSstate.Latitude + " " + SINSstate.Longitude + " " + SINSstate.Height + " " + SINSstate.Vx_0[1] + " " + SINSstate.Vx_0[0] + " " + SINSstate.Vx_0[2] + " " + SINSstate.Heading + " " + SINSstate.Pitch + " " + SINSstate.Roll + " " + SINSstate.Latitude + " 1 " + SINSstate.Longitude + " 1 " + SINSstate.Height + " 1" + " " + SINSstate.Vx_0[1] + " 1 " + SINSstate.Vx_0[0] + " 1 " + SINSstate.Vx_0[2] + " 1" + " " + SINSstate.OdometerData.odometer_left.Value_orig + " " + SINSstate.OdometerData.odometer_left.isReady_orig // СНС-данные //+ " " + SINSstate.GPS_Data.gps_Latitude.isReady_orig //+ " " + SINSstate.GPS_Data.gps_Latitude.Value_orig + " " + SINSstate.GPS_Data.gps_Latitude.isReady_orig //+ " " + SINSstate.GPS_Data.gps_Longitude.Value_orig + " " + SINSstate.GPS_Data.gps_Longitude.isReady_orig //+ " " + SINSstate.GPS_Data.gps_Altitude.Value_orig + " " + SINSstate.GPS_Data.gps_Altitude.isReady_orig //+ " " + SINSstate.GPS_Data.gps_Vn.Value_orig + " " + SINSstate.GPS_Data.gps_Vn.isReady_orig //+ " " + SINSstate.GPS_Data.gps_Ve.Value_orig + " " + SINSstate.GPS_Data.gps_Vn.isReady_orig // или пока заглушка: + " 0" + " 0" + " 0" + " 0" + " 0" + " 0" + " 0" + " 0" + " 0" + " 0" + " 0" + " " + " 0 0" //Скорость GPS вертикальная ); } /*----------------------------------OUTPUT FEEDBACK------------------------------------------------------*/ if (i % SINSstate.FreqOutput == 0) { ProcHelp.datastring = (SINSstate.Time + SINSstate.Time_Alignment) + " " + SINSstate.Count + " " + SINSstate.OdoTimeStepCount + " " + SimpleOperations.AbsoluteVectorValue(SINSstate.OdoSpeed_s) + " " + Math.Round(((SINSstate.Latitude - SINSstate.Latitude_Start) * SINSstate.R_n), 2) + " " + Math.Round(((SINSstate.Longitude - SINSstate.Longitude_Start) * SINSstate.R_e * Math.Cos(SINSstate.Latitude)), 2) + " " + SINSstate.Height + " " + ((SINSstate.Latitude) * SimpleData.ToDegree) + " " + ((SINSstate.Longitude) * SimpleData.ToDegree) + " " + Math.Round(SINSstate.Vx_0[0], 3) + " " + Math.Round(SINSstate.Vx_0[1], 3) + " " + Math.Round(SINSstate.Vx_0[2], 5) + " " + Math.Round((SINSstate.Heading * SimpleData.ToDegree), 8) + " " + Math.Round((SINSstate.Roll * SimpleData.ToDegree), 8) + " " + Math.Round((SINSstate.Pitch * SimpleData.ToDegree), 8) + " " + ProcHelp.distance + " " + (SINSstate.Height - ProcHelp.AltSNS) + " " + SINSstate.OdometerData.odometer_left.Value + " " + SINSstate.GPS_Data.gps_Latitude_pnppk_sol.Value * SimpleData.ToDegree + " " + SINSstate.GPS_Data.gps_Longitude_pnppk_sol.Value * SimpleData.ToDegree + " " + SINSstate.GPS_Data.gps_Altitude_pnppk_sol.Value ; Nav_FeedbackSolution.WriteLine(ProcHelp.datastring); } if (i % SINSstate.FreqOutput == 0) { /*----------------------------------OUTPUT ERRORS------------------------------------------------------*/ ProcHelp.datastring = (SINSstate.Time + SINSstate.Time_Alignment) + " " + SINSstate.Cumulative_StateErrorVector[0] * SimpleOperations.RadiusN(Lat, SINSstate.Height) + " " + SINSstate.Cumulative_StateErrorVector[1] * SimpleOperations.RadiusE(Lat, SINSstate.Height) * Math.Cos(Lat) + " " + SINSstate.Cumulative_StateErrorVector[2] + " " + SINSstate.Cumulative_StateErrorVector[3] + " " + SINSstate.Cumulative_StateErrorVector[4] + " " + SINSstate.Cumulative_StateErrorVector[5] + " " + SINSstate.Cumulative_StateErrorVector[6] * SimpleData.ToDegree + " " + SINSstate.Cumulative_StateErrorVector[7] * SimpleData.ToDegree + " " + SINSstate.Cumulative_StateErrorVector[8] * SimpleData.ToDegree ; Nav_Errors.WriteLine(ProcHelp.datastring); /*----------------------------------OUTPUT StateErrors------------------------------------------------------*/ int iMx = SimpleData.iMx, iMz = SimpleData.iMz, iMq = SimpleData.iMq, iMx_r12_odo = SINSstate.value_iMx_r_odo_12; int iMx_dV_12 = SINSstate.value_iMx_dV_12, iMx_Nu0 = SINSstate.value_iMx_Nu0, f0_12 = SINSstate.value_iMx_f0_12, f0_3 = SINSstate.value_iMx_f0_3 ; double[] Vertical_ErrorConditionVector = new double[KalmanVars.Vertical_ErrorConditionVector_p.Length]; SimpleOperations.CopyArray(Vertical_ErrorConditionVector, SINSstate.Vertical_Cumulative_KalmanErrorVector); ProcHelp.datastring = (SINSstate.Time + SINSstate.Time_Alignment) + " " + SINSstate.Cumulative_KalmanErrorVector[0] + " " + SINSstate.Cumulative_KalmanErrorVector[1] + " " + Vertical_ErrorConditionVector[0] + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_dV_12 + 0)] + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_dV_12 + 1)] + " " + Vertical_ErrorConditionVector[1] + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_alphaBeta + 0)] * SimpleData.ToDegree + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_alphaBeta + 1)] * SimpleData.ToDegree + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_alphaBeta + 2)] * SimpleData.ToDegree + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_Nu0 + 0)] * SimpleData.ToDegree * 3600.0 + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_Nu0 + 1)] * SimpleData.ToDegree * 3600.0 + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_Nu0 + 2)] * SimpleData.ToDegree * 3600.0 + " " + SINSstate.Cumulative_KalmanErrorVector[(f0_12 + 0)] + " " + SINSstate.Cumulative_KalmanErrorVector[(f0_12 + 1)] + " " + Vertical_ErrorConditionVector[SINSstate.Vertical_f0_3 + 0] ; ProcHelp.datastring = ProcHelp.datastring + " " + SINSstate.Cumulative_KalmanErrorVector[iMx_r12_odo] + " " + SINSstate.Cumulative_KalmanErrorVector[iMx_r12_odo + 1] + " " + Vertical_ErrorConditionVector[SINSstate.Vertical_rOdo3] ; if (SINSstate.Vertical_kappa1 > 0) { ProcHelp.datastring = ProcHelp.datastring + " " + Vertical_ErrorConditionVector[SINSstate.Vertical_kappa1] * SimpleData.ToDegree; } else { ProcHelp.datastring = ProcHelp.datastring + " 0"; } if (iMx_kappa_3_ds > 0) { ProcHelp.datastring = ProcHelp.datastring + " " + SINSstate.Cumulative_KalmanErrorVector[iMx_kappa_3_ds + 0] * SimpleData.ToDegree + " " + SINSstate.Cumulative_KalmanErrorVector[iMx_kappa_3_ds + 1] + " " + (-SINSstate.Cumulative_KalmanErrorVector[(iMx_alphaBeta + 2)] + SINSstate.Cumulative_KalmanErrorVector[iMx_kappa_3_ds + 0]) * SimpleData.ToDegree; } else { ProcHelp.datastring = ProcHelp.datastring + " 0 0"; } Nav_StateErrorsVector.WriteLine(ProcHelp.datastring); // ----------------------------------------------------------// // ----------Вывод в файл для GoogleEarth---------------------// // ----------------------------------------------------------// double[] PhiLambdaH_WGS84 = GeodesicVsGreenwich.Geodesic2Geodesic(SINSstate.Latitude, SINSstate.Longitude, SINSstate.Height, 1); KMLFileOut.WriteLine(PhiLambdaH_WGS84[1] * SimpleData.ToDegree + "," + PhiLambdaH_WGS84[0] * SimpleData.ToDegree + "," + (SINSstate.Time + SINSstate.Time_Alignment) ); double[] PhiLambdaH_WGS84_PNPPK = GeodesicVsGreenwich.Geodesic2Geodesic(SINSstate.GPS_Data.gps_Latitude_pnppk_sol.Value, SINSstate.GPS_Data.gps_Longitude_pnppk_sol.Value, SINSstate.GPS_Data.gps_Altitude_pnppk_sol.Value, 1); KMLFileOut_PNPPK.WriteLine(PhiLambdaH_WGS84_PNPPK[1] * SimpleData.ToDegree + "," + PhiLambdaH_WGS84_PNPPK[0] * SimpleData.ToDegree + "," + (SINSstate.Time + SINSstate.Time_Alignment) ); } if (i % Convert.ToInt32(Math.Round(1.0 / SINSstate.Freq)) == 0) { int iMx = SimpleData.iMx, iMx_r12_odo = SINSstate.value_iMx_r_odo_12; int iMx_dV_12 = SINSstate.value_iMx_dV_12, iMx_Nu0 = SINSstate.value_iMx_Nu0, f0_12 = SINSstate.value_iMx_f0_12, f0_3 = SINSstate.value_iMx_f0_3; int iMxV = SimpleData.iMx_Vertical, vert_f0_3 = SINSstate.Vertical_f0_3, vert_kappa1 = SINSstate.Vertical_kappa1, Vertical_rOdo3 = SINSstate.Vertical_rOdo3; ProcHelp.datastring = (SINSstate.Time + SINSstate.Time_Alignment) + " " + KalmanVars.CovarianceMatrixS_p[0 * iMx + 0] + " " + KalmanVars.CovarianceMatrixS_p[1 * iMx + 1] + " " + KalmanVars.Vertical_CovarianceMatrixS_m[0 * iMxV + 0] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_r12_odo + 0) * iMx + (iMx_r12_odo + 0)] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_r12_odo + 1) * iMx + (iMx_r12_odo + 1)] + " " + KalmanVars.Vertical_CovarianceMatrixS_m[Vertical_rOdo3 * iMxV + Vertical_rOdo3] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_dV_12 + 0) * iMx + (iMx_dV_12 + 0)] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_dV_12 + 1) * iMx + (iMx_dV_12 + 1)] + " " + KalmanVars.Vertical_CovarianceMatrixS_m[1 * iMxV + 1] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_alphaBeta + 0) * iMx + (iMx_alphaBeta + 0)] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_alphaBeta + 1) * iMx + (iMx_alphaBeta + 1)] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_alphaBeta + 2) * iMx + (iMx_alphaBeta + 2)] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_Nu0 + 0) * iMx + (iMx_Nu0 + 0)] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_Nu0 + 1) * iMx + (iMx_Nu0 + 1)] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_Nu0 + 2) * iMx + (iMx_Nu0 + 2)] + " " + KalmanVars.CovarianceMatrixS_p[(f0_12 + 0) * iMx + (f0_12 + 0)] + " " + KalmanVars.CovarianceMatrixS_p[(f0_12 + 1) * iMx + (f0_12 + 1)] + " " + KalmanVars.Vertical_CovarianceMatrixS_m[(vert_f0_3 + 0) * iMxV + (vert_f0_3 + 0)] + " " + KalmanVars.Vertical_CovarianceMatrixS_m[(vert_kappa1 + 0) * iMxV + (vert_kappa1 + 0)] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_kappa_3_ds + 0) * iMx + (iMx_kappa_3_ds + 0)] + " " + KalmanVars.CovarianceMatrixS_p[(iMx_kappa_3_ds + 1) * iMx + (iMx_kappa_3_ds + 1)] ; Nav_CovarianceDiagonal.WriteLine(ProcHelp.datastring); } }