public static void OutPutInfo_Nav_Alignment(Proc_Help ProcHelp, SINS_State SINSstate, SINS_State SINSstate2, StreamReader myFile, Kalman_Vars KalmanVars,
StreamWriter Alignment_Errors, StreamWriter Alignment_SINSstate, StreamWriter Alignment_Corrected_State, StreamWriter Alignment_StateErrorsVector)
{
if (SINSstate.Count % SINSstate.FreqOutput == 0)
{
ProcHelp.datastring = KalmanVars.ErrorConditionVector_p[0].ToString() + " " + KalmanVars.ErrorConditionVector_p[1].ToString()
+ " " + KalmanVars.ErrorConditionVector_p[2].ToString() + " " + KalmanVars.ErrorConditionVector_p[3].ToString()
+ " " + (KalmanVars.ErrorConditionVector_p[4] * 180.0 / 3.141592).ToString() + " " + (KalmanVars.ErrorConditionVector_p[5] * 180.0 / 3.141592).ToString() + " " + (KalmanVars.ErrorConditionVector_p[6] * 180.0 / 3.141592).ToString()
+ " " + KalmanVars.ErrorConditionVector_p[7].ToString() + " " + KalmanVars.ErrorConditionVector_p[8].ToString() + " " + KalmanVars.ErrorConditionVector_p[9].ToString()
+ " " + KalmanVars.ErrorConditionVector_p[10].ToString() + " " + KalmanVars.ErrorConditionVector_p[11].ToString() + " " + KalmanVars.ErrorConditionVector_p[12].ToString();
Alignment_Errors.WriteLine(ProcHelp.datastring);
ProcHelp.datastring = (SINSstate.Count * SINSstate.timeStep).ToString() + " " + SINSstate.Count.ToString() + " " +
(SINSstate.Latitude * SimpleData.ToDegree).ToString() + " " + (SINSstate.Longitude * SimpleData.ToDegree).ToString() + " " + SINSstate.Height.ToString() + " "
+ ProcHelp.LatSNS.ToString() + " " + ProcHelp.LongSNS.ToString() + " " + SINSstate.Vx_0[0].ToString() + " " + SINSstate.Vx_0[1].ToString() + " " + (SINSstate.Heading * SimpleData.ToDegree).ToString() + " "
+ (SINSstate.Roll * SimpleData.ToDegree).ToString() + " " + (SINSstate.Pitch * SimpleData.ToDegree).ToString();
Alignment_SINSstate.WriteLine(ProcHelp.datastring);
ProcHelp.datastring = (SINSstate.Count * SINSstate.timeStep).ToString() + " " + SINSstate.Count.ToString() + " " + (SINSstate2.Latitude * SimpleData.ToDegree).ToString() + " " + (SINSstate.Latitude * SimpleData.ToDegree).ToString()
+ " " + (SINSstate2.Longitude * SimpleData.ToDegree).ToString() + " " + (SINSstate.Longitude * SimpleData.ToDegree).ToString() + " " + SINSstate2.Height.ToString() + " "
+ SINSstate2.Vx_0[0].ToString() + " " + SINSstate2.Vx_0[1].ToString() + " " + SINSstate2.Vx_0[2].ToString() + " " +
(SINSstate.Heading * SimpleData.ToDegree).ToString() + " " + (SINSstate2.Heading * SimpleData.ToDegree).ToString() + " "
+ (SINSstate.Roll * SimpleData.ToDegree).ToString() + " " + (SINSstate2.Roll * SimpleData.ToDegree).ToString() + " "
+ (SINSstate.Pitch * SimpleData.ToDegree).ToString() + " " + (SINSstate2.Pitch * SimpleData.ToDegree).ToString();
Alignment_Corrected_State.WriteLine(ProcHelp.datastring);
ProcHelp.datastring = (SINSstate.DeltaLatitude * SimpleData.ToDegree).ToString() + " " + (SINSstate.DeltaLongitude * SimpleData.ToDegree).ToString() + " " + SINSstate.DeltaV_1.ToString() + " " + SINSstate.DeltaV_2.ToString() + " "
+ SINSstate.DeltaV_3.ToString() + " " + SINSstate.DeltaHeading.ToString() + " " + SINSstate.DeltaRoll.ToString() + " " + SINSstate.DeltaPitch.ToString();
Alignment_StateErrorsVector.WriteLine(ProcHelp.datastring);
}
}
public static void OutPutInfo_Class_Alignment(Proc_Help ProcHelp, SINS_State SINSstate, SINS_State SINSstate2, StreamReader myFile, Kalman_Align KalmanAlign,
StreamWriter Alignment_Errors, StreamWriter Alignment_SINSstate, StreamWriter Alignment_Corrected_State, StreamWriter Alignment_StateErrorsVector)
{
if (SINSstate.Count % SINSstate.FreqOutput == 0)
{
ProcHelp.datastring = (SINSstate.Count * SINSstate.timeStep).ToString()
+ " " + (KalmanAlign.ErrorConditionVector_p[0] * 180.0 / 3.141592).ToString()
+ " " + (KalmanAlign.ErrorConditionVector_p[1] * 180.0 / 3.141592).ToString()
+ " " + (KalmanAlign.ErrorConditionVector_p[2] * 180.0 / 3.141592).ToString()
+ " " + KalmanAlign.ErrorConditionVector_p[3].ToString()
+ " " + (KalmanAlign.ErrorConditionVector_p[4]).ToString()
+ " " + (KalmanAlign.ErrorConditionVector_p[5]).ToString()
+ " " + (KalmanAlign.ErrorConditionVector_p[6]).ToString()
+ " " + KalmanAlign.ErrorConditionVector_p[7].ToString()
+ " " + KalmanAlign.ErrorConditionVector_p[8].ToString()
;
Alignment_StateErrorsVector.WriteLine(ProcHelp.datastring);
ProcHelp.datastring = (SINSstate.Count * SINSstate.timeStep).ToString()
+ " " + SINSstate.Count.ToString()
+ " " + (SINSstate.Latitude * SimpleData.ToDegree).ToString()
+ " " + (SINSstate.Longitude * SimpleData.ToDegree).ToString() + " " + SINSstate.Height.ToString() + " "
+ SINSstate.Vx_0[0].ToString() + " " + SINSstate.Vx_0[1].ToString() + " "
+ (SINSstate.Heading * SimpleData.ToDegree).ToString() + " " + " " + ((SINSstate.Heading - SINSstate.DeltaHeading) * SimpleData.ToDegree).ToString() + " "
+ (SINSstate.Roll * SimpleData.ToDegree).ToString() + " " + ((SINSstate.Roll - SINSstate.DeltaRoll) * SimpleData.ToDegree).ToString() + " "
+ (SINSstate.Pitch * SimpleData.ToDegree).ToString() + " " + ((SINSstate.Pitch - SINSstate.DeltaPitch) * SimpleData.ToDegree).ToString();
Alignment_SINSstate.WriteLine(ProcHelp.datastring);
}
}
/*-------------------------------Вспомогательные функции---------------------------------------------------------*/
public static void DefSNSData(Proc_Help ProcHelp, SINS_State SINSstate)
{
//if (SINSstate.GPS_Data.gps_Latitude.isReady == 1)
if (SINSstate.GPS_Data.gps_Latitude.Value > 0.1)
{
ProcHelp.LatSNS = SINSstate.GPS_Data.gps_Latitude.Value * 180 / Math.PI;
ProcHelp.LongSNS = SINSstate.GPS_Data.gps_Longitude.Value * 180 / Math.PI;
ProcHelp.AltSNS = SINSstate.GPS_Data.gps_Altitude.Value;
ProcHelp.SpeedSNS = 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);
ProcHelp.Ve_SNS = SINSstate.GPS_Data.gps_Ve.Value;
ProcHelp.Vn_SNS = SINSstate.GPS_Data.gps_Vn.Value;
}
}
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 Make_H_POSITION(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod, Proc_Help ProcHelp)
{
int iMx = SimpleData.iMx, iMz = SimpleData.iMz, iMq = SimpleData.iMq, iMx_kappa_3_ds = SINSstate.value_iMx_kappa_3_ds, iMx_kappa_1 = SINSstate.value_iMx_kappa_1,
iMx_r12_odo = SINSstate.value_iMx_r_odo_12, value_iMx_dr3 = SINSstate.value_iMx_dr3, value_iMx_dV3 = SINSstate.value_iMx_dV3;
int iMx_dV_12 = SINSstate.value_iMx_dV_12,
iMx_alphaBeta = SINSstate.value_iMx_alphaBeta,
iMx_Nu0 = SINSstate.value_iMx_Nu0,
f0_12 = SINSstate.value_iMx_f0_12,
f0_3 = SINSstate.value_iMx_f0_3,
iMx_r_odo_3 = SINSstate.value_iMx_r_odo_3
;
double[] tempVect = new double[3];
// --- шум измерения. Если объект стоит - уменьшаем
double Noize = 1.0;
if (SINSstate.global_odo_measure_noise > 0.001)
{
Noize = SINSstate.global_odo_measure_noise;
}
double longOdoIncrement = SINSstate.OdometerData.odometer_left.Value - SINSstate.OdometerLeft_ArrayOfPrev[Math.Min(20, SINSstate.OdometerLeft_ArrayOfPrev.Length)];
double longOdoIncrement_dt = SINSstate.Time + SINSstate.Time_Alignment - SINSstate.OdometerLeft_ArrayOfPrevTime[Math.Min(20, SINSstate.OdometerLeft_ArrayOfPrev.Length)];
if (longOdoIncrement / longOdoIncrement_dt == 0.0)
{
Noize = 0.01;
// -- Блок формирования измерений по разнице координат БИНС и внешней информации
//KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 0] = 1.0;
//KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + 1] = 1.0;
////Формирование измерений по географическим координатам
//KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = (SINSstate.Longitude - SINSstate_OdoMod.Longitude) * SINSstate.R_e * Math.Cos(SINSstate.Latitude);
//KalmanVars.Measure[(KalmanVars.cnt_measures + 1)] = (SINSstate.Latitude - SINSstate_OdoMod.Latitude) * SINSstate.R_n;
//// --- шум измерения
//KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 0.1;
//KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = 0.1;
//KalmanVars.cnt_measures += 2;
//KalmanVars.Vertical_Matrix_H[(KalmanVars.Vertical_cnt_measures + 0) * SimpleData.iMx_Vertical + 0] = 1.0;
//KalmanVars.Vertical_Measure[(KalmanVars.Vertical_cnt_measures + 0)] = SINSstate.Height - SINSstate_OdoMod.Height;
//KalmanVars.Vertical_Noize_Z[(KalmanVars.Vertical_cnt_measures + 0)] = 0.1;
//KalmanVars.Vertical_cnt_measures += 1;
//// -- Блок формирования измерений по разнице координат одометрического решения и внешней информации
//KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r12_odo + 0] = 1.0;
//KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + iMx_r12_odo + 1] = 1.0;
////Формирование измерений по географическим координатам
//KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = (SINSstate_OdoMod.Longitude - Longitude_CP) * SINSstate_OdoMod.R_e * Math.Cos(SINSstate_OdoMod.Latitude);
//KalmanVars.Measure[(KalmanVars.cnt_measures + 1)] = (SINSstate_OdoMod.Latitude - Latitude_CP) * SINSstate_OdoMod.R_n;
//// --- шум измерения
//KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = Noize;
//KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = Noize;
//KalmanVars.cnt_measures += 2;
//if (Convert.ToInt32(SINSstate.Count) % 50 == 0)
//{
// KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + (iMx_dV_12 + 0)] = 1.0;
// KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + (iMx_dV_12 + 1)] = 1.0;
// for (int i = 0; i < 3; i++)
// KalmanVars.Measure[(KalmanVars.cnt_measures + i)] = SINSstate.Vx_0[i];
// KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 0.1;
// KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = 0.1;
// KalmanVars.cnt_measures += 2;
//}
}
//---Разбиение на три составляющие---
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 0] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + 1] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r12_odo + 0] = -1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + iMx_r12_odo + 1] = -1.0;
//Формирование измерений по географическим координатам
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = (SINSstate.Longitude - SINSstate_OdoMod.Longitude) * SINSstate.R_e * Math.Cos(SINSstate.Latitude);
KalmanVars.Measure[(KalmanVars.cnt_measures + 1)] = (SINSstate.Latitude - SINSstate_OdoMod.Latitude) * SINSstate.R_n;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = Noize;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = Noize;
KalmanVars.cnt_measures += 2;
//---ДОПОЛНИТЕЛЬНОЕ измерение по вертикальной скорости---
if (SINSstate.flag_iMx_r3_dV3)
{
if (SINSstate.add_velocity_to_position)
{
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + value_iMx_dV3] = 1.0;
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Vx_0[2] - SINSstate_OdoMod.Vx_0[2];
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = SINSstate.A_x0s[2, 1] * KalmanVars.OdoNoise_V;
if (SINSstate.flag_iMx_kappa_13_ds)
{
if (iMx_kappa_1 > 0)
{
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_kappa_1 + 0] = -SINSstate.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[2, 2] + SINSstate.OdoSpeed_s[2] * SINSstate_OdoMod.A_x0s[2, 1];
}
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_kappa_3_ds + 0] = SINSstate.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[2, 0] - SINSstate.OdoSpeed_s[0] * SINSstate_OdoMod.A_x0s[2, 1];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_kappa_3_ds + 1] = -SINSstate.OdoSpeed_x0[2];
}
KalmanVars.cnt_measures += 1;
}
else if (!SINSstate.add_velocity_to_position)
{
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + value_iMx_dr3] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r_odo_3] = -1.0;
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Height - SINSstate_OdoMod.Height;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = KalmanVars.OdoNoise_Dist;
KalmanVars.cnt_measures += 1;
}
}
// ----------------------------------------------------------//
// ----------------------------------------------------------//
// ----------------------------------------------------------//
if (SINSstate.flag_SeparateHorizVSVertical == true)
{
// --- Если в данный момент выставлен флаг коррекции по Vertical_ZUPT, то здесь не добавляем измерение, добавляем в спец. функции для ZUPT --- //
if (SINSstate.flag_Vertical_ZUPT == false)
{
KalmanVars.Vertical_Matrix_H[(KalmanVars.Vertical_cnt_measures + 0) * SimpleData.iMx_Vertical + 0] = 1.0;
KalmanVars.Vertical_Matrix_H[(KalmanVars.Vertical_cnt_measures + 0) * SimpleData.iMx_Vertical + SINSstate.Vertical_rOdo3] = -1.0;
KalmanVars.Vertical_Measure[(KalmanVars.Vertical_cnt_measures + 0)] = SINSstate.Height - SINSstate_OdoMod.Height;
KalmanVars.Vertical_Noize_Z[(KalmanVars.Vertical_cnt_measures + 0)] = Noize * SINSstate.OdoVerticalNoiseMultiplicator;
//KalmanVars.Vertical_Noize_Z[(KalmanVars.Vertical_cnt_measures + 0)] = SINSstate.OdoAcceleration_s;
if (SINSstate.global_odo_measure_noise_Vertical > 0.001)
{
KalmanVars.Vertical_Noize_Z[(KalmanVars.Vertical_cnt_measures + 0)] = SINSstate.global_odo_measure_noise_Vertical;
}
KalmanVars.Vertical_cnt_measures += 1;
}
}
}
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------ПОЗИЦИОННАЯ КОРЕКЦИЯ-----------------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
public static void Make_H_POSITION(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod, Proc_Help ProcHelp)
{
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;
double[] tempVect = new double[3];
//---Разбиение на три составляющие---
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 0] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + 1] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r12_odo + 0] = -1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + iMx_r12_odo + 1] = -1.0;
//Формирование измерений по географическим координатам
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = (SINSstate.Longitude - SINSstate_OdoMod.Longitude) * SINSstate.R_e * Math.Cos(SINSstate.Latitude);
KalmanVars.Measure[(KalmanVars.cnt_measures + 1)] = (SINSstate.Latitude - SINSstate_OdoMod.Latitude) * SINSstate.R_n;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = SINSstate.A_x0s[0, 1] * KalmanVars.OdoNoise_Dist;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = SINSstate.A_x0s[1, 1] * KalmanVars.OdoNoise_Dist;
KalmanVars.cnt_measures += 2;
//double[] deltaOdoVSsins = new double[3], deltaOdoVSsins_x = new double[3];
//if (SINSstate.Global_file == "Saratov_run_2014_07_23")
//{
// deltaOdoVSsins[0] = 0.617 * Math.Cos(11.25 * SimpleData.ToRadian);
// deltaOdoVSsins[1] = -0.917;
// deltaOdoVSsins[2] = 0.617 * Math.Sin(11.25 * SimpleData.ToRadian);
// SimpleOperations.CopyArray(deltaOdoVSsins_x, SINSstate.A_x0s * deltaOdoVSsins);
// for (int i = 0; i < 3; i++)
// deltaOdoVSsins[i] = deltaOdoVSsins[i];
// KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] += deltaOdoVSsins_x[0];
// KalmanVars.Measure[(KalmanVars.cnt_measures + 1)] += deltaOdoVSsins_x[1];
//}
//---ДОПОЛНИТЕЛЬНОЕ измерение по вертикальной скорости---
if (SINSstate.flag_iMx_r3_dV3)
{
if (SINSstate.add_velocity_to_position)
{
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Vx_0[2] - SINSstate_OdoMod.Vx_0[2];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r3_dV3 + 1] = 1.0;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = SINSstate.A_x0s[2, 1] * KalmanVars.OdoNoise_V;
if (SINSstate.flag_iMx_kappa_13_ds)
{
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_odo_model + 0] = SINSstate.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[2, 2];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_odo_model + 1] = -SINSstate.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[2, 0];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_odo_model + 2] = -SINSstate.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[2, 1];
}
KalmanVars.cnt_measures += 1;
}
else if (!SINSstate.add_velocity_to_position)
{
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r3_dV3] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r12_odo + 2] = -1.0;
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Altitude - SINSstate_OdoMod.Altitude;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = SINSstate.A_x0s[2, 1] * KalmanVars.OdoNoise_Dist;
//KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = KalmanVars.OdoNoise_Dist;
//if (SINSstate.Global_file == "Saratov_run_2014_07_23")
//{
// KalmanVars.Measure[(KalmanVars.cnt_measures + 2)] += deltaOdoVSsins_x[2];
//}
KalmanVars.cnt_measures += 1;
}
}
}
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------Коррекция дрейфа на стоянке-----------------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
public static void Make_H_DRIFTS(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod, Proc_Help ProcHelp)
{
int iMx = SimpleData.iMx, iMz = SimpleData.iMz, iMq = SimpleData.iMq, iMx_kappa_3_ds = SINSstate.value_iMx_kappa_3_ds, iMx_r12_odo = SINSstate.value_iMx_r_odo_12;
int iMx_dV_12 = SINSstate.value_iMx_dV_12,
iMx_alphaBeta = SINSstate.value_iMx_alphaBeta,
iMx_Nu0 = SINSstate.value_iMx_Nu0,
f0_12 = SINSstate.value_iMx_f0_12,
f0_3 = SINSstate.value_iMx_f0_3
;
double[] tempVect = new double[3];
// --- шум измерения. Если объект стоит - уменьшаем
double Noize = 0.00001 * SimpleData.ToRadian;
//---Разбиение на три составляющие---
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_Nu0 + 0] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + iMx_Nu0 + 1] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 2) * iMx + iMx_Nu0 + 2] = 1.0;
SimpleOperations.CopyArray(SINSstate.u_s, SINSstate.A_sx0 * SINSstate.u_x);
double[] W_z_avg = new double[3];
W_z_avg[0] = SINSstate.forDriftMeasureWsAvg[0] / SINSstate.forDriftMeasureWsAvg_cnt;
W_z_avg[1] = SINSstate.forDriftMeasureWsAvg[1] / SINSstate.forDriftMeasureWsAvg_cnt;
W_z_avg[2] = SINSstate.forDriftMeasureWsAvg[2] / SINSstate.forDriftMeasureWsAvg_cnt;
// --- Формирование измерений по разности координат БИНС и одометрического счисления
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = W_z_avg[0] - SINSstate.u_s[0];
KalmanVars.Measure[(KalmanVars.cnt_measures + 1)] = W_z_avg[1] - SINSstate.u_s[1];
KalmanVars.Measure[(KalmanVars.cnt_measures + 2)] = W_z_avg[2] - SINSstate.u_s[2];
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = Noize;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = Noize;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 2)] = Noize;
KalmanVars.cnt_measures += 3;
SINSstate.flag_UsingCorrection = true;
}
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------ПОЗИЦИОННАЯ КОРЕКЦИЯ-----------------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
public static void Make_H_POSITION(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod, Proc_Help ProcHelp)
{
int iMx = SimpleData.iMx, iMz = SimpleData.iMz, iMq = SimpleData.iMq, iMx_kappa_3_ds = SINSstate.value_iMx_kappa_3_ds, iMx_r12_odo = SINSstate.value_iMx_r_odo_12;
int iMx_dV_12 = SINSstate.value_iMx_dV_12,
iMx_alphaBeta = SINSstate.value_iMx_alphaBeta,
iMx_Nu0 = SINSstate.value_iMx_Nu0,
f0_12 = SINSstate.value_iMx_f0_12,
f0_3 = SINSstate.value_iMx_f0_3
;
double[] tempVect = new double[3];
// --- шум измерения. Если объект стоит - уменьшаем
double Noize = 1.0;
double longOdoIncrement = SINSstate.OdometerData.odometer_left.Value - SINSstate.OdometerLeft_ArrayOfPrev[Math.Min(20, SINSstate.OdometerLeft_ArrayOfPrev.Length)];
double longOdoIncrement_dt = SINSstate.Time + SINSstate.Time_Alignment - SINSstate.OdometerLeft_ArrayOfPrevTime[Math.Min(20, SINSstate.OdometerLeft_ArrayOfPrev.Length)];
if (longOdoIncrement / longOdoIncrement_dt == 0.0)
{
Noize = 0.01;
}
//---Разбиение на три составляющие---
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 0] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + 1] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r12_odo + 0] = -1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + iMx_r12_odo + 1] = -1.0;
// --- Формирование измерений по разности координат БИНС и одометрического счисления
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = (SINSstate.Longitude - SINSstate_OdoMod.Longitude) * SINSstate.R_e * Math.Cos(SINSstate.Latitude);
KalmanVars.Measure[(KalmanVars.cnt_measures + 1)] = (SINSstate.Latitude - SINSstate_OdoMod.Latitude) * SINSstate.R_n;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = Noize;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = Noize;
KalmanVars.cnt_measures += 2;
// ----------------------------------------------------------//
// --------Измерение для коррекции вертикального канала-------------//
// ----------------------------------------------------------//
{
KalmanVars.Vertical_Matrix_H[(KalmanVars.Vertical_cnt_measures + 0) * SimpleData.iMx_Vertical + 0] = 1.0;
KalmanVars.Vertical_Matrix_H[(KalmanVars.Vertical_cnt_measures + 0) * SimpleData.iMx_Vertical + SINSstate.Vertical_rOdo3] = -1.0;
KalmanVars.Vertical_Measure[(KalmanVars.Vertical_cnt_measures + 0)] = SINSstate.Height - SINSstate_OdoMod.Height;
// --- шум измерения
KalmanVars.Vertical_Noize_Z[(KalmanVars.Vertical_cnt_measures + 0)] = Noize * SINSstate.OdoVerticalNoiseMultiplicator;
KalmanVars.Vertical_cnt_measures += 1;
}
KalmanVars.counter_odoPosCorrection++;
}
public static void StartSINS_Parameters(SINS_State SINSstate, SINS_State SINSstate_OdoMod, Kalman_Vars KalmanVars, ParamToStart ParamStart, Proc_Help ProcHelp)
{
StreamReader configFile = new StreamReader(SimpleData.ConfigurationFileIn);
string frequency = "", StartLatitude = "", StartLongitude = "", StartHeight = "", AlignmentDuration = "", StartCov_Kappa1 = "", StartCov_Kappa3 = ""
, StartHeightCorrection_value = "0", StartHeightCorrection_flag = "0"
, alpha_kappa_1 = "0"
, alpha_kappa_3 = "0"
, ScaleError = "0"
, Alignment_HeadingDetermined = "0", Alignment_HeadingValue = ""
, Alignment_RollDetermined = "0", Alignment_RollValue = ""
, Alignment_PitchDetermined = "0", Alignment_PitchValue = ""
, SINS_is_accurateMounted_by_kappa_1 = "0"
, SINS_is_accurateMounted_by_kappa_3 = "0"
, SINS_is_accurateMounted_by_scaleError = "0"
, initError_kappa_1 = "0", initError_kappa_3 = "0", initError_ScaleError = "0"
, CalibrationStartMode = "0"
;
string AlignmentEngineIsOff = "1";
string NoiseParamDetermin_mode = "0", NoiseParamDetermin_startTime = "0", NoiseParamDetermin_endTime = "0", NoiseParamDetermin_SigmaValueF = "0", NoiseParamDetermin_SigmaValueNu = "0";
// --- Чтение параметров из Конфигурационного файла --- //
for (int i = 0; ; i++)
{
if (configFile.EndOfStream == true)
{
break;
}
string configLine = configFile.ReadLine();
if (configLine.Substring(0, Math.Min(2, configLine.Length)) != "//" && configLine.IndexOf('=') != -1)
{
string[] tmpstr = configLine.Split('=');
if (tmpstr[0].Trim() == "filename")
{
SINSstate.DataInFileName = tmpstr[1];
}
if (tmpstr[0].Trim() == "frequency")
{
frequency = tmpstr[1];
}
if (tmpstr[0].Trim() == "StartLatitude")
{
StartLatitude = tmpstr[1];
}
if (tmpstr[0].Trim() == "StartLongitude")
{
StartLongitude = tmpstr[1];
}
if (tmpstr[0].Trim() == "StartHeight")
{
StartHeight = tmpstr[1];
}
if (tmpstr[0].Trim() == "AlignmentDuration")
{
AlignmentDuration = tmpstr[1];
}
if (tmpstr[0].Trim() == "AlignmentEngineIsOff")
{
AlignmentEngineIsOff = tmpstr[1];
}
if (tmpstr[0].Trim() == "Alignment_HeadingDetermined")
{
Alignment_HeadingDetermined = tmpstr[1];
}
if (tmpstr[0].Trim() == "Alignment_HeadingValue")
{
Alignment_HeadingValue = tmpstr[1];
}
if (tmpstr[0].Trim() == "NoiseParamDetermin_mode")
{
NoiseParamDetermin_mode = tmpstr[1];
}
if (tmpstr[0].Trim() == "NoiseParamDetermin_startTime")
{
NoiseParamDetermin_startTime = tmpstr[1];
}
if (tmpstr[0].Trim() == "NoiseParamDetermin_endTime")
{
NoiseParamDetermin_endTime = tmpstr[1];
}
if (tmpstr[0].Trim() == "NoiseParamDetermin_SigmaValueF")
{
NoiseParamDetermin_SigmaValueF = tmpstr[1];
}
if (tmpstr[0].Trim() == "NoiseParamDetermin_SigmaValueNu")
{
NoiseParamDetermin_SigmaValueNu = tmpstr[1];
}
if (tmpstr[0].Trim() == "Alignment_RollDetermined")
{
Alignment_RollDetermined = tmpstr[1];
}
if (tmpstr[0].Trim() == "Alignment_RollValue")
{
Alignment_RollValue = tmpstr[1];
}
if (tmpstr[0].Trim() == "Alignment_PitchDetermined")
{
Alignment_PitchDetermined = tmpstr[1];
}
if (tmpstr[0].Trim() == "Alignment_PitchValue")
{
Alignment_PitchValue = tmpstr[1];
}
if (tmpstr[0].Trim() == "SINS_is_accurateMounted_by_kappa_1")
{
SINS_is_accurateMounted_by_kappa_1 = tmpstr[1];
}
if (tmpstr[0].Trim() == "SINS_is_accurateMounted_by_kappa_3")
{
SINS_is_accurateMounted_by_kappa_3 = tmpstr[1];
}
if (tmpstr[0].Trim() == "SINS_is_accurateMounted_by_scaleError")
{
SINS_is_accurateMounted_by_scaleError = tmpstr[1];
}
if (tmpstr[0].Trim() == "StartHeightCorrection_flag")
{
StartHeightCorrection_flag = tmpstr[1];
}
if (tmpstr[0].Trim() == "StartHeightCorrection_value")
{
StartHeightCorrection_value = tmpstr[1];
}
if (tmpstr[0].Trim() == "alpha_kappa_1")
{
alpha_kappa_1 = tmpstr[1];
}
if (tmpstr[0].Trim() == "alpha_kappa_3")
{
alpha_kappa_3 = tmpstr[1];
}
if (tmpstr[0].Trim() == "ScaleError")
{
ScaleError = tmpstr[1];
}
if (tmpstr[0].Trim() == "initError_kappa_1")
{
initError_kappa_1 = tmpstr[1];
}
if (tmpstr[0].Trim() == "initError_kappa_3")
{
initError_kappa_3 = tmpstr[1];
}
if (tmpstr[0].Trim() == "initError_ScaleError")
{
initError_ScaleError = tmpstr[1];
}
//if (tmpstr[0].Trim() == "CalibrationStartMode")
// CalibrationStartMode = tmpstr[1];
}
}
SINSstate.timeStep = SINSstate.Freq = Convert.ToDouble(frequency);
// --- Лишь каждое OdoLimitMeasuresNum обновление показаний одометра будут использоваться для коррекции --- //
SINSstate.OdoLimitMeasuresNum = 5;
// --- Количество тактов БИНС для начальной выставки от начала --- //
ProcHelp.AlignmentCounts = Convert.ToInt32(Math.Round(Convert.ToDouble(AlignmentDuration) / SINSstate.Freq));
SINSstate.AlignmentEngineIsOff = Convert.ToInt32(AlignmentEngineIsOff);
SINSstate.NoiseParamDetermin_mode = Convert.ToInt32(NoiseParamDetermin_mode);
SINSstate.NoiseParamDetermin_startTime = Convert.ToInt32(Math.Round(Convert.ToDouble(NoiseParamDetermin_startTime) / SINSstate.Freq));
SINSstate.NoiseParamDetermin_endTime = Math.Min(Convert.ToInt32(Math.Round(Convert.ToDouble(NoiseParamDetermin_endTime) / SINSstate.Freq)), ProcHelp.AlignmentCounts);
SINSstate.NoiseParamDetermin_SigmaValueF = Convert.ToDouble(NoiseParamDetermin_SigmaValueF);
SINSstate.NoiseParamDetermin_SigmaValueNu = Convert.ToDouble(NoiseParamDetermin_SigmaValueNu);
// --- Заданные значения начальных углов ориентации: флаги и значения --- //
if (Convert.ToInt32(Alignment_HeadingDetermined) == 1)
{
SINSstate.Alignment_HeadingDetermined = true;
}
else
{
SINSstate.Alignment_HeadingDetermined = false;
}
SINSstate.Alignment_HeadingValue = Convert.ToDouble(Alignment_HeadingValue) * SimpleData.ToRadian;
if (Convert.ToInt32(Alignment_RollDetermined) == 1)
{
SINSstate.Alignment_RollDetermined = true;
}
else
{
SINSstate.Alignment_RollDetermined = false;
}
SINSstate.Alignment_RollValue = Convert.ToDouble(Alignment_RollValue) * SimpleData.ToRadian;
if (Convert.ToInt32(Alignment_PitchDetermined) == 1)
{
SINSstate.Alignment_PitchDetermined = true;
}
else
{
SINSstate.Alignment_PitchDetermined = false;
}
SINSstate.Alignment_PitchValue = Convert.ToDouble(Alignment_PitchValue) * SimpleData.ToRadian;
if (Convert.ToInt32(StartHeightCorrection_flag) == 1)
{
SINSstate.first_N_meters_StartHeightCorrection_flag = true;
}
else
{
SINSstate.first_N_meters_StartHeightCorrection_flag = false;
}
SINSstate.first_N_meters_StartHeightCorrection_value = Convert.ToDouble(StartHeightCorrection_value);
//if (Convert.ToInt32(CalibrationStartMode) == 1)
// SINSstate.CalibrationStartMode = true;
// --- Углы рассогласования осей БИНС и динамических осей объекта, если это нужно --- //
SINSstate.alpha_kappa_3 = Convert.ToDouble(alpha_kappa_3) * SimpleData.ToRadian; // -- Угол рассогласования по курсу
SINSstate.alpha_kappa_1 = Convert.ToDouble(alpha_kappa_1) * SimpleData.ToRadian; // -- Угол рассогласования по тангажу
SINSstate.alpha_scaleError = Convert.ToDouble(ScaleError) / 100.0;
SINSstate.initError_kappa_3 = Convert.ToDouble(initError_kappa_3) * SimpleData.ToRadian; // -- Специальная введенная ошибка по курсу
SINSstate.initError_kappa_1 = Convert.ToDouble(initError_kappa_1) * SimpleData.ToRadian; // -- Специальная введенная ошибка по тангажу
SINSstate.initError_scaleError = Convert.ToDouble(initError_ScaleError) / 100.0;
// --- Шум по горизонтальным ошибкам координат --- //
KalmanVars.Noise_Pos = 1.0;
// --- Шум по вертикальным ошибкам координат --- //
KalmanVars.Noise_Pos_Vertical = 0.01;
// -------------------------------------------//
// --- определяем мультипликатор для шума вертикального измерения по одометру
SINSstate.OdoVerticalNoiseMultiplicator = 2;
// --- Начальные ковариации --- //
SINSstate.stdR = 0.01; // по ошибке координат БИНС, м
SINSstate.stdOdoR = 0.01; // по ошибке координат одометрического счисления, м
SINSstate.stdV = 0.01;
// --- По флагу Точно ли установлен БИНС на корпусе объекта, определяем соответствующие начальные значения ковариаций
if (Convert.ToInt32(SINS_is_accurateMounted_by_kappa_1) == 1)
{
SINSstate.stdKappa1 = 1.0;
}
else
{
SINSstate.stdKappa1 = 10.0;
}
if (Convert.ToInt32(SINS_is_accurateMounted_by_kappa_3) == 1)
{
SINSstate.stdKappa3 = 1.0;
}
else
{
SINSstate.stdKappa3 = 10.0;
}
SINSstate.stdScale = 0.01;
SINSstate.Noise_Marker_PositionError = 0.1; // в метрах
SINSstate.Noise_GPS_PositionError = 2.0; // в метрах
KalmanVars.OdoNoise_STOP = 0.1;
// --- Начальные координаты --- //
ProcHelp.LongSNS = SINSstate_OdoMod.Longitude = SINSstate.Longitude_Start = SINSstate.LongSNS = SINSstate.Longitude = Convert.ToDouble(StartLongitude) * SimpleData.ToRadian;
ProcHelp.LatSNS = SINSstate_OdoMod.Latitude = SINSstate.Latitude_Start = SINSstate.LatSNS = SINSstate.Latitude = Convert.ToDouble(StartLatitude) * SimpleData.ToRadian;
ProcHelp.AltSNS = SINSstate_OdoMod.Height = SINSstate.Altitude_Start = SINSstate.AltSNS = SINSstate.Height = SINSstate.Altitude_prev = Convert.ToDouble(StartHeight);
////--- В случае выставления значения поправки на угол kappa_3 именшаем нач.ковариацию ---//
//if (Math.Abs(SINSstate.alpha_kappa_3) > 0.00001 * SimpleData.ToRadian)
// SINSstate.stdKappa3 = 1.0; //минут
////--- В случае выставления значения поправки на угол kappa_1 именшаем нач.ковариацию ---//
//if (Math.Abs(SINSstate.alpha_kappa_1) > 0.00001 * SimpleData.ToRadian)
// SINSstate.stdKappa1 = 1.0; //минут
//--- В случае выставления значения поправки на погрешность масштаба именшаем нач.ковариацию ---//
if (Math.Abs(SINSstate.alpha_scaleError) > 0.00001)
{
SINSstate.stdScale = 0.001;
}
ProcHelp.LongSNS = ProcHelp.LongSNS * 180 / Math.PI;
ProcHelp.LatSNS = ProcHelp.LatSNS * 180 / Math.PI;
ApplyMatrixStartCondition(SINSstate);
ApplyMatrixStartCondition(SINSstate_OdoMod);
//--- Если запуск производится в режиме калибровочки, то меняем начальные ковариации
if (SINSstate.CalibrationStartMode)
{
//KalmanVars.Noise_Pos = 0.1;
if (SINSstate.alpha_kappa_1 == 0)
{
SINSstate.stdKappa1 = 60.0;
}
if (SINSstate.alpha_kappa_3 == 0)
{
SINSstate.stdKappa3 = 60.0;
}
if (SINSstate.alpha_scaleError == 0)
{
SINSstate.stdScale = 0.02;
}
}
}
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);
}
}
public static void ReadSINSStateFromString(Proc_Help ProcHelp, StreamReader myFile, SINS_State SINSstate, SINS_State SINSstate_OdoMod)
{
int t = 0;
string[] dataArray;
ProcHelp.datastring = myFile.ReadLine();
if (ProcHelp.datastring.Contains("Count") || ProcHelp.datastring.Contains("Latitude"))
{
ProcHelp.datastring = myFile.ReadLine();
}
if (myFile.EndOfStream == false)
{
dataArray = ProcHelp.datastring.Split(' ');
for (int y = 0; y < dataArray.Length; y++)
{
if (dataArray[y] != "")
{
t++;
}
}
string[] dataArray2 = new string[t];
t = 0;
for (int y = 0; y < dataArray.Length; y++)
{
if (dataArray[y] != "")
{
dataArray2[t] = dataArray[y];
t++;
}
}
SINSstate.i_global++;
SINSstate.Count = Convert.ToDouble(dataArray2[0]);
if (ProcHelp.initCount == false)
{
ProcHelp.initCount = true; SINSstate.initCount = SINSstate.Count - 1;
}
SINSstate.Time = (SINSstate.Count - SINSstate.initCount) * Math.Abs(SINSstate.timeStep);
SINSstate.Input_nMode = Convert.ToInt32(dataArray2[22]);
SINSstate.F_z[1] = Convert.ToDouble(dataArray2[1]);
SINSstate.F_z[2] = Convert.ToDouble(dataArray2[2]);
SINSstate.F_z[0] = Convert.ToDouble(dataArray2[3]);
SINSstate.W_z[1] = Convert.ToDouble(dataArray2[4]);
SINSstate.W_z[2] = Convert.ToDouble(dataArray2[5]);
SINSstate.W_z[0] = Convert.ToDouble(dataArray2[6]);
// --- Поворот показаний датчиков на заданные углы несоосности БИНС и динамических осей объекта --- //
{
double[] fz = new double[3], Wz = new double[3];
fz[0] = SINSstate.F_z[0] - SINSstate.alpha_kappa_3 * SINSstate.F_z[1];
fz[1] = SINSstate.F_z[1] + SINSstate.alpha_kappa_3 * SINSstate.F_z[0] - SINSstate.alpha_kappa_1 * SINSstate.F_z[2];
fz[2] = SINSstate.F_z[2] + SINSstate.alpha_kappa_1 * SINSstate.F_z[1];
Wz[0] = SINSstate.W_z[0] - SINSstate.alpha_kappa_3 * SINSstate.W_z[1];
Wz[1] = SINSstate.W_z[1] + SINSstate.alpha_kappa_3 * SINSstate.W_z[0] - SINSstate.alpha_kappa_1 * SINSstate.W_z[2];
Wz[2] = SINSstate.W_z[2] + SINSstate.alpha_kappa_1 * SINSstate.W_z[1];
SimpleOperations.CopyArray(SINSstate.F_z, fz);
SimpleOperations.CopyArray(SINSstate.W_z, Wz);
}
/*Поворачиваем на моделируюемую ошибку*/
if (SINSstate.initError_kappa_1 != 0 || SINSstate.initError_kappa_3 != 0)
{
double[] fz = new double[3], Wz = new double[3];
fz[0] = SINSstate.F_z[0] + SINSstate.initError_kappa_3 * SINSstate.F_z[1];
fz[1] = SINSstate.F_z[1] - SINSstate.initError_kappa_3 * SINSstate.F_z[0] + SINSstate.initError_kappa_1 * SINSstate.F_z[2];
fz[2] = SINSstate.F_z[2] - SINSstate.initError_kappa_1 * SINSstate.F_z[1];
Wz[0] = SINSstate.W_z[0] + SINSstate.initError_kappa_3 * SINSstate.W_z[1];
Wz[1] = SINSstate.W_z[1] - SINSstate.initError_kappa_3 * SINSstate.W_z[0] + SINSstate.initError_kappa_1 * SINSstate.W_z[2];
Wz[2] = SINSstate.W_z[2] - SINSstate.initError_kappa_1 * SINSstate.W_z[1];
SimpleOperations.CopyArray(SINSstate.F_z, fz);
SimpleOperations.CopyArray(SINSstate.W_z, Wz);
}
//---Запоминаем координаты предыдущей контрольной точки
if (SINSstate.GPS_Data.gps_Latitude.isReady == 1)
{
SINSstate.GPS_Data.gps_Latitude_prev.Value = SINSstate.GPS_Data.gps_Latitude.Value;
SINSstate.GPS_Data.gps_Longitude_prev.Value = SINSstate.GPS_Data.gps_Longitude.Value;
SINSstate.GPS_Data.gps_Altitude_prev.Value = SINSstate.GPS_Data.gps_Altitude.Value;
SINSstate.OdometerData.odometer_left_prev.Value = SINSstate.OdometerData.odometer_left.Value;
SINSstate.OdometerData.odometer_right_prev.Value = SINSstate.OdometerData.odometer_right.Value;
}
// --- Заполняем структуру показний GPS, если они имеются
// --- Предполагается, что GPS записываются в WGS84
SINSstate.GPS_Data.gps_Latitude.Value = Convert.ToDouble(dataArray2[7]);
SINSstate.GPS_Data.gps_Latitude.isReady = Convert.ToInt32(dataArray2[8]);
SINSstate.GPS_Data.gps_Longitude.Value = Convert.ToDouble(dataArray2[9]);
SINSstate.GPS_Data.gps_Longitude.isReady = Convert.ToInt32(dataArray2[10]);
SINSstate.GPS_Data.gps_Altitude.Value = Convert.ToDouble(dataArray2[11]);
SINSstate.GPS_Data.gps_Altitude.isReady = Convert.ToInt32(dataArray2[12]);
SINSstate.GPS_Data.gps_Vn.Value = Convert.ToDouble(dataArray2[13]);
SINSstate.GPS_Data.gps_Vn.isReady = Convert.ToInt32(dataArray2[14]);
SINSstate.GPS_Data.gps_Ve.Value = Convert.ToDouble(dataArray2[15]);
SINSstate.GPS_Data.gps_Ve.isReady = Convert.ToInt32(dataArray2[16]);
if (SINSstate.GPS_Data.gps_Latitude.isReady == 2)
{
SINSstate.GPS_Data.gps_Latitude.isReady = 1;
}
if (SINSstate.GPS_Data.gps_Longitude.isReady == 2)
{
SINSstate.GPS_Data.gps_Longitude.isReady = 1;
}
if (SINSstate.GPS_Data.gps_Altitude.isReady == 2)
{
SINSstate.GPS_Data.gps_Altitude.isReady = 1;
}
if (SINSstate.GPS_Data.gps_Vn.isReady == 2)
{
SINSstate.GPS_Data.gps_Vn.isReady = 1;
}
if (SINSstate.GPS_Data.gps_Ve.isReady == 2)
{
SINSstate.GPS_Data.gps_Ve.isReady = 1;
}
//--- Если режим не Движение, то игнорируем показания СНС ---
if (SINSstate.Input_nMode != 16)
{
SINSstate.GPS_Data.gps_Latitude.isReady = 0;
SINSstate.GPS_Data.gps_Longitude.isReady = 0;
SINSstate.GPS_Data.gps_Altitude.isReady = 0;
SINSstate.GPS_Data.gps_Vn.isReady = 0;
SINSstate.GPS_Data.gps_Ve.isReady = 0;
}
if (SINSstate.GPS_Data.gps_Latitude.isReady == 1)
{
SINSstate.GPS_CounterOfPoints++;
}
if (SINSstate.GPS_Data.gps_Latitude.isReady == 1)
{
SINSstate.GPS_Data.gps_Vn.Value_prev = SINSstate.GPS_Data.gps_Vn.Value;
SINSstate.GPS_Data.gps_Ve.Value_prev = SINSstate.GPS_Data.gps_Ve.Value;
}
SINSstate.FLG_Stop = Convert.ToInt32(dataArray2[17]);
// --- Обрабатываем ситуацию, когда одометр установлен неправильно и не введен масштабный коэффициент
double Odo_SINS_VS_distance_coefficient = SINSstate.OdometerData.odometer_left.Value / SINSstate.distance_by_SINS;
if (SINSstate.distance_by_SINS < 110.0 && Math.Abs(Odo_SINS_VS_distance_coefficient) > 0.3)
{
if (SINSstate.distance_by_SINS > 5.0 && Odo_SINS_VS_distance_coefficient < 0)
{
SINSstate.OdometerData_Sign = -1;
SINSstate.OdometerLeftPrev *= SINSstate.OdometerData_Sign;
}
if (SINSstate.distance_by_SINS > 100.0 && SINSstate.OdometerData_RoughtScale_flag == 0 && Math.Abs(1 - Odo_SINS_VS_distance_coefficient) > 0.5)
{
SINSstate.OdometerData_RoughtScale = Odo_SINS_VS_distance_coefficient;
SINSstate.OdometerLeftPrev /= SINSstate.OdometerData_RoughtScale;
SINSstate.OdometerData_RoughtScale_flag = 1;
}
}
// --- Считываем показания одометров
SINSstate.OdometerData.odometer_left.Value = (Convert.ToDouble(dataArray2[18]) - SINSstate.OdometerData.odometer_left.Value_Correction) * SINSstate.OdometerData_Sign / SINSstate.OdometerData_RoughtScale;
SINSstate.OdometerData.odometer_left.isReady = Convert.ToInt32(dataArray2[19]);
SINSstate.OdometerData.odometer_right.Value = (Convert.ToDouble(dataArray2[20]) - SINSstate.OdometerData.odometer_left.Value_Correction) * SINSstate.OdometerData_Sign / SINSstate.OdometerData_RoughtScale;
SINSstate.OdometerData.odometer_right.isReady = Convert.ToInt32(dataArray2[21]);
// --- ЗАГЛУШКА для файла со стоянкой, поскольку здесь всегда для одометра isReady = 0 ---
if (SINSstate.DataInFileName.Contains("GCEF_format_Azimuth10B_450612_48H_17-Feb-2016") == true && SINSstate.Count % 5 == 0)
{
SINSstate.OdometerData.odometer_left.isReady = 1;
}
// --- ЗАГЛУШКА ---
if (SINSstate.flag_notUseOdometer == true)
{
SINSstate.OdometerData.odometer_left.isReady = 2;
}
if (SINSstate.OdometerData.odometer_left.isReady == 1)
{
// Проверка на случай, когда одометр начинает имзерять растояние не с нуля
if (SINSstate.firstNotNullOdoValue_flg == 0 && SINSstate.OdometerData.odometer_left.isReady == 1)
{
SINSstate.firstNotNullOdoValue_flg = 1;
if (SINSstate.OdometerData.odometer_left.Value > 1.0)
{
SINSstate.OdometerData.odometer_left.Value_Correction = SINSstate.OdometerData.odometer_left.Value;
SINSstate.OdometerData.odometer_left.Value -= SINSstate.OdometerData.odometer_left.Value_Correction;
}
}
SINSstate.OdometerData.odometer_left.Value_prev = SINSstate.OdometerData.odometer_left.Value;
SINSstate.OdoLimitMeasuresNum_Count++;
if (SINSstate.OdoLimitMeasuresNum_Count < SINSstate.OdoLimitMeasuresNum)
{
SINSstate.OdometerData.odometer_left.isReady = 2;
SINSstate.OdometerData.odometer_right.isReady = 2;
}
else
{
SINSstate.OdoLimitMeasuresNum_Count = 0;
}
}
// --- Делаем поправку на введенное значение ошибки масштаба одометра
SINSstate.OdometerData.odometer_left.Value /= (1.0 + SINSstate.alpha_scaleError - SINSstate.initError_scaleError);
SINSstate.OdometerData.odometer_right.Value /= (1.0 + SINSstate.alpha_scaleError - SINSstate.initError_scaleError);
// --- Фильтруем всплески в показаниях одометра
double CurOdoSpeed_s1 = (SINSstate.OdometerData.odometer_left.Value - SINSstate.OdometerLeftPrev) / (SINSstate.OdoTimeStepCount + 1) / SINSstate.timeStep;
if (SINSstate.OdometerData.odometer_left.isReady == 1 &&
SINSstate.OdoSpeed_s[1] > 1.0 &&
(CurOdoSpeed_s1 - SINSstate.OdoSpeed_s[1]) > 10.0 &&
CurOdoSpeed_s1 > 1.5 * SINSstate.OdoSpeed_s[1])
{
double timeMark = SINSstate.Time + SINSstate.Time_Alignment;
SINSstate.OdometerLeftPrev = SINSstate.OdometerData.odometer_left.Value - SINSstate.OdometerVector[1];
SINSstate.OdometerData.odometer_left.isReady = 2;
SINSstate.OdometerData.odometer_right.isReady = 2;
}
// --- Сохраняем оригинальные считанные значение, если проставлен флаг вывода в GRTV ---//
if (SINSstate.flag_GRTV_output)
{
for (int y = 0; y < 3; y++)
{
SINSstate.F_z_orig[y] = SINSstate.F_z[y] / 9.81;
SINSstate.W_z_orig[y] = SINSstate.W_z[y];
}
SINSstate.OdometerData.odometer_left.Value_orig = Convert.ToDouble(dataArray2[18]);
SINSstate.OdometerData.odometer_left.isReady_orig = Convert.ToInt32(dataArray2[19]);
if (SINSstate.OdometerData.odometer_left.isReady_orig != 1)
{
SINSstate.OdometerData.odometer_left.isReady_orig = 0;
SINSstate.OdometerData.odometer_left.Value_orig = SINSstate.OdometerData.odometer_left.Value_prev;
}
SINSstate.GPS_Data.gps_Vn.Value_orig = Convert.ToDouble(dataArray2[13]);
SINSstate.GPS_Data.gps_Vn.isReady_orig = Convert.ToInt32(dataArray2[14]);
if (SINSstate.GPS_Data.gps_Vn.isReady_orig != 1)
{
SINSstate.GPS_Data.gps_Vn.isReady_orig = 0;
SINSstate.GPS_Data.gps_Vn.Value_orig = SINSstate.GPS_Data.gps_Vn.Value_prev;
}
SINSstate.GPS_Data.gps_Ve.Value_orig = Convert.ToDouble(dataArray2[15]);
SINSstate.GPS_Data.gps_Ve.isReady_orig = Convert.ToInt32(dataArray2[16]);
if (SINSstate.GPS_Data.gps_Ve.isReady_orig != 1)
{
SINSstate.GPS_Data.gps_Ve.isReady_orig = 0;
SINSstate.GPS_Data.gps_Ve.Value_orig = SINSstate.GPS_Data.gps_Ve.Value_prev;
}
SINSstate.GPS_Data.gps_Latitude.Value_orig = Convert.ToDouble(dataArray2[7]);
SINSstate.GPS_Data.gps_Longitude.Value_orig = Convert.ToDouble(dataArray2[9]);
SINSstate.GPS_Data.gps_Altitude.Value_orig = Convert.ToDouble(dataArray2[11]);
if (SINSstate.GPS_Data.gps_Latitude.isReady != 1)
{
SINSstate.GPS_Data.gps_Latitude.Value_orig = SINSstate.GPS_Data.gps_Latitude_prev.Value;
SINSstate.GPS_Data.gps_Longitude.Value_orig = SINSstate.GPS_Data.gps_Longitude_prev.Value;
SINSstate.GPS_Data.gps_Altitude.Value_orig = SINSstate.GPS_Data.gps_Altitude_prev.Value;
}
SINSstate.GPS_Data.gps_Latitude.isReady_orig = Convert.ToInt32(dataArray2[8]);
SINSstate.GPS_Data.gps_Longitude.isReady_orig = Convert.ToInt32(dataArray2[10]);
SINSstate.GPS_Data.gps_Altitude.isReady_orig = Convert.ToInt32(dataArray2[12]);
if (SINSstate.GPS_Data.gps_Latitude.isReady_orig != 1)
{
SINSstate.GPS_Data.gps_Latitude.isReady_orig = 0;
}
if (SINSstate.GPS_Data.gps_Longitude.isReady_orig != 1)
{
SINSstate.GPS_Data.gps_Longitude.isReady_orig = 0;
}
if (SINSstate.GPS_Data.gps_Altitude.isReady_orig != 1)
{
SINSstate.GPS_Data.gps_Altitude.isReady_orig = 0;
}
}
SINSstate.GPS_Data.gps_Latitude_pnppk_sol.Value = Convert.ToDouble(dataArray2[25]);
SINSstate.GPS_Data.gps_Longitude_pnppk_sol.Value = Convert.ToDouble(dataArray2[26]);
SINSstate.GPS_Data.gps_Altitude_pnppk_sol.Value = Convert.ToDouble(dataArray2[27]);
// --- Проставляем флаги, что перваястрока из файла с данными считана
if (SINSstate.firstLineRead == false)
{
SINSstate.Roll_prev = SINSstate.Roll;
SimpleOperations.CopyArray(SINSstate.F_z_prev, SINSstate.F_z);
SimpleOperations.CopyArray(SINSstate.W_z_prev, SINSstate.W_z);
SINSstate.OdometerStartValue = SINSstate.OdometerData.odometer_left.Value;
SINSstate.OdometerLeftPrev = SINSstate.OdometerData.odometer_left.Value;
SINSstate.OdometerRightPrev = SINSstate.OdometerData.odometer_right.Value;
SINSstate.firstLineRead = true;
}
}
}
