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], f_sum_squared = new double[3]; double[] w_avg = new double[3], w_sum_squared = 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_InputData = new StreamWriter(SimpleData.PathOutputString + "\\Alignment\\Alignment_InputData.txt"); StreamWriter Alignment_avg_rougthMovingAVG = new StreamWriter(SimpleData.PathOutputString + "\\Alignment\\Alignment_avg_rougth_MovingAVG.txt"); // --- вектора СКО double[] sigma_f = new double[3]; double[] sigma_w = new double[3]; Alignment_avg_rougth.WriteLine("time f_1 f_2 f_3 w_1 w_2 w_3 heading roll pitch Latitude"); Alignment_avg_rougthMovingAVG.WriteLine("time MA_f_1 MA_f_2 MA_f_3 MA_w_1 MA_w_2 MA_w_3"); Alignment_InputData.WriteLine("time f_1 f_1_avg f_1_sigma f_2 f_2_avg f_2_sigma f_3 f_3_avg f_3_sigma w_1 w_1_avg w_1_sigma w_2 w_2_avg w_2_sigma w_3 w_3_avg w_3_sigma"); while (true) { i++; if (i < 1) { myFile.ReadLine(); continue; } if (SINSstate.FLG_Stop == 0 && false) { // --- Чтение строки их входного файла с данными и разкладывание по структурам ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, SINSstate, SINSstate_OdoMod); } else { i--; break; } } int t = i; double Latitude = 0.0, Pitch = 0.0, Roll = 0.0, Heading = 0.0; // --- длинна окна для скользящего среднего int MovingWindow = 500; // --- вспомогательные массивы double[] array_f_1 = new double[MovingWindow], array_f_2 = new double[MovingWindow], array_f_3 = new double[MovingWindow]; double[] array_w_1 = new double[MovingWindow], array_w_2 = new double[MovingWindow], array_w_3 = new double[MovingWindow]; // --- Массив скользящих средних для датчиков double[] MovingAverageAccGyro = new double[6]; // --- k_f, k_nu - отдельные счетчики сколько обновлений соответствующих датчиков были использованы для осреднения (в некоторых заездах // --- почему-то ньютонометры на начальной выставке поставляют константное значение) int k_f = 0, k_nu = 0; for (i = t; ; i++) { // --- Чтение строки их входного файла с данными и разкладывание по структурам ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, SINSstate, SINSstate_OdoMod); if ((ProcHelp.AlignmentCounts != 0 && i == ProcHelp.AlignmentCounts)) { break; } int k_mode = k % MovingWindow; array_f_1[k_mode] = SINSstate.F_z[0]; array_f_2[k_mode] = SINSstate.F_z[1]; array_f_3[k_mode] = SINSstate.F_z[2]; array_w_1[k_mode] = SINSstate.W_z[0]; array_w_2[k_mode] = SINSstate.W_z[1]; array_w_3[k_mode] = SINSstate.W_z[2]; // --- Вычисляем среднее значение показаний акселерометров. Цель - детектирование константы в показаниях ньютонометров double tmp_f1_avg = 0.0, tmp_w1_avg = 0.0; int u = 0; for (u = 1; u <= Math.Min(i, 50); u++) { if (k_mode - u < 0) { tmp_f1_avg += array_f_1[MovingWindow + k_mode - u]; } else { tmp_f1_avg += array_f_1[k_mode - u]; } } tmp_f1_avg /= (u - 1); // --- Вычисляем среднее значение показаний ДУСов u = 0; for (u = 1; u <= Math.Min(i, 50); u++) { if (k_mode - u < 0) { tmp_w1_avg += array_w_1[MovingWindow + k_mode - u]; } else { tmp_w1_avg += array_w_1[k_mode - u]; } } tmp_w1_avg /= (u - 1); // --- Если показания датчиков меняются, то заполняем соответствующие массивы if (SINSstate.NoiseParamDetermin_mode != 1 || SINSstate.NoiseParamDetermin_mode == 1 && SINSstate.i_global > SINSstate.NoiseParamDetermin_startTime && SINSstate.i_global < SINSstate.NoiseParamDetermin_endTime) { if (Math.Abs(tmp_f1_avg - array_f_1[k_mode]) > 1E-9) { f_avg[0] += SINSstate.F_z[0]; f_avg[1] += SINSstate.F_z[1]; f_avg[2] += SINSstate.F_z[2]; f_sum_squared[0] += SINSstate.F_z[0] * SINSstate.F_z[0]; f_sum_squared[1] += SINSstate.F_z[1] * SINSstate.F_z[1]; f_sum_squared[2] += SINSstate.F_z[2] * SINSstate.F_z[2]; k_f++; } // --- Если показания датчиков меняются, то заполняем соответствующие массивы if (Math.Abs(tmp_w1_avg - array_w_1[k_mode]) > 1E-9) { w_avg[0] += SINSstate.W_z[0]; w_avg[1] += SINSstate.W_z[1]; w_avg[2] += SINSstate.W_z[2]; w_sum_squared[0] += SINSstate.W_z[0] * SINSstate.W_z[0]; w_sum_squared[1] += SINSstate.W_z[1] * SINSstate.W_z[1]; w_sum_squared[2] += SINSstate.W_z[2] * SINSstate.W_z[2]; k_nu++; } if (SINSstate.i_global % 250 == 0 && k_f > 1 && k_nu > 1) { // --- вычисляем СКО датчиков в процессе sigma_f[0] = Math.Sqrt((f_sum_squared[0] - k_f * Math.Pow(f_avg[0] / k_f, 2)) / (k_f - 1)); sigma_f[1] = Math.Sqrt((f_sum_squared[1] - k_f * Math.Pow(f_avg[1] / k_f, 2)) / (k_f - 1)); sigma_f[2] = Math.Sqrt((f_sum_squared[2] - k_f * Math.Pow(f_avg[1] / k_f, 2)) / (k_f - 1)); sigma_w[0] = Math.Sqrt((w_sum_squared[0] - k_nu * Math.Pow(w_avg[0] / k_nu, 2)) / (k_nu - 1)); sigma_w[1] = Math.Sqrt((w_sum_squared[1] - k_nu * Math.Pow(w_avg[1] / k_nu, 2)) / (k_nu - 1)); sigma_w[2] = Math.Sqrt((w_sum_squared[2] - k_nu * Math.Pow(w_avg[2] / k_nu, 2)) / (k_nu - 1)); } } k++; // --- Вычисление скользящего среднего для его вывода в файл и только SimpleOperations.NullingOfArray(MovingAverageAccGyro); for (int u1 = 1; u1 < Math.Min(k, MovingWindow); u1++) { MovingAverageAccGyro[0] += array_f_1[u1]; MovingAverageAccGyro[1] += array_f_2[u1]; MovingAverageAccGyro[2] += array_f_3[u1]; MovingAverageAccGyro[3] += array_w_1[u1]; MovingAverageAccGyro[4] += array_w_2[u1]; MovingAverageAccGyro[5] += array_w_3[u1]; } for (int u1 = 0; u1 < 6; u1++) { MovingAverageAccGyro[u1] = MovingAverageAccGyro[u1] / (Math.Min(k, MovingWindow) - 1); } // --- Вычисляем текущее значение углов 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); if (Math.Abs(w_avg[0] / k_nu - 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); } // --- Вывод текущих вычисленных параметров в файлы if (k > MovingWindow && k % 10 == 0) { Alignment_avg_rougth.WriteLine(SINSstate.Time.ToString() + " " + (f_avg[0] / Math.Max(k_f, 1)).ToString() + " " + (f_avg[1] / Math.Max(k_f, 1)).ToString() + " " + (f_avg[2] / Math.Max(k_f, 1)).ToString() + " " + (w_avg[0] / Math.Max(k_nu, 1)).ToString() + " " + (w_avg[1] / Math.Max(k_nu, 1)).ToString() + " " + (w_avg[2] / Math.Max(k_nu, 1)).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]); } // --- Вывод в файл показаний датчиков, среднего и сигмы. Для аналитики Alignment_InputData.WriteLine(SINSstate.Time + " " + SINSstate.F_z[0] + " " + f_avg[0] / Math.Max(k_f, 1) + " " + (f_avg[0] / Math.Max(k_f, 1) + sigma_f[0]) + " " + SINSstate.F_z[1] + " " + f_avg[1] / Math.Max(k_f, 1) + " " + (f_avg[1] / Math.Max(k_f, 1) + sigma_f[1]) + " " + SINSstate.F_z[2] + " " + f_avg[2] / Math.Max(k_f, 1) + " " + (f_avg[2] / Math.Max(k_f, 1) + sigma_f[2]) + " " + SINSstate.W_z[0] + " " + w_avg[0] / Math.Max(k_nu, 1) + " " + (w_avg[0] / Math.Max(k_nu, 1) + sigma_w[0]) + " " + SINSstate.W_z[1] + " " + w_avg[1] / Math.Max(k_nu, 1) + " " + (w_avg[1] / Math.Max(k_nu, 1) + sigma_w[1]) + " " + SINSstate.W_z[2] + " " + w_avg[2] / Math.Max(k_nu, 1) + " " + (w_avg[2] / Math.Max(k_nu, 1) + sigma_w[2]) ); // --- Вывод данных для формирования 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 вертикальная ); } } //sigma_mu = Math.Sqrt(sigma_mu); // --- Вычисляем средние значения показаний каждого из датчиков 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; // --- вычисляем СКО датчиков sigma_f[0] = Math.Sqrt((f_sum_squared[0] - k_f * f_avg[0] * f_avg[0]) / (k_f - 1)); sigma_f[1] = Math.Sqrt((f_sum_squared[1] - k_f * f_avg[1] * f_avg[1]) / (k_f - 1)); sigma_f[2] = Math.Sqrt((f_sum_squared[2] - k_f * f_avg[2] * f_avg[2]) / (k_f - 1)); sigma_w[0] = Math.Sqrt((w_sum_squared[0] - k_nu * w_avg[0] * w_avg[0]) / (k_nu - 1)); sigma_w[1] = Math.Sqrt((w_sum_squared[1] - k_nu * w_avg[1] * w_avg[1]) / (k_nu - 1)); sigma_w[2] = Math.Sqrt((w_sum_squared[2] - k_nu * w_avg[2] * w_avg[2]) / (k_nu - 1)); // --- вычисляются шумы ньютонометров и дусов --- // for (int j = 0; j < 3; j++) { // --- Если двигатель на стоянке включен, то уменьшаем шум double decrementNoiseF = 1, decrementNoiseNu = 1; if (SINSstate.AlignmentEngineIsOff == 0) { decrementNoiseF = 4; decrementNoiseNu = 7; } KalmanVars.Noise_Vel[j] = sigma_f[j] / decrementNoiseF; KalmanVars.Noise_Angl[j] = sigma_w[j] / decrementNoiseNu; } // --- Если выбран режим задание конкретных значений сигм шумов датчиков if (SINSstate.NoiseParamDetermin_mode == 2) { for (int j = 0; j < 3; j++) { KalmanVars.Noise_Vel[j] = SINSstate.NoiseParamDetermin_SigmaValueF; KalmanVars.Noise_Angl[j] = SINSstate.NoiseParamDetermin_SigmaValueNu; } } 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]); SINSstate.Heading = -Math.Atan2(w_avg_x[0], w_avg_x[1]); 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]; } // --- алгебраическая калибровка нулей ньютонометров 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 - SINSstate.initError_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 + SINSstate.initError_kappa_1; } 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.AT = Matrix.Multiply(SINSstate.AT, SINSstate.A_nxi); Alignment_avg_rougth.Close(); Alignment_InputData.Close(); Alignment_avg_rougthMovingAVG.Close(); return(i); }
public static void SINS_Autonomous_Processing(int l, StreamReader myFile, SINS_State SINSstate, SINS_State SINSstate2, Kalman_Vars KalmanVars, Proc_Help ProcHelp, SINS_State SINSstate_OdoMod, StreamWriter GRTV_output) { int t = 0; double[,] distance_GK_Sarat = new double[5, 46]; StreamWriter STD_data = new StreamWriter(SimpleData.PathOutputString + "Debaging//S_STD.txt"); StreamWriter ForHelp_2 = new StreamWriter(SimpleData.PathOutputString + "Debaging//ForHelp_2.txt"); StreamWriter Nav_FeedbackSolution = new StreamWriter(SimpleData.PathOutputString + "S_SlnFeedBack.txt"); StreamWriter Nav_EstimateSolution = new StreamWriter(SimpleData.PathOutputString + "S_SlnEstimate.txt"); StreamWriter Nav_Errors = new StreamWriter(SimpleData.PathOutputString + "S_Errors.txt"); StreamWriter Nav_Autonomous = new StreamWriter(SimpleData.PathOutputString + "S_Autonomous.txt"); StreamWriter Nav_StateErrorsVector = new StreamWriter(SimpleData.PathOutputString + "S_ErrVect.txt"); StreamWriter Nav_Smoothed = new StreamWriter(SimpleData.PathOutputString + "S_smoothed_SlnFeedBack.txt"); StreamWriter ForHelp = new StreamWriter(SimpleData.PathOutputString + "Debaging//ForHelp.txt"); StreamWriter KMLFileOut = new StreamWriter(SimpleData.PathOutputString + "KMLFiles//KMLFileOut_Forward.kml"); StreamWriter KMLFileOutSmthd = new StreamWriter(SimpleData.PathOutputString + "KMLFiles//KMLFileOut_Smoothed.kml"); StreamWriter Speed_Angles = new StreamWriter(SimpleData.PathOutputString + "Debaging//Speed_Angles.txt"); StreamWriter DinamicOdometer = new StreamWriter(SimpleData.PathOutputString + "DinamicOdometer.txt"); StreamWriter Cicle_Debag_Solution = new StreamWriter(SimpleData.PathOutputString + "Debaging//Solution_.txt"); Nav_Errors.WriteLine("dLat dLong dV_x1 dV_x2 dV_x3 dHeading dRoll dPitch"); Nav_Autonomous.WriteLine("Time OdoCnt OdoV Latitude Longitude Altitude LatSNS-Lat LngSNS-Lng LatSNS LongSNS LatSNSrad LongSNSrad SpeedSNS V_x1 V_x2 V_x3 Yaw Roll Pitch PosError PosError_Start Azimth"); double[] dS_x = new double[3]; SINSstate2.Latitude = SINSstate.Latitude; SINSstate2.Longitude = SINSstate.Longitude; //---Инициализация начальной матрицы ковариации--- SINSprocessing.InitOfCovarianceMatrixes(SINSstate, KalmanVars); //SINSstate.LastCountForRead = 100000; for (int i = l; i < SINSstate.LastCountForRead; i++) { if (SINSstate.flag_UsingClasAlignment == false) { if (i < ProcHelp.AlignmentCounts) { myFile.ReadLine(); continue; } } ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, null, SINSstate, SINSstate_OdoMod, false); ProcessingHelp.DefSNSData(ProcHelp, SINSstate); if (t == 0) { SimpleOperations.CopyArray(SINSstate.F_z_prev, SINSstate.F_z); SimpleOperations.CopyArray(SINSstate.W_z_prev, SINSstate.W_z); t = 1; } if (SINSstate.OdometerData.odometer_left.isReady != 1) { SINSstate.OdoTimeStepCount++; SINSstate.flag_UsingCorrection = false; //V_increment_SINS = V_increment_SINS + Math.Sqrt(Math.Pow(SINSstate.Vx_0[0] - SINSstate.Vx_0_prev[0], 2) + Math.Pow(SINSstate.Vx_0[1] - SINSstate.Vx_0_prev[1], 2) + Math.Pow(SINSstate.Vx_0[2] - SINSstate.Vx_0_prev[2], 2)); } else if (SINSstate.OdometerData.odometer_left.isReady == 1) { SINSstate.OdometerVector[0] = 0.0; SINSstate.OdometerVector[2] = 0.0; SINSstate.OdoTimeStepCount++; SINSstate.OdometerVector[1] = SINSstate.OdometerData.odometer_left.Value - SINSstate.OdometerLeftPrev; SimpleOperations.CopyArray(dS_x, SINSstate.A_x0s * SINSstate.OdometerVector); SINSstate2.Latitude = SINSstate2.Latitude + dS_x[1] / SINSstate.R_n; SINSstate2.Longitude = SINSstate2.Longitude + dS_x[0] / SINSstate.R_e / Math.Cos(SINSstate2.Latitude); SINSstate.OdometerVector[1] = (SINSstate.OdometerData.odometer_left.Value - SINSstate.OdometerLeftPrev) / SINSstate.OdoTimeStepCount / SINSstate.timeStep; SimpleOperations.CopyArray(SINSstate.OdoSpeed_x0, SINSstate.A_x0s * SINSstate.OdometerVector); SINSstate.flag_UsingCorrection = true; } SINSprocessing.StateIntegration_AT(SINSstate, KalmanVars, SINSstate2, SINSstate_OdoMod); //SINSprocessing.bins(SINSstate); SINSprocessing.Make_A(SINSstate, KalmanVars, SINSstate_OdoMod); //if (SINSstate.OdometerData.odometer_left.isReady == 1) //KalmanProcs.KalmanForecast(KalmanVars); double dT = SINSstate.timeStep; SINSstate.InertialOdometer = SINSstate.InertialOdometer + dT * (SINSstate.InertialOdometer_V + dT * (SINSstate.F_z[1] - SINSstate.g * Math.Sin(SINSstate.Pitch))); SINSstate.InertialOdometer_V = SINSstate.InertialOdometer_V + dT * (SINSstate.F_z[1] - SINSstate.g * Math.Sin(SINSstate.Pitch)); if (i % 10 == 0) { ForHelp_2.WriteLine(SINSstate.Time + " " + SINSstate.InertialOdometer + " " + SINSstate.OdometerData.odometer_left.Value + " " + SINSstate.InertialOdometer_V + " " + SINSstate.OdoSpeed_s[1]); } //SINSstate.OdometerData.odometer_left.Value = SINSstate.InertialOdometer; SimpleOperations.CopyArray(SINSstate.OdoSpeed_x0, SINSstate.A_x0s * SINSstate.OdometerVector); if (i % 10 == 0) { ForHelp.WriteLine(SINSstate.Time + " " + SINSstate.CourseHeading + " " + SINSstate.Heading + " " + SINSstate.CoursePitch + " " + SINSstate.beta_c + " " + SINSstate.alpha_c + " " + SINSstate.gamma_c + " " + SINSstate.OdoSpeed_x0[0] + " " + SINSstate.OdoSpeed_x0[1] + " " + SINSstate.Vx_0[0] + " " + SINSstate.Vx_0[1] + " " + SINSstate2.Vx_0[0] + " " + SINSstate2.Vx_0[1] + " " + SINSstate.A_x0s[0, 1] + " " + SINSstate.A_x0s[1, 1] + " " + SINSstate.A_x0s[2, 1]); } //ForHelp.WriteLine(((SINSstate2.Latitude - SINSstate.Latitude_Start) * SINSstate.R_n).ToString() + " " + ((SINSstate2.Longitude - SINSstate.Longitude_Start) * SINSstate.R_n).ToString()); //ForHelp.WriteLine(SINSstate.Count + " " + SINSstate.A_x0s[0, 0] + " " + SINSstate.A_x0s[1, 1] + " " + SINSstate.A_x0s[2, 2] + " " + SINSstate.A_x0s[0, 1] + " " + SINSstate.A_x0s[0, 2] + " " + SINSstate.A_x0s[1, 2]); /*----------------------------------------END---------------------------------------------*/ /*------------------------------------OUTPUT-------------------------------------------------*/ if (i > 10000 && i % 4000 == 0) { Console.WriteLine(SINSstate.Count.ToString() + ", FromSNS=" + Math.Round(ProcHelp.distance, 2) + " м" + ", FromStart=" + Math.Round(ProcHelp.distance_from_start, 2) + " м" + ", Vx_1=" + Math.Round(SINSstate.Vx_0[0], 2) + ", Vx_2=" + Math.Round(SINSstate.Vx_0[1], 3) ); } ProcessingHelp.OutPutInfo(i, i, ProcHelp, SINSstate, SINSstate, SINSstate2, SINSstate2, KalmanVars, Nav_EstimateSolution, Nav_Autonomous, Nav_FeedbackSolution, Nav_StateErrorsVector, Nav_Errors, STD_data, Speed_Angles, DinamicOdometer, Speed_Angles, KMLFileOut, KMLFileOut, GRTV_output, Cicle_Debag_Solution, Cicle_Debag_Solution); if (SINSstate.OdometerData.odometer_left.isReady == 1) { if (SINSstate.flag_UsingCorrection == true) { SINSstate.OdometerLeftPrev = SINSstate.OdometerData.odometer_left.Value; SINSstate.OdometerRightPrev = SINSstate.OdometerData.odometer_right.Value; SINSstate.OdoTimeStepCount = 0; } } } ForHelp.Close(); Nav_FeedbackSolution.Close(); Nav_EstimateSolution.Close(); Nav_StateErrorsVector.Close(); }
public static int SINS_Alignment_Classical(Proc_Help ProcHelp, SINS_State SINSstate, SINS_State SINSstate2, SINS_State SINSstate_OdoMod, StreamReader myFile, Kalman_Vars KalmanVars, StreamWriter GRTV_output) { int i = 0, t = 0; int iMx = 9; int iMq = 3; int iMz = 7; StreamWriter Alignment_Errors = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_Errors.txt"); StreamWriter Alignment_SINSstate = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_SINSstate.txt"); StreamWriter Alignment_Corrected_State = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_Corrected_State.txt"); StreamWriter Alignment_StateErrorsVector = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_StateErrorsVector.txt"); StreamWriter Alignment_STD_Data = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_STD_Data.txt"); Alignment_Errors.WriteLine("DeltaHeading DeltaRoll DeltaPitch"); Alignment_StateErrorsVector.WriteLine("Time Beta1 Beta2 Beta3 dF1 dF2 dF3 Nu1 Nu2 Nu3 "); Alignment_SINSstate.WriteLine("Time Count Lat Long Altitude V1 V2 Heading HeadingCor Roll RollCor Pitch PitchCor"); bool exist_real_Classical_Alignment = false; int AlignmentCounts_tmp = ProcHelp.AlignmentCounts; //---Этап грубой выставки--- //int temp_AlgnCnt = ProcHelp.AlgnCnt; //ProcHelp.AlgnCnt = Convert.ToInt32(120.0 / SINSstate.Freq); if (exist_real_Classical_Alignment == true) { ProcHelp.AlignmentCounts = Convert.ToInt32(Math.Round(ProcHelp.AlignmentCounts / 3.0)); SINSstate.Alignment_HeadingDetermined = false; } i = Alignment.RougthAlignment(ProcHelp, SINSstate, myFile, KalmanVars, SINSstate_OdoMod, GRTV_output); if (exist_real_Classical_Alignment == true) { ProcHelp.AlignmentCounts = AlignmentCounts_tmp; } //ProcHelp.AlgnCnt = temp_AlgnCnt; SINSstate.flag_Alignment = true; if (exist_real_Classical_Alignment) { Kalman_Align KalmanAlign = new Kalman_Align(); SimpleOperations.CopyArray(KalmanAlign.Noise_Vel, KalmanVars.Noise_Vel); SimpleOperations.CopyArray(KalmanAlign.Noise_Angl, KalmanVars.Noise_Angl); Alignment_Classical.InitOfCovarianceMatrixes(KalmanAlign); for (int j = i; j < ProcHelp.AlignmentCounts; j++) { ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, null, SINSstate, SINSstate_OdoMod, true); Alignment_Classical.AlignStateIntegration_AT(SINSstate, KalmanVars, SINSstate2, SINSstate_OdoMod); Alignment_Classical.Make_A(SINSstate, KalmanAlign); Alignment_Classical.MatrixNoise_ReDef(SINSstate, KalmanAlign); KalmanProcs.Make_F_Align(SINSstate.timeStep, KalmanAlign); Alignment_Classical.Make_H_and_Correction(SINSstate, KalmanAlign); KalmanProcs.KalmanForecast_Align(KalmanAlign); i = j; if (j % 200 == 0) { Console.WriteLine(SINSstate.Count.ToString() + ", " + (SINSstate.Latitude * SimpleData.ToDegree - ProcHelp.LatSNS).ToString() + ", " + SINSstate.F_x[2].ToString().ToString()); } SINSstate.DeltaRoll = -(KalmanAlign.ErrorConditionVector_p[0] * Math.Sin(SINSstate.Heading) + KalmanAlign.ErrorConditionVector_p[1] * Math.Cos(SINSstate.Heading)) / Math.Cos(SINSstate.Pitch); SINSstate.DeltaPitch = -KalmanAlign.ErrorConditionVector_p[0] * Math.Cos(SINSstate.Heading) + KalmanAlign.ErrorConditionVector_p[1] * Math.Sin(SINSstate.Heading); SINSstate.DeltaHeading = KalmanAlign.ErrorConditionVector_p[2] + SINSstate.DeltaRoll * Math.Sin(SINSstate.Pitch); Alignment_Errors.WriteLine(SINSstate.DeltaHeading * 180.0 / 3.141592 + " " + SINSstate.DeltaRoll * 180.0 / 3.141592 + " " + SINSstate.DeltaPitch * 180.0 / 3.141592); Alignment_STD_Data.WriteLine(KalmanProcs.Sigmf_Disp(0, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(1, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(2, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(3, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(4, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(5, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(6, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(7, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(8, KalmanAlign)); Alignment.OutPutInfo_Class_Alignment(ProcHelp, SINSstate, SINSstate2, myFile, KalmanAlign, Alignment_Errors, Alignment_SINSstate, Alignment_Corrected_State, Alignment_StateErrorsVector); } SimpleOperations.PrintMatrixToFile(KalmanAlign.CovarianceMatrixS_p, SimpleData.iMx_Align, SimpleData.iMx_Align); SINSstate.Heading = SINSstate.Heading - SINSstate.DeltaHeading; SINSstate.Roll = SINSstate.Roll - SINSstate.DeltaRoll; SINSstate.Pitch = SINSstate.Pitch - SINSstate.DeltaPitch; //KalmanVars.CovarianceMatrixS_m[4 * SimpleData.iMx + 4] = KalmanVars.CovarianceMatrixS_p[4 * SimpleData.iMx + 4] = KalmanProcs.Sigmf_Disp(0, KalmanAlign); //KalmanVars.CovarianceMatrixS_m[5 * SimpleData.iMx + 5] = KalmanVars.CovarianceMatrixS_p[5 * SimpleData.iMx + 5] = KalmanProcs.Sigmf_Disp(1, KalmanAlign); //KalmanVars.CovarianceMatrixS_m[6 * SimpleData.iMx + 6] = KalmanVars.CovarianceMatrixS_p[6 * SimpleData.iMx + 6] = KalmanProcs.Sigmf_Disp(2, KalmanAlign); //KalmanVars.CovarianceMatrixS_m[10 * SimpleData.iMx + 10] = KalmanVars.CovarianceMatrixS_p[10 * SimpleData.iMx + 10] = KalmanProcs.Sigmf_Disp(3, KalmanAlign); //KalmanVars.CovarianceMatrixS_m[11 * SimpleData.iMx + 11] = KalmanVars.CovarianceMatrixS_p[11 * SimpleData.iMx + 11] = KalmanProcs.Sigmf_Disp(4, KalmanAlign); //KalmanVars.CovarianceMatrixS_m[12 * SimpleData.iMx + 12] = KalmanVars.CovarianceMatrixS_p[12 * SimpleData.iMx + 12] = KalmanProcs.Sigmf_Disp(5, KalmanAlign); //KalmanVars.CovarianceMatrixS_m[7 * SimpleData.iMx + 7] = KalmanVars.CovarianceMatrixS_p[7 * SimpleData.iMx + 7] = KalmanProcs.Sigmf_Disp(6, KalmanAlign); //KalmanVars.CovarianceMatrixS_m[8 * SimpleData.iMx + 8] = KalmanVars.CovarianceMatrixS_p[8 * SimpleData.iMx + 8] = KalmanProcs.Sigmf_Disp(7, KalmanAlign); //KalmanVars.CovarianceMatrixS_m[9 * SimpleData.iMx + 9] = KalmanVars.CovarianceMatrixS_p[9 * SimpleData.iMx + 9] = KalmanProcs.Sigmf_Disp(8, KalmanAlign); SINSstate.Time_Alignment = SINSstate.Time; 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.A_nxi = SimpleOperations.A_ne(SINSstate.Time - SINSstate.Time_Alignment, SINSstate.Longitude_Start); SINSstate.AT = Matrix.Multiply(SINSstate.A_sx0, SINSstate.A_x0n); SINSstate.AT = Matrix.Multiply(SINSstate.AT, SINSstate.A_nxi); /*----------------------------------------------------------------------------------------*/ } Alignment_Errors.Close(); Alignment_Corrected_State.Close(); Alignment_SINSstate.Close(); Alignment_StateErrorsVector.Close(); Alignment_STD_Data.Close(); ProcHelp.initCount = false; SINSstate.flag_Alignment = false; return(i); }
public static int SINS_Alignment_Navigation(Proc_Help ProcHelp, SINS_State SINSstate, SINS_State SINSstate2, SINS_State SINSstate_OdoMod, StreamReader myFile, Kalman_Vars KalmanVars, StreamWriter GRTV_output) { int i = 0, t = 0; SimpleData.iMx = 13; SimpleData.iMq = 5; SimpleData.iMz = 7; StreamWriter Alignment_Errors = new StreamWriter(SimpleData.PathOutputString + "Alignment_Errors.txt"); StreamWriter Alignment_SINSstate = new StreamWriter(SimpleData.PathOutputString + "Alignment_SINSstate.txt"); StreamWriter Alignment_Corrected_State = new StreamWriter(SimpleData.PathOutputString + "Alignment_Corrected_State.txt"); StreamWriter Alignment_StateErrorsVector = new StreamWriter(SimpleData.PathOutputString + "Alignment_StateErrorsVector.txt"); Alignment_Errors.WriteLine("dR1 dR2 dV1 dV2 Alpha1 Alpha2 Beta3 Nu1 Nu2 Nu3 dF1 dF2 dF3"); Alignment_Corrected_State.WriteLine("Time Count LatCrtd Lat LongCrtd Long AltitudeCrtd V1 V2 V3 Heading HeadingCor Roll RollCor Pitch PitchCor"); //---Этап грубой выставки--- //int temp_AlgnCnt = ProcHelp.AlgnCnt; //ProcHelp.AlgnCnt = Convert.ToInt32(200.0 / SINSstate.Freq); i = Alignment.RougthAlignment(ProcHelp, SINSstate, myFile, KalmanVars, SINSstate_OdoMod, GRTV_output); //ProcHelp.AlgnCnt = temp_AlgnCnt; SINSstate.flag_Alignment = true; if (false) { Alignment_Navigation.InitOfCovarianceMatrixes(KalmanVars); //---Инициализация ковариационных матриц матриц вектора ошибок---// /*----------------------------------------------------------------------------------------*/ while (true) { if (SINSstate.FLG_Stop == 0 || (ProcHelp.AlignmentCounts != 0 && i == ProcHelp.AlignmentCounts)) { break; } ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, null, SINSstate, SINSstate_OdoMod, true); if (t == 0) { SimpleOperations.CopyArray(SINSstate.F_z_prev, SINSstate.F_z); SimpleOperations.CopyArray(SINSstate.W_z_prev, SINSstate.W_z); t = 1; } SINSprocessing.StateIntegration_AT(SINSstate, KalmanVars, SINSstate2, SINSstate2); Alignment_Navigation.MatrixNoise_ReDef(SINSstate, KalmanVars); //изменить все эти функции Alignment_Navigation.Make_A_easy(SINSstate2, KalmanVars); KalmanProcs.Make_F(SINSstate.timeStep, KalmanVars, SINSstate); KalmanProcs.KalmanForecast(KalmanVars, SINSstate); Alignment_Navigation.Make_H(KalmanVars, SINSstate); KalmanProcs.KalmanCorrection(KalmanVars, SINSstate, SINSstate); Alignment_Navigation.CalcStateErrors(KalmanVars.ErrorConditionVector_p, SINSstate); Alignment_Navigation.StateCorrection(KalmanVars.ErrorConditionVector_p, SINSstate, SINSstate2); Alignment.OutPutInfo_Nav_Alignment(ProcHelp, SINSstate, SINSstate2, myFile, KalmanVars, Alignment_Errors, Alignment_SINSstate, Alignment_Corrected_State, Alignment_StateErrorsVector); SimpleOperations.CopyArray(SINSstate.F_z_prev, SINSstate.F_z); SimpleOperations.CopyArray(SINSstate.W_z_prev, SINSstate.W_z); if (i > 100 && i % 500 == 0) { Console.WriteLine(SINSstate.Count.ToString() + ", " + (SINSstate.Longitude * SimpleData.ToDegree).ToString() + ", " + (SINSstate.Heading * SimpleData.ToDegree).ToString() + ", " + (SINSstate2.Heading * SimpleData.ToDegree).ToString() + ", " + KalmanVars.ErrorConditionVector_p[0].ToString() + ", " + KalmanVars.ErrorConditionVector_p[1].ToString()); } i++; } SINSstate.Heading = SINSstate.Heading - SINSstate.DeltaHeading; SINSstate.Roll = SINSstate.Roll - SINSstate.DeltaRoll; SINSstate.Pitch = SINSstate.Pitch - SINSstate.DeltaPitch; KalmanVars.CovarianceMatrixS_m[0 * SimpleData.iMx + 0] = KalmanVars.CovarianceMatrixS_p[0 * SimpleData.iMx + 0] = KalmanProcs.Sigmf_Disp(0, KalmanVars); KalmanVars.CovarianceMatrixS_m[1 * SimpleData.iMx + 1] = KalmanVars.CovarianceMatrixS_p[1 * SimpleData.iMx + 1] = KalmanProcs.Sigmf_Disp(1, KalmanVars); KalmanVars.CovarianceMatrixS_m[2 * SimpleData.iMx + 2] = KalmanVars.CovarianceMatrixS_p[2 * SimpleData.iMx + 2] = KalmanProcs.Sigmf_Disp(2, KalmanVars); KalmanVars.CovarianceMatrixS_m[3 * SimpleData.iMx + 3] = KalmanVars.CovarianceMatrixS_p[3 * SimpleData.iMx + 3] = KalmanProcs.Sigmf_Disp(3, KalmanVars); KalmanVars.CovarianceMatrixS_m[10 * SimpleData.iMx + 10] = KalmanVars.CovarianceMatrixS_p[10 * SimpleData.iMx + 10] = KalmanProcs.Sigmf_Disp(10, KalmanVars); KalmanVars.CovarianceMatrixS_m[11 * SimpleData.iMx + 11] = KalmanVars.CovarianceMatrixS_p[11 * SimpleData.iMx + 11] = KalmanProcs.Sigmf_Disp(11, KalmanVars); KalmanVars.CovarianceMatrixS_m[12 * SimpleData.iMx + 12] = KalmanVars.CovarianceMatrixS_p[12 * SimpleData.iMx + 12] = KalmanProcs.Sigmf_Disp(12, KalmanVars); KalmanVars.CovarianceMatrixS_m[7 * SimpleData.iMx + 7] = KalmanVars.CovarianceMatrixS_p[7 * SimpleData.iMx + 7] = KalmanProcs.Sigmf_Disp(7, KalmanVars); KalmanVars.CovarianceMatrixS_m[8 * SimpleData.iMx + 8] = KalmanVars.CovarianceMatrixS_p[8 * SimpleData.iMx + 8] = KalmanProcs.Sigmf_Disp(8, KalmanVars); KalmanVars.CovarianceMatrixS_m[9 * SimpleData.iMx + 9] = KalmanVars.CovarianceMatrixS_p[9 * SimpleData.iMx + 9] = KalmanProcs.Sigmf_Disp(9, KalmanVars); SINSstate.Time_Alignment = SINSstate.Time; // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // if (SINSstate.Global_file == "Azimuth_minsk_race_4_3to6to2") { //SINSstate.Heading = -3.0504734; } // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // 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.A_nxi = SimpleOperations.A_ne(SINSstate.Time - SINSstate.Time_Alignment, SINSstate.Longitude_Start); SINSstate.AT = Matrix.Multiply(SINSstate.A_sx0, SINSstate.A_x0n); SINSstate.AT = Matrix.Multiply(SINSstate.AT, SINSstate.A_nxi); Alignment_Errors.Close(); Alignment_Corrected_State.Close(); Alignment_SINSstate.Close(); Alignment_StateErrorsVector.Close(); /*----------------------------------------------------------------------------------------*/ } ProcHelp.initCount = false; SINSstate.flag_Alignment = false; return(i); }