public static void KalmanCorrection_Align(Kalman_Align KalmanAlign)
{
for (int i = 0; i < SimpleData.iMx_Align; i++)
{
KalmanAlign.KalmanFactor[i] = 0.0;
}
unsafe
{
fixed(double *_xm = KalmanAlign.ErrorConditionVector_m, _xp = KalmanAlign.ErrorConditionVector_p, _sm = KalmanAlign.CovarianceMatrixS_m, _sp = KalmanAlign.CovarianceMatrixS_p, _kf = KalmanAlign.KalmanFactor)
{
for (int t = 0; t < KalmanAlign.cnt_measures; t++)
{
for (int i = 0; i < SimpleData.iMx_Align; i++)
{
KalmanAlign.StringOfMeasure[i] = KalmanAlign.Matrix_H[t * SimpleData.iMx_Align + i];
fixed(double *_h = KalmanAlign.StringOfMeasure)
{
//Коррекция по измерениям
f0b(KalmanAlign.Measure[t], _xm, _sm, _h, KalmanAlign.Noize_Z[t] * KalmanAlign.Noize_Z[t], _xp, _sp, _kf, SimpleData.iMx_Align);
if (t < KalmanAlign.cnt_measures - 1)
{
for (int i = 0; i < SimpleData.iMx_Align; i++)
{
_xm[i] = _xp[i];
for (int j = 0; j < SimpleData.iMx_Align; j++)
{
_sm[i * SimpleData.iMx_Align + j] = _sp[i * SimpleData.iMx_Align + j];
}
}
}
}
}
}
}
}
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);
}
}
