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 int SINS_Alignment_Classical(Proc_Help ProcHelp, SINS_State SINSstate, SINS_State SINSstate2, SINS_State SINSstate_OdoMod, StreamReader myFile, Kalman_Vars KalmanVars, StreamWriter GRTV_output)
{
int i = 0, t = 0;
int iMx = 9;
int iMq = 3;
int iMz = 7;
StreamWriter Alignment_Errors = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_Errors.txt");
StreamWriter Alignment_SINSstate = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_SINSstate.txt");
StreamWriter Alignment_Corrected_State = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_Corrected_State.txt");
StreamWriter Alignment_StateErrorsVector = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_StateErrorsVector.txt");
StreamWriter Alignment_STD_Data = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_STD_Data.txt");
Alignment_Errors.WriteLine("DeltaHeading DeltaRoll DeltaPitch");
Alignment_StateErrorsVector.WriteLine("Time Beta1 Beta2 Beta3 dF1 dF2 dF3 Nu1 Nu2 Nu3 ");
Alignment_SINSstate.WriteLine("Time Count Lat Long Altitude V1 V2 Heading HeadingCor Roll RollCor Pitch PitchCor");
bool exist_real_Classical_Alignment = false;
int AlignmentCounts_tmp = ProcHelp.AlignmentCounts;
//---Этап грубой выставки---
//int temp_AlgnCnt = ProcHelp.AlgnCnt;
//ProcHelp.AlgnCnt = Convert.ToInt32(120.0 / SINSstate.Freq);
if (exist_real_Classical_Alignment == true)
{
ProcHelp.AlignmentCounts = Convert.ToInt32(Math.Round(ProcHelp.AlignmentCounts / 3.0));
SINSstate.Alignment_HeadingDetermined = false;
}
i = Alignment.RougthAlignment(ProcHelp, SINSstate, myFile, KalmanVars, SINSstate_OdoMod, GRTV_output);
if (exist_real_Classical_Alignment == true)
{
ProcHelp.AlignmentCounts = AlignmentCounts_tmp;
}
//ProcHelp.AlgnCnt = temp_AlgnCnt;
SINSstate.flag_Alignment = true;
if (exist_real_Classical_Alignment)
{
Kalman_Align KalmanAlign = new Kalman_Align();
SimpleOperations.CopyArray(KalmanAlign.Noise_Vel, KalmanVars.Noise_Vel);
SimpleOperations.CopyArray(KalmanAlign.Noise_Angl, KalmanVars.Noise_Angl);
Alignment_Classical.InitOfCovarianceMatrixes(KalmanAlign);
for (int j = i; j < ProcHelp.AlignmentCounts; j++)
{
ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, null, SINSstate, SINSstate_OdoMod, true);
Alignment_Classical.AlignStateIntegration_AT(SINSstate, KalmanVars, SINSstate2, SINSstate_OdoMod);
Alignment_Classical.Make_A(SINSstate, KalmanAlign);
Alignment_Classical.MatrixNoise_ReDef(SINSstate, KalmanAlign);
KalmanProcs.Make_F_Align(SINSstate.timeStep, KalmanAlign);
Alignment_Classical.Make_H_and_Correction(SINSstate, KalmanAlign);
KalmanProcs.KalmanForecast_Align(KalmanAlign);
i = j;
if (j % 200 == 0)
{
Console.WriteLine(SINSstate.Count.ToString() + ", " + (SINSstate.Latitude * SimpleData.ToDegree - ProcHelp.LatSNS).ToString() + ", " + SINSstate.F_x[2].ToString().ToString());
}
SINSstate.DeltaRoll = -(KalmanAlign.ErrorConditionVector_p[0] * Math.Sin(SINSstate.Heading) + KalmanAlign.ErrorConditionVector_p[1] * Math.Cos(SINSstate.Heading)) / Math.Cos(SINSstate.Pitch);
SINSstate.DeltaPitch = -KalmanAlign.ErrorConditionVector_p[0] * Math.Cos(SINSstate.Heading) + KalmanAlign.ErrorConditionVector_p[1] * Math.Sin(SINSstate.Heading);
SINSstate.DeltaHeading = KalmanAlign.ErrorConditionVector_p[2] + SINSstate.DeltaRoll * Math.Sin(SINSstate.Pitch);
Alignment_Errors.WriteLine(SINSstate.DeltaHeading * 180.0 / 3.141592 + " " + SINSstate.DeltaRoll * 180.0 / 3.141592 + " " + SINSstate.DeltaPitch * 180.0 / 3.141592);
Alignment_STD_Data.WriteLine(KalmanProcs.Sigmf_Disp(0, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(1, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(2, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(3, KalmanAlign) + " " +
KalmanProcs.Sigmf_Disp(4, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(5, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(6, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(7, KalmanAlign) + " " + KalmanProcs.Sigmf_Disp(8, KalmanAlign));
Alignment.OutPutInfo_Class_Alignment(ProcHelp, SINSstate, SINSstate2, myFile, KalmanAlign, Alignment_Errors, Alignment_SINSstate, Alignment_Corrected_State, Alignment_StateErrorsVector);
}
SimpleOperations.PrintMatrixToFile(KalmanAlign.CovarianceMatrixS_p, SimpleData.iMx_Align, SimpleData.iMx_Align);
SINSstate.Heading = SINSstate.Heading - SINSstate.DeltaHeading;
SINSstate.Roll = SINSstate.Roll - SINSstate.DeltaRoll;
SINSstate.Pitch = SINSstate.Pitch - SINSstate.DeltaPitch;
//KalmanVars.CovarianceMatrixS_m[4 * SimpleData.iMx + 4] = KalmanVars.CovarianceMatrixS_p[4 * SimpleData.iMx + 4] = KalmanProcs.Sigmf_Disp(0, KalmanAlign);
//KalmanVars.CovarianceMatrixS_m[5 * SimpleData.iMx + 5] = KalmanVars.CovarianceMatrixS_p[5 * SimpleData.iMx + 5] = KalmanProcs.Sigmf_Disp(1, KalmanAlign);
//KalmanVars.CovarianceMatrixS_m[6 * SimpleData.iMx + 6] = KalmanVars.CovarianceMatrixS_p[6 * SimpleData.iMx + 6] = KalmanProcs.Sigmf_Disp(2, KalmanAlign);
//KalmanVars.CovarianceMatrixS_m[10 * SimpleData.iMx + 10] = KalmanVars.CovarianceMatrixS_p[10 * SimpleData.iMx + 10] = KalmanProcs.Sigmf_Disp(3, KalmanAlign);
//KalmanVars.CovarianceMatrixS_m[11 * SimpleData.iMx + 11] = KalmanVars.CovarianceMatrixS_p[11 * SimpleData.iMx + 11] = KalmanProcs.Sigmf_Disp(4, KalmanAlign);
//KalmanVars.CovarianceMatrixS_m[12 * SimpleData.iMx + 12] = KalmanVars.CovarianceMatrixS_p[12 * SimpleData.iMx + 12] = KalmanProcs.Sigmf_Disp(5, KalmanAlign);
//KalmanVars.CovarianceMatrixS_m[7 * SimpleData.iMx + 7] = KalmanVars.CovarianceMatrixS_p[7 * SimpleData.iMx + 7] = KalmanProcs.Sigmf_Disp(6, KalmanAlign);
//KalmanVars.CovarianceMatrixS_m[8 * SimpleData.iMx + 8] = KalmanVars.CovarianceMatrixS_p[8 * SimpleData.iMx + 8] = KalmanProcs.Sigmf_Disp(7, KalmanAlign);
//KalmanVars.CovarianceMatrixS_m[9 * SimpleData.iMx + 9] = KalmanVars.CovarianceMatrixS_p[9 * SimpleData.iMx + 9] = KalmanProcs.Sigmf_Disp(8, KalmanAlign);
SINSstate.Time_Alignment = SINSstate.Time;
SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate);
SINSstate.A_x0s = SINSstate.A_sx0.Transpose();
SINSstate.A_x0n = SimpleOperations.A_x0n(SINSstate.Latitude, SINSstate.Longitude);
SINSstate.A_nx0 = SINSstate.A_x0n.Transpose();
SINSstate.A_nxi = SimpleOperations.A_ne(SINSstate.Time - SINSstate.Time_Alignment, SINSstate.Longitude_Start);
SINSstate.AT = Matrix.Multiply(SINSstate.A_sx0, SINSstate.A_x0n);
SINSstate.AT = Matrix.Multiply(SINSstate.AT, SINSstate.A_nxi);
/*----------------------------------------------------------------------------------------*/
}
Alignment_Errors.Close(); Alignment_Corrected_State.Close(); Alignment_SINSstate.Close(); Alignment_StateErrorsVector.Close();
Alignment_STD_Data.Close();
ProcHelp.initCount = false;
SINSstate.flag_Alignment = false;
return(i);
}
public static int SINS_Alignment_Navigation(Proc_Help ProcHelp, SINS_State SINSstate, SINS_State SINSstate2, SINS_State SINSstate_OdoMod, StreamReader myFile, Kalman_Vars KalmanVars, StreamWriter GRTV_output)
{
int i = 0, t = 0;
SimpleData.iMx = 13;
SimpleData.iMq = 5;
SimpleData.iMz = 7;
StreamWriter Alignment_Errors = new StreamWriter(SimpleData.PathOutputString + "Alignment_Errors.txt");
StreamWriter Alignment_SINSstate = new StreamWriter(SimpleData.PathOutputString + "Alignment_SINSstate.txt");
StreamWriter Alignment_Corrected_State = new StreamWriter(SimpleData.PathOutputString + "Alignment_Corrected_State.txt");
StreamWriter Alignment_StateErrorsVector = new StreamWriter(SimpleData.PathOutputString + "Alignment_StateErrorsVector.txt");
Alignment_Errors.WriteLine("dR1 dR2 dV1 dV2 Alpha1 Alpha2 Beta3 Nu1 Nu2 Nu3 dF1 dF2 dF3");
Alignment_Corrected_State.WriteLine("Time Count LatCrtd Lat LongCrtd Long AltitudeCrtd V1 V2 V3 Heading HeadingCor Roll RollCor Pitch PitchCor");
//---Этап грубой выставки---
//int temp_AlgnCnt = ProcHelp.AlgnCnt;
//ProcHelp.AlgnCnt = Convert.ToInt32(200.0 / SINSstate.Freq);
i = Alignment.RougthAlignment(ProcHelp, SINSstate, myFile, KalmanVars, SINSstate_OdoMod, GRTV_output);
//ProcHelp.AlgnCnt = temp_AlgnCnt;
SINSstate.flag_Alignment = true;
if (false)
{
Alignment_Navigation.InitOfCovarianceMatrixes(KalmanVars); //---Инициализация ковариационных матриц матриц вектора ошибок---//
/*----------------------------------------------------------------------------------------*/
while (true)
{
if (SINSstate.FLG_Stop == 0 || (ProcHelp.AlignmentCounts != 0 && i == ProcHelp.AlignmentCounts))
{
break;
}
ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, null, SINSstate, SINSstate_OdoMod, true);
if (t == 0)
{
SimpleOperations.CopyArray(SINSstate.F_z_prev, SINSstate.F_z); SimpleOperations.CopyArray(SINSstate.W_z_prev, SINSstate.W_z); t = 1;
}
SINSprocessing.StateIntegration_AT(SINSstate, KalmanVars, SINSstate2, SINSstate2);
Alignment_Navigation.MatrixNoise_ReDef(SINSstate, KalmanVars); //изменить все эти функции
Alignment_Navigation.Make_A_easy(SINSstate2, KalmanVars);
KalmanProcs.Make_F(SINSstate.timeStep, KalmanVars, SINSstate);
KalmanProcs.KalmanForecast(KalmanVars, SINSstate);
Alignment_Navigation.Make_H(KalmanVars, SINSstate);
KalmanProcs.KalmanCorrection(KalmanVars, SINSstate, SINSstate);
Alignment_Navigation.CalcStateErrors(KalmanVars.ErrorConditionVector_p, SINSstate);
Alignment_Navigation.StateCorrection(KalmanVars.ErrorConditionVector_p, SINSstate, SINSstate2);
Alignment.OutPutInfo_Nav_Alignment(ProcHelp, SINSstate, SINSstate2, myFile, KalmanVars, Alignment_Errors, Alignment_SINSstate, Alignment_Corrected_State, Alignment_StateErrorsVector);
SimpleOperations.CopyArray(SINSstate.F_z_prev, SINSstate.F_z);
SimpleOperations.CopyArray(SINSstate.W_z_prev, SINSstate.W_z);
if (i > 100 && i % 500 == 0)
{
Console.WriteLine(SINSstate.Count.ToString() + ", " + (SINSstate.Longitude * SimpleData.ToDegree).ToString() + ", " + (SINSstate.Heading * SimpleData.ToDegree).ToString() + ", " +
(SINSstate2.Heading * SimpleData.ToDegree).ToString() + ", " + KalmanVars.ErrorConditionVector_p[0].ToString() + ", " + KalmanVars.ErrorConditionVector_p[1].ToString());
}
i++;
}
SINSstate.Heading = SINSstate.Heading - SINSstate.DeltaHeading;
SINSstate.Roll = SINSstate.Roll - SINSstate.DeltaRoll;
SINSstate.Pitch = SINSstate.Pitch - SINSstate.DeltaPitch;
KalmanVars.CovarianceMatrixS_m[0 * SimpleData.iMx + 0] = KalmanVars.CovarianceMatrixS_p[0 * SimpleData.iMx + 0] = KalmanProcs.Sigmf_Disp(0, KalmanVars);
KalmanVars.CovarianceMatrixS_m[1 * SimpleData.iMx + 1] = KalmanVars.CovarianceMatrixS_p[1 * SimpleData.iMx + 1] = KalmanProcs.Sigmf_Disp(1, KalmanVars);
KalmanVars.CovarianceMatrixS_m[2 * SimpleData.iMx + 2] = KalmanVars.CovarianceMatrixS_p[2 * SimpleData.iMx + 2] = KalmanProcs.Sigmf_Disp(2, KalmanVars);
KalmanVars.CovarianceMatrixS_m[3 * SimpleData.iMx + 3] = KalmanVars.CovarianceMatrixS_p[3 * SimpleData.iMx + 3] = KalmanProcs.Sigmf_Disp(3, KalmanVars);
KalmanVars.CovarianceMatrixS_m[10 * SimpleData.iMx + 10] = KalmanVars.CovarianceMatrixS_p[10 * SimpleData.iMx + 10] = KalmanProcs.Sigmf_Disp(10, KalmanVars);
KalmanVars.CovarianceMatrixS_m[11 * SimpleData.iMx + 11] = KalmanVars.CovarianceMatrixS_p[11 * SimpleData.iMx + 11] = KalmanProcs.Sigmf_Disp(11, KalmanVars);
KalmanVars.CovarianceMatrixS_m[12 * SimpleData.iMx + 12] = KalmanVars.CovarianceMatrixS_p[12 * SimpleData.iMx + 12] = KalmanProcs.Sigmf_Disp(12, KalmanVars);
KalmanVars.CovarianceMatrixS_m[7 * SimpleData.iMx + 7] = KalmanVars.CovarianceMatrixS_p[7 * SimpleData.iMx + 7] = KalmanProcs.Sigmf_Disp(7, KalmanVars);
KalmanVars.CovarianceMatrixS_m[8 * SimpleData.iMx + 8] = KalmanVars.CovarianceMatrixS_p[8 * SimpleData.iMx + 8] = KalmanProcs.Sigmf_Disp(8, KalmanVars);
KalmanVars.CovarianceMatrixS_m[9 * SimpleData.iMx + 9] = KalmanVars.CovarianceMatrixS_p[9 * SimpleData.iMx + 9] = KalmanProcs.Sigmf_Disp(9, KalmanVars);
SINSstate.Time_Alignment = SINSstate.Time;
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //
if (SINSstate.Global_file == "Azimuth_minsk_race_4_3to6to2")
{
//SINSstate.Heading = -3.0504734;
}
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //
SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate);
SINSstate.A_x0s = SINSstate.A_sx0.Transpose();
SINSstate.A_x0n = SimpleOperations.A_x0n(SINSstate.Latitude, SINSstate.Longitude);
SINSstate.A_nx0 = SINSstate.A_x0n.Transpose();
SINSstate.A_nxi = SimpleOperations.A_ne(SINSstate.Time - SINSstate.Time_Alignment, SINSstate.Longitude_Start);
SINSstate.AT = Matrix.Multiply(SINSstate.A_sx0, SINSstate.A_x0n);
SINSstate.AT = Matrix.Multiply(SINSstate.AT, SINSstate.A_nxi);
Alignment_Errors.Close(); Alignment_Corrected_State.Close(); Alignment_SINSstate.Close(); Alignment_StateErrorsVector.Close();
/*----------------------------------------------------------------------------------------*/
}
ProcHelp.initCount = false;
SINSstate.flag_Alignment = false;
return(i);
}
