public void IfCollectionDoNotContainsValueWeShouldReturnFalse()
{
int[] collection = new int[] { 1, 2, 3, 4 };
int value = 0;
Assert.False(SimpleOperations.Contains(collection, value));
}
public void IfCollectionContainsValueWeShouldReturnTrue()
{
int[] collection = new int[] { 1, 2, 3, 4 };
int value = 1;
Assert.True(SimpleOperations.Contains(collection, value));
}
public void SumTest()
{
SimpleOperations so = new SimpleOperations();
ViewResult
Assert.AreEqual(20, 30);
}
public static void StateCorrection(double[] ErrorVector, SINS_State SINSstate, SINS_State SINSstate2)
{
//SINSstate2.Altitude = SINSstate.Altitude - SINSstate.DeltaAltitude;
SINSstate2.Latitude = SINSstate.Latitude - SINSstate.DeltaLatitude;
SINSstate2.Longitude = SINSstate.Longitude - SINSstate.DeltaLongitude;
SINSstate2.Vx_0[0] = SINSstate.Vx_0[0] - SINSstate.DeltaV_1;
SINSstate2.Vx_0[1] = SINSstate.Vx_0[1] - SINSstate.DeltaV_2;
SINSstate2.Roll = SINSstate.Roll - SINSstate.DeltaRoll;
SINSstate2.Pitch = SINSstate.Pitch - SINSstate.DeltaPitch;
SINSstate2.Heading = SINSstate.Heading - SINSstate.DeltaHeading;
//корректированная матрица ориентации
SINSstate2.A_sx0 = SimpleOperations.A_sx0(SINSstate);
SINSstate2.A_x0s = SINSstate2.A_sx0.Transpose();
SINSstate2.A_x0n = SimpleOperations.A_x0n(SINSstate.Latitude, SINSstate.Longitude);
SINSstate2.A_nx0 = SINSstate2.A_x0n.Transpose();
}
public void IfCollectionDoNotContainsOddNumbersWeShouldReturnFalse()
{
int[] collection = new int[] { 1, 5, 3, 7 };
Assert.False(SimpleOperations.ConstainsOddNumbers(collection));
}
public void IfCollectionContainsOddNumbersWeShouldReturnTrue()
{
int[] collection = new int[] { 1, 2, 3, 4 };
Assert.True(SimpleOperations.ConstainsOddNumbers(collection));
}
public static void Make_H(Kalman_Vars KalmanVars, SINS_State SINSstate)
{
double[] Wz = new double[3]; double[] Fz = new double[3];
int i = 0;
for (i = 0; i < 3; i++)
{
//Wz[i] = (SINSstate.W_z[i] + SINSstate.W_z_prev[i]) / 2.0;
//Fz[i] = (SINSstate.F_z[i] + SINSstate.F_z_prev[i]) / 2.0;
Wz[i] = SINSstate.W_z[i];
Fz[i] = SINSstate.F_z[i];
}
//Динамические скоростные измерения
KalmanVars.Matrix_H[0 * SimpleData.iMx + 2] = 1.0;
KalmanVars.Matrix_H[1 * SimpleData.iMx + 3] = 1.0;
KalmanVars.Measure[0] = SINSstate.Vx_0[0];
KalmanVars.Measure[1] = SINSstate.Vx_0[1];
KalmanVars.Noize_Z[0] = 0.01;
KalmanVars.Noize_Z[1] = 0.01;
KalmanVars.cnt_measures = 2;
//Позиционные измерения, широта, долгота
if (true)
{
KalmanVars.Matrix_H[KalmanVars.cnt_measures * SimpleData.iMx + 0] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * SimpleData.iMx + 1] = 1.0;
KalmanVars.Measure[KalmanVars.cnt_measures] = (SINSstate.Longitude - SINSstate.Longitude_Start) * Math.Cos(SINSstate.Latitude_Start) * SimpleOperations.RadiusE(SINSstate.Latitude_Start, SINSstate.AltSNS);
KalmanVars.Measure[KalmanVars.cnt_measures + 1] = (SINSstate.Latitude - SINSstate.Latitude_Start) * SimpleOperations.RadiusN(SINSstate.Latitude_Start, SINSstate.AltSNS);
KalmanVars.Noize_Z[KalmanVars.cnt_measures] = 0.75;
KalmanVars.Noize_Z[KalmanVars.cnt_measures + 1] = 0.75;
KalmanVars.cnt_measures = KalmanVars.cnt_measures + 2;
}
//Скалярное измерение по модулю угловой скорости
if (false)
{
KalmanVars.Matrix_H[KalmanVars.cnt_measures * SimpleData.iMx + 7] = 2 * Wz[0];
KalmanVars.Matrix_H[KalmanVars.cnt_measures * SimpleData.iMx + 8] = 2 * Wz[1];
KalmanVars.Matrix_H[KalmanVars.cnt_measures * SimpleData.iMx + 9] = 2 * Wz[2];
KalmanVars.Measure[KalmanVars.cnt_measures] = Math.Pow(SimpleOperations.AbsoluteVectorValue(Wz), 2) - SimpleData.U * SimpleData.U;
KalmanVars.Noize_Z[KalmanVars.cnt_measures] = 0.00001;//0.01 * SimpleData.ToRadian / 3600.0;
KalmanVars.cnt_measures++;
}
//i++;
////Скалярное измерение по модулю силы тяжести
//KalmanVars.Matrix_H[i * SimpleData.iMx + 7] = 2 * Fz[0];
//KalmanVars.Matrix_H[i * SimpleData.iMx + 8] = 2 * Fz[1];
//KalmanVars.Matrix_H[i * SimpleData.iMx + 9] = 2 * Fz[2];
//KalmanVars.Measure[i] = Math.Pow(SimpleOperations.AbsoluteVectorValue(Fz), 2) - SINSstate.g * SINSstate.g;
//KalmanVars.Noize_Z[i] = 0.05;
//KalmanVars.cnt_measures = KalmanVars.cnt_measures + 1;
//Скалярное измерение по модулvю силы тяжести и угловой скорости
if (false)
{
KalmanVars.Matrix_H[KalmanVars.cnt_measures * SimpleData.iMx + 7] = Fz[0];
KalmanVars.Matrix_H[KalmanVars.cnt_measures * SimpleData.iMx + 8] = Fz[1];
KalmanVars.Matrix_H[KalmanVars.cnt_measures * SimpleData.iMx + 9] = Fz[2];
KalmanVars.Matrix_H[KalmanVars.cnt_measures * SimpleData.iMx + 10] = Wz[0];
KalmanVars.Matrix_H[KalmanVars.cnt_measures * SimpleData.iMx + 11] = Wz[1];
KalmanVars.Matrix_H[KalmanVars.cnt_measures * SimpleData.iMx + 12] = Wz[2];
KalmanVars.Measure[KalmanVars.cnt_measures] = SimpleOperations.SkalyarProduct(Fz, Wz) - SimpleData.U * SINSstate.g * Math.Sin(SINSstate.Latitude);
KalmanVars.Noize_Z[KalmanVars.cnt_measures] = 0.001;
KalmanVars.cnt_measures++;
}
Alignment_Scalyar.WriteLine(SINSstate.Count.ToString() + " " + KalmanVars.Measure[0].ToString() + " " + KalmanVars.Measure[1].ToString() + " " + KalmanVars.Measure[2].ToString()
+ " " + KalmanVars.Measure[3].ToString() + " " + KalmanVars.Measure[4].ToString() + " " + KalmanVars.Measure[5].ToString() + " " + KalmanVars.Measure[6].ToString());
//Alignment_Measures.WriteLine(SINSstate.Count.ToString() + " " + Wz[0] + " " + SINSstate.W_z[0].ToString() + " " + Wz[1] + " " + Wz[2] + " " + Fz[0] + " " + Fz[1] + " " + Fz[2]);
/*KalmanVars.Measure[6] = SINSstate.F_x[0];
* KalmanVars.Measure[7] = SINSstate.F_x[1];
* KalmanVars.Matrix_H[6 * SimpleData.iMx + 7] = -SINSstate.g;
* KalmanVars.Matrix_H[6 * SimpleData.iMx + 12] = 1.0;
* KalmanVars.Matrix_H[7 * SimpleData.iMx + 6] = SINSstate.g;
* KalmanVars.Matrix_H[7 * SimpleData.iMx + 13] = 1.0;*/
}
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 void FormatIntegerWithZerosLeftTest()
{
string numFormatted = SimpleOperations.FormatIntegerWithZerosLeft(123);
Assert.IsTrue(numFormatted == "00000123");
}
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 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);
}
/**********************************************************/
//Constructor
public Chef()
{
_spotify = new SpotifyUtils();
_commentsTable = new CommentsTable();
_validations = new SimpleOperations();
}
private void button1_Click(object sender, EventArgs e)
{
int t = 0, moreThanOne = 0, tripling = 0, triplingAbs = 0, doublingAbs = 0, odoCount = 0, flag = 0, left_NULL = 0, right_NULL = 0, doubling_interval = 0, GPS_cnt = 0;
double lastcount = 0, dCount, lOdo = 0, rOdo = 0, dTime = 0.01024, dLeftOdo, dRightOdo, temp = 0;
double[] F_z_lastlast = new double[3], W_z_lastlast = new double[3];
bool GPS_flg = false;
//myFile = new StreamReader("D://Ботва//Postgraduate//1#_Scientific work//1#_Software//1#_Mine//MovingImitator_Azimut//SINS motion processing_new data//All_data//povorot_12-Sep-2012,13-26-38_dat.dat");
//myFile = new StreamReader("D://Ботва//Postgraduate//1#_Scientific work//1#_Software//1#_Mine//Motion Imitator//MovingImitator//SINS motion processing_new data//ktn004_marsh16_repeat_21-Mar-2012,17-21-07_dat.txt");
myFile = new StreamReader(SimpleData.PathInputString + "120222_AzimutB_210530_Race_4_Control_3-6-2_11-49-20_dat.dat"); last_count = 77000;
//myFile = new StreamReader(PathInputString + "120222_AzimutB_210530_Race_2_Adjustment_6-3_DPCnotCal_10-27-49_dat.dat"); last_count = 55370;
//myFile = new StreamReader(PathInputString + "120222_AzimutB_210530_Race_3_Adjustment_6-3_11-19-46_dat.dat"); last_count = 51525;
//myFile = new StreamReader(PathInputString + "120222_AzimutB_210530_Race_5_Control_2-3_12-26-56_dat.dat"); last_count = 66100;
//myFile = new StreamReader(PathInputString + "120222_AzimutB_210530_Race_6_Control_3-2_12-55-50_dat.dat"); last_count = 47000;
//StreamReader myFile = new StreamReader("D://enjoy2.txt");
//myFile.ReadLine();
int doubling_65_80 = 0, doubling_10_20 = 0, doubling_40_65 = 0, doubling_80_ = 0, doubling_20_40 = 0, doubling_0_5 = 0, doubling_5_10 = 0;
//Odo_Explaration.WriteLine("count LeftOdo RightOdo dLeft dRight V_l V_r");
//myFile = new StreamReader("D://Ботва//Postgraduate//1#_Scientific work//1#_Software//1#_Mine//MovingImitator_Azimut//SINS motion processing_new data//All_data//SarOEG_20121213.cnf");
int ii = 0;
SINSstate.Time = 11;
string datastring_prev = "";
datastring_prev = myFile.ReadLine();
//int j = 0, iii=0;
//for (iii = 0; iii < 1411; iii++)
//{
// datastring = myFile.ReadLine();
// if (datastring.Contains("Автор") == true)
// {
// enjoy_parse.WriteLine(datastring_prev);
// }
// datastring_prev = datastring;
//}
//enjoy_parse.Close();
if (false)
{
for (int i = 0; i < last_count; i++)
{
string[] dataArray;
datastring = myFile.ReadLine();
dataArray = datastring.Split(' ');
int tt = 0;
for (int y = 0; y < dataArray.Length; y++)
{
if (dataArray[y] != "")
{
tt++;
}
}
string[] dataArray2 = new string[tt];
tt = 0;
for (int y = 0; y < dataArray.Length; y++)
{
if (dataArray[y] != "")
{
dataArray2[tt] = dataArray[y];
tt++;
}
}
if ((SINSstate.F_z[0] != Convert.ToDouble(dataArray2[16]) && SINSstate.F_z[1] != Convert.ToDouble(dataArray2[17]) && SINSstate.F_z[2] != Convert.ToDouble(dataArray2[18])) || ii == 0)
//if (true)
{
ii++;
SINSstate.Count = ii;
dTime = Convert.ToDouble(dataArray2[0]) - SINSstate.Time;
SINSstate.Time = Convert.ToDouble(dataArray2[0]);
SINSstate.F_z[0] = Convert.ToDouble(dataArray2[16]); SINSstate.W_z[0] = Convert.ToDouble(dataArray2[13]);
SINSstate.F_z[1] = Convert.ToDouble(dataArray2[17]); SINSstate.W_z[1] = Convert.ToDouble(dataArray2[14]);
SINSstate.F_z[2] = Convert.ToDouble(dataArray2[18]); SINSstate.W_z[2] = Convert.ToDouble(dataArray2[15]);
SINSstate.OdoSpeed_x0[1] = Convert.ToDouble(dataArray2[24]);
Odo_Explaration.WriteLine(SINSstate.Count.ToString() + " " + SINSstate.Time.ToString() + " " + dTime.ToString() + " " + SINSstate.F_z[0].ToString() + " " + SINSstate.F_z[1].ToString() + " " + SINSstate.F_z[2].ToString()
+ " " + SINSstate.W_z[0].ToString() + " " + SINSstate.W_z[1].ToString() + " " + SINSstate.W_z[2].ToString() + " " + SINSstate.OdoSpeed_x0[1].ToString());
}
if (i > 10000 && i % 10000 == 0)
{
Console.WriteLine(i.ToString());
}
}
myFile.Close();
}
else if (true)
{
for (int i = 0; i < last_count; i++)
{
SINSstate.timeStep = dTime = 0.02048;
ReadSINSStateFromString();
if (false)
{
if (SINSstate.GPS_Data.gps_Ve.isReady == 1)
{
temp = Math.Sqrt(SINSstate.GPS_Data.gps_Ve.Value * SINSstate.GPS_Data.gps_Ve.Value + SINSstate.GPS_Data.gps_Vn.Value * SINSstate.GPS_Data.gps_Vn.Value);
}
if (SINSstate.OdometerData.odometer_right.isReady == 1)
{
odoCount++;
if (t == 0)
{
t++;
SINSstate.OdometerLeftPrev = SINSstate.OdometerData.odometer_left.Value;
SINSstate.OdometerRightPrev = SINSstate.OdometerData.odometer_right.Value;
continue;
}
//if (SINSstate.Count * SINSstate.timeStep < 590.0) SINSstate.FLG_Stop = 1;
//else if (SINSstate.Count * SINSstate.timeStep > 885.0 && SINSstate.Count * SINSstate.timeStep < 1090.0) SINSstate.FLG_Stop = 1;
//else if (SINSstate.Count * SINSstate.timeStep > 1228.0 && SINSstate.Count * SINSstate.timeStep < 1260.0) SINSstate.FLG_Stop = 1;
//else if (SINSstate.Count * SINSstate.timeStep > 1300.0 && SINSstate.Count * SINSstate.timeStep < 1360.0) SINSstate.FLG_Stop = 1;
//else if (SINSstate.Count * SINSstate.timeStep > 1455.0) SINSstate.FLG_Stop = 1;
//else SINSstate.FLG_Stop = 0;
dLeftOdo = SINSstate.OdometerData.odometer_left.Value - SINSstate.OdometerLeftPrev;
dRightOdo = SINSstate.OdometerData.odometer_right.Value - SINSstate.OdometerRightPrev;
Odo_Explaration.WriteLine(SINSstate.Count.ToString() + " " + odoCount.ToString() + " " + SINSstate.OdometerData.odometer_left.Value.ToString() + " " + SINSstate.OdometerData.odometer_right.Value.ToString() + " " + dLeftOdo.ToString()
+ " " + dRightOdo.ToString() + " " + (dLeftOdo / dTime / odoCount) + " " + (dRightOdo / dTime / odoCount) + " " + SINSstate.FLG_Stop.ToString());
if (i > 30000 && i % 1000 == 0)
{
Console.WriteLine(SINSstate.Count.ToString());
}
SINSstate.OdometerLeftPrev = SINSstate.OdometerData.odometer_left.Value;
SINSstate.OdometerRightPrev = SINSstate.OdometerData.odometer_right.Value;
odoCount = 0;
}
else
{
odoCount++;
}
}
else if (false)
{
if (t == 0)
{
t++;
lastcount = SINSstate.Count - 1;
}
if (SINSstate.GPS_Data.gps_Latitude.Value != 0.0)
{
GPS_cnt++;
}
//else
//{
// GPS_cnt++;
// GPS_flg = true;
//}
dCount = SINSstate.Count - lastcount;
//if (Math.Abs(dCount) < 0.01)
// moreThanOne++;
doubling_interval++;
if (true)
{
if (Math.Abs(SINSstate.F_z[2] - SINSstate.F_z_prev[2]) < 0.00001)
{
int bucket = 0;
if (Math.Abs(SINSstate.F_z[2] - F_z_lastlast[2]) < 0.00001)
{
tripling++;
if (Math.Abs(SINSstate.F_z[1] - F_z_lastlast[1]) < 0.00001)
{
triplingAbs++;
}
}
moreThanOne++;
if (Math.Abs(SINSstate.F_z[1] - SINSstate.F_z_prev[1]) < 0.00001)
{
if (Math.Abs(SINSstate.F_z[0] - SINSstate.F_z_prev[0]) < 0.00001)
{
doublingAbs++;
if (doubling_interval > 65 && doubling_interval <= 75)
{
doubling_65_80++;
bucket = 6;
}
if (doubling_interval > 10 && doubling_interval <= 20)
{
doubling_10_20++;
bucket = 3;
}
if (doubling_interval > 5 && doubling_interval <= 10)
{
doubling_5_10++;
bucket = 2;
}
if (doubling_interval > 1 && doubling_interval <= 4)
{
doubling_0_5++;
bucket = 1;
}
if (doubling_interval > 20 && doubling_interval <= 40)
{
doubling_20_40++;
bucket = 4;
}
if (doubling_interval > 40 && doubling_interval <= 65)
{
doubling_40_65++;
bucket = 5;
}
if (doubling_interval > 80)
{
doubling_80_++;
bucket = 7;
}
//18.7152563310686 0.0617665225447807 0 0.0247066090179123 0.0617665225447807 81.0129709697344 0.123533045089561
//Odo_Explaration.WriteLine(SINSstate.Count.ToString() + " " + SINSstate.F_z[0].ToString() + " " + SINSstate.F_z[1].ToString() + " " + SINSstate.F_z[2].ToString() + " "
// + " " + SINSstate.W_z[0].ToString() + " " + SINSstate.W_z[1].ToString() + " " + SINSstate.W_z[2].ToString() + " "
// + moreThanOne.ToString() + " " + doublingAbs.ToString() + " " + doubling_interval.ToString() + " " + triplingAbs.ToString());
Odo_Explaration.WriteLine(SINSstate.Count.ToString() + " " + SINSstate.F_z_prev[0].ToString() + " " + SINSstate.F_z_prev[1].ToString() + " " + SINSstate.F_z_prev[2].ToString()
+ " " + SINSstate.W_z[0].ToString() + " " + SINSstate.W_z[1].ToString() + " " + SINSstate.W_z[2].ToString()
+ " " + moreThanOne.ToString() + " " + doublingAbs.ToString() + " " + doubling_interval.ToString() + " " + triplingAbs.ToString() + " " + bucket.ToString());
//Console.WriteLine(SINSstate.Count.ToString());
doubling_interval = 0;
}
}
}
}
else
{
if (Math.Abs(SINSstate.W_z[2] - SINSstate.W_z_prev[2]) < 0.000001)
{
int bucket = 0;
if (Math.Abs(SINSstate.W_z[2] - W_z_lastlast[2]) < 0.000001)
{
tripling++;
if (Math.Abs(SINSstate.W_z[1] - W_z_lastlast[1]) < 0.000001)
{
triplingAbs++;
}
}
moreThanOne++;
if (Math.Abs(SINSstate.W_z[1] - SINSstate.W_z_prev[1]) < 0.000001)
{
if (Math.Abs(SINSstate.W_z[0] - SINSstate.W_z_prev[0]) < 0.000001)
{
doublingAbs++;
if (doubling_interval > 65 && doubling_interval <= 75)
{
doubling_65_80++;
bucket = 6;
}
if (doubling_interval > 10 && doubling_interval <= 20)
{
doubling_10_20++;
bucket = 3;
}
if (doubling_interval > 5 && doubling_interval <= 10)
{
doubling_5_10++;
bucket = 2;
}
if (doubling_interval > 1 && doubling_interval <= 4)
{
doubling_0_5++;
bucket = 1;
}
if (doubling_interval > 20 && doubling_interval <= 40)
{
doubling_20_40++;
bucket = 4;
}
if (doubling_interval > 40 && doubling_interval <= 65)
{
doubling_40_65++;
bucket = 5;
}
if (doubling_interval > 80)
{
doubling_80_++;
bucket = 7;
}
//18.7152563310686 0.0617665225447807 0 0.0247066090179123 0.0617665225447807 81.0129709697344 0.123533045089561
//Odo_Explaration.WriteLine(SINSstate.Count.ToString() + " " + SINSstate.F_z[0].ToString() + " " + SINSstate.F_z[1].ToString() + " " + SINSstate.F_z[2].ToString() + " "
// + " " + SINSstate.W_z[0].ToString() + " " + SINSstate.W_z[1].ToString() + " " + SINSstate.W_z[2].ToString() + " "
// + moreThanOne.ToString() + " " + doublingAbs.ToString() + " " + doubling_interval.ToString() + " " + triplingAbs.ToString());
Odo_Explaration.WriteLine(SINSstate.Count.ToString() + " " + SINSstate.F_z_prev[0].ToString() + " " + SINSstate.F_z_prev[1].ToString() + " " + SINSstate.F_z_prev[2].ToString()
+ " " + SINSstate.W_z[0].ToString() + " " + SINSstate.W_z[1].ToString() + " " + SINSstate.W_z[2].ToString()
+ " " + moreThanOne.ToString() + " " + doublingAbs.ToString() + " " + doubling_interval.ToString() + " " + triplingAbs.ToString() + " " + bucket.ToString());
//Console.WriteLine(SINSstate.Count.ToString());
doubling_interval = 0;
}
}
}
}
SimpleOperations.CopyArray(F_z_lastlast, SINSstate.F_z_prev);
SimpleOperations.CopyArray(W_z_lastlast, SINSstate.W_z_prev);
SimpleOperations.CopyArray(SINSstate.F_z_prev, SINSstate.F_z);
SimpleOperations.CopyArray(SINSstate.W_z_prev, SINSstate.W_z);
}
else if (false)
{
if (SINSstate.GPS_Data.gps_Ve.isReady == 1)
{
temp = Math.Sqrt(SINSstate.GPS_Data.gps_Ve.Value * SINSstate.GPS_Data.gps_Ve.Value + SINSstate.GPS_Data.gps_Vn.Value * SINSstate.GPS_Data.gps_Vn.Value);
}
Odo_Explaration.WriteLine(SINSstate.Count.ToString() + " " + SINSstate.GPS_Data.gps_Ve.Value.ToString() + " " + SINSstate.GPS_Data.gps_Vn.Value.ToString() +
" " + (temp).ToString());
if (i > 30000 && i % 1000 == 0)
{
Console.WriteLine(SINSstate.Count.ToString() + " " + moreThanOne.ToString());
}
lastcount = SINSstate.Count;
SimpleOperations.CopyArray(SINSstate.F_z_prev, SINSstate.F_z);
}
else
{
if (SINSstate.OdometerData.odometer_left.isReady == 0)
{
left_NULL++;
}
if (SINSstate.OdometerData.odometer_right.isReady == 0)
{
right_NULL++;
}
SINSstate.OdoTimeStepCount++;
if (true)
{
if ((SINSstate.OdometerData.odometer_left.isReady == 1 && SINSstate.OdometerData.odometer_right.isReady != 1) ||
(SINSstate.OdometerData.odometer_left.isReady != 1 && SINSstate.OdometerData.odometer_right.isReady == 1))
{
flag = 1;
}
else
{
flag = 0;
}
}
if (SINSstate.OdometerData.odometer_left.isReady == 1)
{
Odo_Explaration.WriteLine(SINSstate.Count.ToString() + " " + SINSstate.OdoTimeStepCount.ToString() + " " + SINSstate.OdometerData.odometer_left.Value + " " + SINSstate.OdometerData.odometer_right.Value);
SINSstate.OdoTimeStepCount = 0;
}
if (i > 30000 && i % 1000 == 0)
{
Console.WriteLine(left_NULL.ToString() + " " + right_NULL.ToString());
}
}
}
Console.WriteLine((100 * (double)doubling_0_5 / doublingAbs).ToString() + " " + (100 * (double)doubling_5_10 / doublingAbs).ToString() + " " + (100 * (double)doubling_10_20 / doublingAbs).ToString() + " " + (100 * (double)doubling_20_40 / doublingAbs).ToString() + " " + (100 * (double)doubling_40_65 / doublingAbs).ToString() + " " + (100 * (double)doubling_65_80 / doublingAbs).ToString() + " " + ((100 * (double)doubling_80_ / doublingAbs)).ToString());
}
Odo_Explaration.Close();
this.Close();
}
public static void Make_H_and_Correction(SINS_State SINSstate, Kalman_Align KalmanAlign)
{
KalmanAlign.Matrix_H[0 * SimpleData.iMx_Align + 1] = -SINSstate.g;
KalmanAlign.Matrix_H[1 * SimpleData.iMx_Align + 0] = SINSstate.g;
KalmanAlign.Matrix_H[0 * SimpleData.iMx_Align + 3] = SINSstate.A_x0s[0, 0];
KalmanAlign.Matrix_H[0 * SimpleData.iMx_Align + 4] = SINSstate.A_x0s[0, 1];
KalmanAlign.Matrix_H[0 * SimpleData.iMx_Align + 5] = SINSstate.A_x0s[0, 2];
KalmanAlign.Matrix_H[1 * SimpleData.iMx_Align + 3] = SINSstate.A_x0s[1, 0];
KalmanAlign.Matrix_H[1 * SimpleData.iMx_Align + 4] = SINSstate.A_x0s[1, 1];
KalmanAlign.Matrix_H[1 * SimpleData.iMx_Align + 5] = SINSstate.A_x0s[1, 2];
KalmanAlign.Matrix_H[2 * SimpleData.iMx_Align + 3] = SINSstate.A_x0s[2, 0];
KalmanAlign.Matrix_H[2 * SimpleData.iMx_Align + 4] = SINSstate.A_x0s[2, 1];
KalmanAlign.Matrix_H[2 * SimpleData.iMx_Align + 5] = SINSstate.A_x0s[2, 2];
SimpleOperations.CopyArray(SINSstate.F_x, SINSstate.A_x0s * SINSstate.F_z);
KalmanAlign.Measure[0] = SINSstate.F_x[0];
KalmanAlign.Measure[1] = SINSstate.F_x[1];
KalmanAlign.Measure[2] = SINSstate.F_x[2] - SINSstate.g;
//KalmanAlign.Noize_Z[0] = 0.0003;
//KalmanAlign.Noize_Z[1] = 0.0003;
//KalmanAlign.Noize_Z[2] = 0.0003;
KalmanAlign.Noize_Z[0] = Math.Abs(SINSstate.A_x0s[0, 0] * KalmanAlign.Noise_Vel[0] + SINSstate.A_x0s[0, 1] * KalmanAlign.Noise_Vel[1] + SINSstate.A_x0s[0, 2] * KalmanAlign.Noise_Vel[2]);
KalmanAlign.Noize_Z[1] = Math.Abs(SINSstate.A_x0s[1, 0] * KalmanAlign.Noise_Vel[0] + SINSstate.A_x0s[1, 1] * KalmanAlign.Noise_Vel[1] + SINSstate.A_x0s[1, 2] * KalmanAlign.Noise_Vel[2]);
KalmanAlign.Noize_Z[2] = Math.Abs(SINSstate.A_x0s[2, 0] * KalmanAlign.Noise_Vel[0] + SINSstate.A_x0s[2, 1] * KalmanAlign.Noise_Vel[1] + SINSstate.A_x0s[2, 2] * KalmanAlign.Noise_Vel[2]);
KalmanAlign.cnt_measures = 3;
if (false)
{
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 3] = SINSstate.W_z[0];
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 4] = SINSstate.W_z[1];
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 5] = SINSstate.W_z[2];
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 6] = SINSstate.F_z[0];
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 7] = SINSstate.F_z[1];
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 8] = SINSstate.F_z[2];
KalmanAlign.Measure[KalmanAlign.cnt_measures] = SimpleOperations.SkalyarProduct(SINSstate.F_z, SINSstate.W_z) - SimpleData.U * SINSstate.g * Math.Sin(SINSstate.Latitude);
//KalmanAlign.Noize_Z[KalmanAlign.cnt_measures] = SimpleOperations.SkalyarProduct(SINSstate.F_z, KalmanAlign.Noise_Angl);
KalmanAlign.Noize_Z[KalmanAlign.cnt_measures] = 0.05;
KalmanAlign.cnt_measures++;
}
if (false)
{
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 6] = 2.0 * SINSstate.W_z[0];
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 7] = 2.0 * SINSstate.W_z[1];
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 8] = 2.0 * SINSstate.W_z[2];
KalmanAlign.Measure[KalmanAlign.cnt_measures] = SimpleOperations.SkalyarProduct(SINSstate.W_z, SINSstate.W_z) - SimpleData.U * SimpleData.U;
//KalmanAlign.Noize_Z[KalmanAlign.cnt_measures] = 2.0 * SimpleOperations.SkalyarProduct(SINSstate.W_z, KalmanAlign.Noise_Angl);
KalmanAlign.Noize_Z[KalmanAlign.cnt_measures] = 0.00000000001;
KalmanAlign.cnt_measures++;
}
if (false)
{
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 3] = 2.0 * SINSstate.F_z[0];
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 4] = 2.0 * SINSstate.F_z[1];
KalmanAlign.Matrix_H[KalmanAlign.cnt_measures * SimpleData.iMx_Align + 5] = 2.0 * SINSstate.F_z[2];
KalmanAlign.Measure[KalmanAlign.cnt_measures] = SimpleOperations.SkalyarProduct(SINSstate.F_z, SINSstate.F_z) - SINSstate.g * SINSstate.g;
//KalmanAlign.Noize_Z[KalmanAlign.cnt_measures] = 2.0 * SimpleOperations.SkalyarProduct(SINSstate.F_z, KalmanAlign.Noise_Vel);
KalmanAlign.Noize_Z[KalmanAlign.cnt_measures] = 0.01;
KalmanAlign.cnt_measures++;
}
//SimpleOperations.PrintMatrixToFile(KalmanAlign.Matrix_H, KalmanAlign.cnt_measures, SimpleData.iMx_Align);
KalmanProcs.KalmanCorrection_Align(KalmanAlign);
}
public static void AlignStateIntegration_AT(SINS_State SINSstate, Kalman_Vars KalmanVars, SINS_State SINSstate2, SINS_State SINSstate_OdoMod)
{
double[] fz = new double[3], Wz = 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, dlt, dlt2, kren, tang, gkurs;
SimpleOperations.CopyMatrix(AT_z_xi, SINSstate.AT);
SimpleOperations.CopyMatrix(B_x_eta, SINSstate.A_x0n);
SINSstate.A_nxi = SimpleOperations.A_ne(SINSstate.Time, SINSstate.Longitude_Start);
//C_eta_xi = Matrix.DoA_eta_xi(SINSstate.Time);
SimpleOperations.CopyArray(fz, SINSstate.F_z);
SimpleOperations.CopyArray(Wz, SINSstate.W_z);
SINSstate.R_e = SimpleOperations.RadiusE(SINSstate.Latitude, SINSstate.Height);
SINSstate.R_n = SimpleOperations.RadiusN(SINSstate.Latitude, SINSstate.Height);
SINSstate.u_x = SimpleOperations.U_x0(SINSstate.Latitude);
//-------------ИНТЕГРИРОВАНИЕ МАТРИЦЫ AT_Z_XI И ПЕРВОЕ ВЫЧИСЛЕНИЕ МАТРИЦЫ D_X_Z---------
if (SINSstate.flag_UsingAvegering == true)
{
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);
SimpleOperations.CopyMatrix(dMatrix, (E + Hat1 * dlt + Hat2 * dlt2));
SimpleOperations.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];
}
}
SimpleOperations.CopyMatrix(SINSstate.AT, AT_z_xi);
SimpleOperations.CopyMatrix(W_x_xi, B_x_eta * SINSstate.A_nxi);
SimpleOperations.CopyMatrix(D_x_z, W_x_xi * SINSstate.AT.Transpose());
//--------------------------------------------------------------------------------------
//--- надо вычислять, используется, например в выставке ---//
SINSstate.g = 9.78049 * (1.0 + 0.0053020 * Math.Pow(Math.Sin(SINSstate.Latitude), 2) - 0.000007 * Math.Pow(Math.Sin(2 * SINSstate.Latitude), 2)) - 0.00014;
if (true)
{
SINSstate.g -= 2 * 0.000001538 * SINSstate.Height;
}
//----------------Вычисление углов и переприсвоение матриц---------------------------
SimpleOperations.CopyMatrix(SINSstate.A_sx0, D_x_z.Transpose());
SimpleOperations.CopyMatrix(SINSstate.A_x0s, D_x_z);
SimpleOperations.CopyArray(SINSstate.W_x, SINSstate.A_x0s * Wz);
SimpleOperations.CopyArray(SINSstate.F_z_prev, fz);
SimpleOperations.CopyArray(SINSstate.W_z_prev, Wz);
SimpleOperations.CopyArray(SINSstate.W_z, Wz);
//--------------------------------------------------------------------------------------
}
public void IfCollectionContainsPrimeNumbersWeShouldReturnTrue()
{
int[] collection = new int[] { 2, 3, 5, 11 };
Assert.True(SimpleOperations.ConstainsPrimes(collection));
}
public void IfCollectionDoNotContainsPrimeNumbersWeShouldReturnFalse()
{
int[] collection = new int[] { 1, 6, 12, 9 };
Assert.False(SimpleOperations.ConstainsPrimes(collection));
}
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);
}
