public static void Redifinition_OdoCounts(SINS_State SINSstate, SINS_State SINSstate_OdoMod)
{
if (SINSstate.OdometerData.odometer_left.isReady == 1)
{
if (SINSstate.flag_UsingCorrection == true || SINSstate.flag_AutonomouseSolution == true)
{
SINSstate.OdometerLeftPrev = SINSstate.OdometerData.odometer_left.Value;
SINSstate.OdometerRightPrev = SINSstate.OdometerData.odometer_right.Value;
SINSstate.OdoTimeStepCount = 0;
SINSstate.odotime_prev = SINSstate.Time;
SINSstate.Latitude_prev = SINSstate.Latitude;
SINSstate.Longitude_prev = SINSstate.Longitude;
SINSstate.Altitude_prev = SINSstate.Height;
/* --- Запоминаем предыдущие значения показания одометра --- */
for (int i = 0; i < SINSstate.OdometerLeft_ArrayOfPrev.Length - 1; i++)
{
SINSstate.OdometerLeft_ArrayOfPrev[SINSstate.OdometerLeft_ArrayOfPrev.Length - 1 - i]
= SINSstate.OdometerLeft_ArrayOfPrev[SINSstate.OdometerLeft_ArrayOfPrev.Length - 1 - i - 1];
SINSstate.OdometerLeft_ArrayOfPrevTime[SINSstate.OdometerLeft_ArrayOfPrev.Length - 1 - i]
= SINSstate.OdometerLeft_ArrayOfPrevTime[SINSstate.OdometerLeft_ArrayOfPrev.Length - 1 - i - 1];
}
SINSstate.OdometerLeft_ArrayOfPrev[0] = SINSstate.OdometerData.odometer_left.Value;
SINSstate.OdometerLeft_ArrayOfPrevTime[0] = SINSstate.Time + SINSstate.Time_Alignment;
}
}
}
public static void InitOfCovarianceMatrixes(SINS_State SINSstate, Kalman_Vars KalmanVars)
{
SINSprocessing.InitOfCovarianceMatrixes(SINSstate, KalmanVars);
// --- нач. ковариации горизонтальных ошибок координат одометрического счисления --- //
KalmanVars.CovarianceMatrixS_m[SINSstate.value_iMx_r_odo_12 * SimpleData.iMx + SINSstate.value_iMx_r_odo_12]
= KalmanVars.CovarianceMatrixS_p[SINSstate.value_iMx_r_odo_12 * SimpleData.iMx + SINSstate.value_iMx_r_odo_12] = SINSstate.stdOdoR;
KalmanVars.CovarianceMatrixS_m[(SINSstate.value_iMx_r_odo_12 + 1) * SimpleData.iMx + (SINSstate.value_iMx_r_odo_12 + 1)]
= KalmanVars.CovarianceMatrixS_p[(SINSstate.value_iMx_r_odo_12 + 1) * SimpleData.iMx + (SINSstate.value_iMx_r_odo_12 + 1)] = SINSstate.stdOdoR;
// --- нач. ковариации ошибки высоты одометрического счисления --- //
if (SINSstate.flag_iMx_r3_dV3)
{
KalmanVars.CovarianceMatrixS_m[(SINSstate.value_iMx_r_odo_3 + 0) * SimpleData.iMx + (SINSstate.value_iMx_r_odo_3 + 0)]
= KalmanVars.CovarianceMatrixS_p[(SINSstate.value_iMx_r_odo_3 + 0) * SimpleData.iMx + (SINSstate.value_iMx_r_odo_3 + 0)] = SINSstate.stdOdoR;
}
SimpleOperations.PrintMatrixToFile(KalmanVars.CovarianceMatrixS_m, SimpleData.iMx, SimpleData.iMx, "StartCovariance");
// ------------------ВЕРТИКАЛЬНЫЙ КАНАЛ ОТДЕЛЬНО-----------------------//
if (SINSstate.flag_SeparateHorizVSVertical == true)
{
KalmanVars.Vertical_CovarianceMatrixS_m[SINSstate.Vertical_rOdo3 * SimpleData.iMx_Vertical + SINSstate.Vertical_rOdo3]
= KalmanVars.Vertical_CovarianceMatrixS_p[SINSstate.Vertical_rOdo3 * SimpleData.iMx_Vertical + SINSstate.Vertical_rOdo3] = SINSstate.stdOdoR;
}
}
public static void ApplyCompensatedErrorsToSolution(SINS_State SINSstate)
{
if (SINSstate.Heading > Math.PI)
{
SINSstate.Heading = SINSstate.Heading - 2.0 * Math.PI;
}
if (SINSstate.Heading < -Math.PI)
{
SINSstate.Heading = SINSstate.Heading + 2.0 * Math.PI;
}
//корректированная матрица ориентации
SINSstate.A_sx0 = A_sx0(SINSstate);
SINSstate.A_x0s = SINSstate.A_sx0.Transpose();
SINSstate.A_x0n = A_x0n(SINSstate.Latitude, SINSstate.Longitude);
SINSstate.A_nx0 = SINSstate.A_x0n.Transpose();
SINSstate.AT = Matrix.Multiply(SINSstate.A_sx0, SINSstate.A_x0n);
SINSstate.AT = Matrix.Multiply(SINSstate.AT, SINSstate.A_nxi);
SINSstate.R_e = RadiusE(SINSstate.Latitude, SINSstate.Height);
SINSstate.R_n = RadiusN(SINSstate.Latitude, SINSstate.Height);
SINSstate.u_x = U_x0(SINSstate.Latitude);
SINSstate.Omega_x[0] = -(SINSstate.Vx_0[1] + SINSstate.Vx_0_prev[1]) / 2.0 / SINSstate.R_n;
SINSstate.Omega_x[1] = (SINSstate.Vx_0[0] + SINSstate.Vx_0_prev[0]) / 2.0 / SINSstate.R_e;
SINSstate.Omega_x[2] = Math.Tan(SINSstate.Latitude) * SINSstate.Omega_x[1];
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;
SINSstate.g -= 2 * 0.000001538 * SINSstate.Height;
}
public static Matrix C_convultion_ScaleError(SINS_State SINSstate)
{
Matrix MatrixResult = new Matrix(1, SimpleData.iMx);;
MatrixResult[0, SINSstate.value_iMx_kappa_3_ds + 1] = 1.0;
return(MatrixResult);
}
public static void AnglesHRP(double CurrentTime, SINS_State SINSstate, double StartHeading, double StartRoll, double StartPitch)
{
double[] ResultVect = new double[3];
SINSstate.Heading = StartHeading + 30.0 * SimpleData.ToRadian * Math.Sin(0.04 * CurrentTime);
SINSstate.Roll = StartRoll + 5.0 * SimpleData.ToRadian * Math.Sin(0.021 * CurrentTime);
SINSstate.Pitch = StartPitch + 0.0 * SimpleData.ToRadian * Math.Sin(0.025 * CurrentTime);
}
public static void KalmanForecast(Kalman_Vars KalmanVars, SINS_State SINSstate)
{
unsafe
{
fixed(double *_xm = KalmanVars.ErrorConditionVector_m, _xp = KalmanVars.ErrorConditionVector_p, _sm = KalmanVars.CovarianceMatrixS_m, _sp = KalmanVars.CovarianceMatrixS_p,
_f = KalmanVars.TransitionMatrixF, _sq = KalmanVars.CovarianceMatrixNoise)
{
dgq0b(_xp, _sp, _f, _sq, _xm, _sm, SimpleData.iMx, SimpleData.iMq);
}
}
SimpleOperations.CopyArray(KalmanVars.CovarianceMatrixS_p, KalmanVars.CovarianceMatrixS_m);
SimpleOperations.CopyArray(KalmanVars.ErrorConditionVector_p, KalmanVars.ErrorConditionVector_m);
// ----------------------------------------------------------//
if (SINSstate.flag_SeparateHorizVSVertical == true)
{
unsafe
{
fixed(double *_xm = KalmanVars.Vertical_ErrorConditionVector_m, _xp = KalmanVars.Vertical_ErrorConditionVector_p, _sm = KalmanVars.Vertical_CovarianceMatrixS_m,
_sp = KalmanVars.Vertical_CovarianceMatrixS_p, _f = KalmanVars.Vertical_TransitionMatrixF, _sq = KalmanVars.Vertical_CovarianceMatrixNoise)
{
dgq0b(_xp, _sp, _f, _sq, _xm, _sm, SimpleData.iMx_Vertical, SimpleData.iMq_Vertical);
}
}
SimpleOperations.CopyArray(KalmanVars.Vertical_CovarianceMatrixS_p, KalmanVars.Vertical_CovarianceMatrixS_m);
SimpleOperations.CopyArray(KalmanVars.Vertical_ErrorConditionVector_p, KalmanVars.Vertical_ErrorConditionVector_m);
}
}
public static void Smoothing(Kalman_Vars KalmanVars, SINS_State SINSstate, int iMx)
{
double[] temp_array_1 = new double[iMx * iMx], temp_array_2 = new double[iMx * iMx];
if (iMx == 1)
{
KalmanVars.ErrorVector_Smoothed[0] =
(Math.Pow(KalmanVars.CovarianceMatrix_SP_m[0], 2) * KalmanVars.ErrorVector_Straight[0]
+ Math.Pow(KalmanVars.CovarianceMatrix_SP_Straight[0], 2) * KalmanVars.ErrorVector_m[0]
) /
(Math.Pow(KalmanVars.CovarianceMatrix_SP_m[0], 2) + Math.Pow(KalmanVars.CovarianceMatrix_SP_Straight[0], 2))
;
}
else
{
unsafe
{
fixed(double *Xb = KalmanVars.ErrorVector_m, Sb = KalmanVars.CovarianceMatrix_SP_m, Xf = KalmanVars.ErrorVector_Straight, Sf = KalmanVars.CovarianceMatrix_SP_Straight,
Xsgl = KalmanVars.ErrorVector_Smoothed, Ssgl = KalmanVars.CovarianceMatrix_SP_Smoothed, _temp_1 = temp_array_1, _temp_2 = temp_array_2)
{
smooth_sub(Xf, Sf, Xb, Sb, Xsgl, Ssgl, iMx, _temp_1, _temp_2);
}
}
}
}
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 Check_Measurement(SINS_State SINSstate, Kalman_Vars KalmanVars)
{
SimpleOperations.NullingOfArray(KalmanVars.StringOfMeasure);
SimpleOperations.NullingOfArray(KalmanVars.pdResidual);
SimpleOperations.NullingOfArray(KalmanVars.pdSigmaApriori);
for (int i = 0; i < KalmanVars.cnt_measures; i++)
{
KalmanVars.pdResidual[i] = KalmanVars.Measure[i];
for (int j = 0; j < SimpleData.iMx; j++)
{
KalmanVars.StringOfMeasure[j] = KalmanVars.Matrix_H[i * SimpleData.iMx + j];
KalmanVars.pdResidual[i] -= KalmanVars.StringOfMeasure[j];
}
unsafe
{
fixed(double *StringOfMeasure = KalmanVars.StringOfMeasure, CovarianceMatrixS_m = KalmanVars.CovarianceMatrixS_m)
{
KalmanVars.pdSigmaApriori[i] = Math.Sqrt(sigmba(StringOfMeasure, CovarianceMatrixS_m, SimpleData.iMx)) + KalmanVars.Noize_Z[i] * KalmanVars.Noize_Z[i];
}
}
}
}
public static void CalcStateErrors(double[] ErrorVector, SINS_State SINSstate, SINS_State SINSstate_OdoMod, Kalman_Vars KalmanVars)
{
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
;
SINSstate.DeltaLatitude = ErrorVector[1] / SINSstate.R_n;
SINSstate.DeltaLongitude = ErrorVector[0] / SINSstate.R_e / Math.Cos(SINSstate.Latitude);
SINSstate.DeltaV_1 = ErrorVector[iMx_dV_12 + 0];
SINSstate.DeltaV_2 = ErrorVector[iMx_dV_12 + 1];
SINSstate.DeltaRoll = -(ErrorVector[iMx_alphaBeta + 0] * Math.Sin(SINSstate.Heading) + ErrorVector[iMx_alphaBeta + 1] * Math.Cos(SINSstate.Heading)) / Math.Cos(SINSstate.Pitch);
SINSstate.DeltaPitch = -ErrorVector[iMx_alphaBeta + 0] * Math.Cos(SINSstate.Heading) + ErrorVector[iMx_alphaBeta + 1] * Math.Sin(SINSstate.Heading);
SINSstate.DeltaHeading = ErrorVector[iMx_alphaBeta + 2] + SINSstate.DeltaRoll * Math.Sin(SINSstate.Pitch);
//--- Поправки к одометрическому счислению ---//
SINSstate_OdoMod.DeltaLatitude = ErrorVector[iMx_r12_odo + 1] / SINSstate_OdoMod.R_n;
SINSstate_OdoMod.DeltaLongitude = ErrorVector[iMx_r12_odo + 0] / SINSstate_OdoMod.R_e / Math.Cos(SINSstate_OdoMod.Latitude);
// ----------------------------------------------------------//
// -------Поправки на основе оценок вертикального канала-----------//
// ----------------------------------------------------------//
SINSstate.DeltaAltitude = KalmanVars.Vertical_ErrorConditionVector_p[0];
SINSstate.DeltaV_3 = KalmanVars.Vertical_ErrorConditionVector_p[1];
SINSstate_OdoMod.DeltaAltitude = KalmanVars.Vertical_ErrorConditionVector_p[SINSstate.Vertical_rOdo3];
}
//--------------------------------------------------------------------------
//-------------------------------КНС---------------------------------------
//--------------------------------------------------------------------------
public static void Make_H_KNS(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod)
{
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
;
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)] = KalmanVars.OdoNoise_STOP;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = KalmanVars.OdoNoise_STOP;
KalmanVars.cnt_measures += 2;
if (SINSstate.flag_iMx_r3_dV3)
{
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + value_iMx_dV3] = 1.0;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = KalmanVars.OdoNoise_STOP;
KalmanVars.cnt_measures += 1;
}
}
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 KalmanForecast(Kalman_Vars KalmanVars, SINS_State SINSstate)
{
unsafe
{
fixed(double *_xm = KalmanVars.ErrorConditionVector_m, _xp = KalmanVars.ErrorConditionVector_p, _sm = KalmanVars.CovarianceMatrixS_m, _sp = KalmanVars.CovarianceMatrixS_p,
_f = KalmanVars.TransitionMatrixF, _sq = KalmanVars.CovarianceMatrixNoise)
{
dgq0b(_xp, _sp, _f, _sq, _xm, _sm, SimpleData.iMx, SimpleData.iMq);
}
}
SimpleOperations.CopyArray(KalmanVars.CovarianceMatrixS_p, KalmanVars.CovarianceMatrixS_m);
SimpleOperations.CopyArray(KalmanVars.ErrorConditionVector_p, KalmanVars.ErrorConditionVector_m);
// ----------------------------------------------------------//
// -----------------Прогноз для вертикального фильтра--------------------//
unsafe
{
fixed(double *_xm = KalmanVars.Vertical_ErrorConditionVector_m, _xp = KalmanVars.Vertical_ErrorConditionVector_p, _sm = KalmanVars.Vertical_CovarianceMatrixS_m,
_sp = KalmanVars.Vertical_CovarianceMatrixS_p, _f = KalmanVars.Vertical_TransitionMatrixF, _sq = KalmanVars.Vertical_CovarianceMatrixNoise)
{
dgq0b(_xp, _sp, _f, _sq, _xm, _sm, SimpleData.iMx_Vertical, SimpleData.iMq_Vertical);
}
}
SimpleOperations.CopyArray(KalmanVars.Vertical_CovarianceMatrixS_p, KalmanVars.Vertical_CovarianceMatrixS_m);
SimpleOperations.CopyArray(KalmanVars.Vertical_ErrorConditionVector_p, KalmanVars.Vertical_ErrorConditionVector_m);
}
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------ПОЗИЦИОННАЯ КОРЕКЦИЯ-----------------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
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 double[] RelativeAngular_x0s(double CurrentTime, SINS_State SINSstate, double[] dAngular)
{
double[] ResultVect = new double[3];
ResultVect[0] = -dAngular[2] * Math.Cos(SINSstate.Heading) - dAngular[1] * Math.Sin(SINSstate.Heading) * Math.Cos(SINSstate.Pitch);
ResultVect[1] = dAngular[2] * Math.Sin(SINSstate.Heading) - dAngular[1] * Math.Cos(SINSstate.Heading) * Math.Cos(SINSstate.Pitch);
ResultVect[2] = dAngular[0] - dAngular[1] * Math.Sin(SINSstate.Pitch);
return(ResultVect);
}
public static double[] RelativeAngular_sx0(double CurrentTime, SINS_State SINSstate, double[] dAngular)
{
double[] ResultVect = new double[3];
ResultVect[0] = dAngular[2] * Math.Cos(SINSstate.Roll) + dAngular[0] * Math.Cos(SINSstate.Pitch) * Math.Sin(SINSstate.Roll);
ResultVect[1] = dAngular[1] - dAngular[0] * Math.Sin(SINSstate.Pitch);
ResultVect[2] = dAngular[2] * Math.Sin(SINSstate.Roll) - dAngular[0] * Math.Cos(SINSstate.Pitch) * Math.Cos(SINSstate.Roll);
return(ResultVect);
}
public static double[] DerivativeOfAnglesHRP(double dT, SINS_State SINSstate)
{
double[] ResultVect = new double[3];
ResultVect[0] = (SINSstate.Heading - SINSstate.Heading_prev) / dT;
ResultVect[1] = (SINSstate.Roll - SINSstate.Roll_prev) / dT;
ResultVect[2] = (SINSstate.Pitch - SINSstate.Pitch_prev) / dT;
return(ResultVect);
}
public static SINS_State DeepCopy(SINS_State other)
{
using (MemoryStream ms = new MemoryStream())
{
BinaryFormatter formatter = new BinaryFormatter();
formatter.Serialize(ms, other);
ms.Position = 0;
return((SINS_State)formatter.Deserialize(ms));
}
}
//--------------------------------------------------------------------------
//--------------------------СКОРОСТНАЯ КОРЕКЦИЯ-----------------------------
//--------------------------------------------------------------------------
public static void Make_H_VELOCITY(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod)
{
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;
for (int i = 0; i < 3; i++)
{
KalmanVars.Measure[(KalmanVars.cnt_measures + i)] = SINSstate.Vx_0[i] - SINSstate_OdoMod.Vx_0[i];
}
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 2] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + 3] = 1.0;
//---КОРРЕКТИРОВАНИЕ ПО СКОРОСТИ В ПРОЕКЦИИ НА ГЕОГРАФИЮ---
if (SINSstate.flag_OdoSINSWeakConnect_MODIF)
{
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 0] = -SINSstate.Omega_x[0] * Math.Tan(SINSstate.Latitude);
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r12_odo] = SINSstate.Omega_x[0] * Math.Tan(SINSstate.Latitude);
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + 0] = -SINSstate.Omega_x[2];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + iMx_r12_odo] = SINSstate.Omega_x[2];
}
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[0, 2];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_odo_model + 1] = -SINSstate.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[0, 0];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_odo_model + 2] = -SINSstate.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[0, 1];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + iMx_odo_model + 0] = SINSstate.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[1, 2];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + iMx_odo_model + 1] = -SINSstate.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[1, 0];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + iMx_odo_model + 2] = -SINSstate.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[1, 1];
}
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = SINSstate.A_x0s[0, 1] * KalmanVars.OdoNoise_V;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = SINSstate.A_x0s[1, 1] * KalmanVars.OdoNoise_V;
KalmanVars.cnt_measures += 2;
if (SINSstate.flag_iMx_r3_dV3)
{
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;
}
}
public static Matrix C_convultion_HorizontalVelocity(SINS_State SINSstate)
{
Matrix MatrixResult = new Matrix(2, SimpleData.iMx);
MatrixResult[0, SINSstate.value_iMx_dV_12 + 0] = 1.0;
MatrixResult[0, SINSstate.value_iMx_alphaBeta + 2] = SINSstate.Vx_0[1];
MatrixResult[1, SINSstate.value_iMx_dV_12 + 1] = 1.0;
MatrixResult[1, SINSstate.value_iMx_alphaBeta + 2] = -SINSstate.Vx_0[0];
return(MatrixResult);
}
public static void ApplyMatrixStartCondition(SINS_State SINSstate)
{
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.Longitude_Start);
SINSstate.AT = Matrix.Multiply(SINSstate.A_sx0, SINSstate.A_x0n);
SINSstate.AT = Matrix.Multiply(SINSstate.AT, SINSstate.A_nxi);
SINSstate.R_e = RadiusE(SINSstate.Latitude, SINSstate.Height);
SINSstate.R_n = RadiusN(SINSstate.Latitude, SINSstate.Height);
}
public static Matrix MakeC_forSmoothing(double[] ErrVect, SINS_State SINSstate)
{
Matrix MatrixResult = new Matrix(7, SimpleData.iMx);
MatrixResult[0, 1] = 1.0 / SINSstate.R_n;
MatrixResult[1, 0] = 1.0 / SINSstate.R_e / Math.Cos(SINSstate.Latitude);
MatrixResult[2, 2] = 1.0;
MatrixResult[2, 7] = SINSstate.Vx_0[1];
MatrixResult[3, 3] = 1.0;
MatrixResult[3, 7] = -SINSstate.Vx_0[0];
return(MatrixResult);
}
/*-------------------------------Вспомогательные функции---------------------------------------------------------*/
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 void Make_H_GPS(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod)
{
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];
//---КОРРЕКЦИЯ ПО СПУТНИКОВОЙ ИНФОРМАЦИИ---
if (SINSstate.flag_Using_SNS == true && SINSstate.GPS_Data.gps_Altitude.isReady == 1 && Math.Abs(SINSstate.GPS_Data.gps_Latitude.Value) > 0.01)
{
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 0] = 1.0;
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = (SINSstate.Longitude - SINSstate.GPS_Data.gps_Longitude.Value) * RadiusE(SINSstate.GPS_Data.gps_Latitude.Value, SINSstate.Height) * Math.Cos(SINSstate.GPS_Data.gps_Latitude.Value);
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 3.0;
KalmanVars.cnt_measures += 1;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 1] = 1.0;
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = (SINSstate.Latitude - SINSstate.GPS_Data.gps_Latitude.Value) * RadiusN(SINSstate.GPS_Data.gps_Latitude.Value, SINSstate.Height);
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 3.0;
KalmanVars.cnt_measures += 1;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + (iMx_dV_12 + 0)] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + (iMx_alphaBeta + 2)] = SINSstate.Vx_0[1];
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Vx_0[0] - SINSstate.GPS_Data.gps_Ve.Value;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 1.0;
KalmanVars.cnt_measures += 1;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + (iMx_dV_12 + 1)] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + (iMx_alphaBeta + 2)] = -SINSstate.Vx_0[0];
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Vx_0[1] - SINSstate.GPS_Data.gps_Vn.Value;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 1.0;
KalmanVars.cnt_measures += 1;
if (SINSstate.flag_iMx_r3_dV3)
{
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + value_iMx_dr3] = 1.0;
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Height - SINSstate.GPS_Data.gps_Altitude.Value;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 5.0;
KalmanVars.cnt_measures += 1;
}
}
}
public static void FillKMLOutputFile(SINS_State SINSstate, StreamWriter KMLFileOut, string Part, string append)
{
if (Part == "Start")
{
KMLFileOut.WriteLine("<?xml version='1.0' encoding='UTF-8'?> ");
KMLFileOut.WriteLine("<kml xmlns='http://earth.google.com/kml/2.2'> ");
KMLFileOut.WriteLine("<Document> ");
KMLFileOut.WriteLine("<name>" + append + SINSstate.DataInFileName + "</name>");
KMLFileOut.WriteLine("<visibility>1</visibility> ");
KMLFileOut.WriteLine("<open>1</open> ");
KMLFileOut.WriteLine("<Style id='MarkerIcon'> ");
KMLFileOut.WriteLine(" <IconStyle> ");
KMLFileOut.WriteLine(" <scale>1</scale> ");
KMLFileOut.WriteLine(" <Icon> ");
KMLFileOut.WriteLine(" <href>http://maps.google.com/mapfiles/kml/shapes/cross-hairs.png</href>");
KMLFileOut.WriteLine(" </Icon> ");
KMLFileOut.WriteLine(" </IconStyle> ");
KMLFileOut.WriteLine("</Style> ");
KMLFileOut.WriteLine("<Style id='MarkerLine'> ");
KMLFileOut.WriteLine(" <LineStyle> ");
KMLFileOut.WriteLine(" <color>ff000000</color> ");
KMLFileOut.WriteLine(" <width>2</width> ");
KMLFileOut.WriteLine(" </LineStyle> ");
KMLFileOut.WriteLine("</Style> ");
KMLFileOut.WriteLine("<Folder> ");
KMLFileOut.WriteLine(" <name>Path</name> ");
KMLFileOut.WriteLine(" <visibility>1</visibility> ");
KMLFileOut.WriteLine(" <open>0</open> ");
KMLFileOut.WriteLine(" <Placemark> ");
KMLFileOut.WriteLine(" <name>Markers</name> ");
KMLFileOut.WriteLine(" <visibility>1</visibility> ");
KMLFileOut.WriteLine(" <description>The markers scheme</description> ");
KMLFileOut.WriteLine(" <styleUrl>#MarkerLine</styleUrl> ");
KMLFileOut.WriteLine(" <LineString> ");
KMLFileOut.WriteLine(" <extrude>0</extrude> ");
KMLFileOut.WriteLine(" <tessellate>1</tessellate> ");
KMLFileOut.WriteLine(" <altitudeMode>clampToGround</altitudeMode> ");
KMLFileOut.WriteLine(" <coordinates> ");
}
else if (Part == "End")
{
KMLFileOut.WriteLine(" </coordinates>");
KMLFileOut.WriteLine(" </LineString> ");
KMLFileOut.WriteLine(" </Placemark> ");
KMLFileOut.WriteLine("</Folder> ");
KMLFileOut.WriteLine("</Document> ");
KMLFileOut.WriteLine("</kml> ");
}
}
public static void MatrixNoise_ReDef(SINS_State SINSstate, Kalman_Vars KalmanVars, bool AlignmentFLG)
{
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
;
SINSprocessing.MatrixNoise_ReDef(SINSstate, KalmanVars, SINSstate.flag_Alignment);
double sqrt_freq = Math.Sqrt(SINSstate.Freq);
//sqrt_freq = Math.Sqrt(Math.Abs(SINSstate.timeStep) / SINSstate.TimeBetweenForecast);
//sqrt_freq = 1.0;
if (SINSstate.flag_iMqDeltaRodo)
{
KalmanVars.CovarianceMatrixNoise[(iMx_r12_odo + 0) * iMq + iMx_r12_odo + 0] = KalmanVars.Noise_Pos * sqrt_freq;
KalmanVars.CovarianceMatrixNoise[(iMx_r12_odo + 1) * iMq + iMx_r12_odo + 1] = KalmanVars.Noise_Pos * sqrt_freq;
if (SINSstate.flag_iMx_r3_dV3)
{
KalmanVars.CovarianceMatrixNoise[(iMx_r_odo_3 + 0) * iMq + iMx_r_odo_3 + 0] = KalmanVars.Noise_Pos * sqrt_freq;
}
}
// --- Считаем, что по вертикали у нас шумы только по скорости ---
// ----------------------------------------------------------//
if (SINSstate.flag_SeparateHorizVSVertical == true)
{
if (SINSstate.flag_iMqDeltaRodo)
{
KalmanVars.Vertical_CovarianceMatrixNoise[SINSstate.Vertical_rOdo3 * SimpleData.iMq_Vertical + SINSstate.Vertical_rOdo3] = KalmanVars.Noise_Pos_Odo * sqrt_freq;
}
if (SINSstate.flag_iMqVarkappa1)
{
KalmanVars.Vertical_CovarianceMatrixNoise[SINSstate.Vertical_kappa1 * SimpleData.iMq_Vertical + SINSstate.Vertical_kappa1] = KalmanVars.Noise_OdoKappa_1 * sqrt_freq;
}
}
}
public static Matrix A_cx0(SINS_State SINSState)
{
Matrix MatrixResult = new Matrix(3, 3);
MatrixResult[0, 0] = Math.Cos(SINSState.CourseHeading);
MatrixResult[0, 1] = -Math.Sin(SINSState.CourseHeading);
MatrixResult[0, 2] = 0.0;
MatrixResult[1, 0] = Math.Sin(SINSState.CourseHeading) * Math.Cos(SINSState.CoursePitch);
MatrixResult[1, 1] = Math.Cos(SINSState.CourseHeading) * Math.Cos(SINSState.CoursePitch);
MatrixResult[1, 2] = Math.Sin(SINSState.CoursePitch);
MatrixResult[2, 0] = -Math.Sin(SINSState.CourseHeading) * Math.Sin(SINSState.CoursePitch);
MatrixResult[2, 1] = -Math.Cos(SINSState.CourseHeading) * Math.Sin(SINSState.CoursePitch);
MatrixResult[2, 2] = Math.Cos(SINSState.CoursePitch);
return(MatrixResult);
}
public static Matrix A_sx0_Gyro(SINS_State SINSState)
{
Matrix MatrixResult = new Matrix(3, 3);
MatrixResult[0, 0] = Math.Cos(SINSState.GyroHeading) * Math.Cos(SINSState.Roll) + Math.Sin(SINSState.GyroHeading) * Math.Sin(SINSState.Pitch) * Math.Sin(SINSState.Roll);
MatrixResult[0, 1] = -Math.Sin(SINSState.GyroHeading) * Math.Cos(SINSState.Roll) + Math.Cos(SINSState.GyroHeading) * Math.Sin(SINSState.Pitch) * Math.Sin(SINSState.Roll);
MatrixResult[0, 2] = -Math.Cos(SINSState.Pitch) * Math.Sin(SINSState.Roll);
MatrixResult[1, 0] = Math.Sin(SINSState.GyroHeading) * Math.Cos(SINSState.Pitch);
MatrixResult[1, 1] = Math.Cos(SINSState.GyroHeading) * Math.Cos(SINSState.Pitch);
MatrixResult[1, 2] = Math.Sin(SINSState.Pitch);
MatrixResult[2, 0] = Math.Cos(SINSState.GyroHeading) * Math.Sin(SINSState.Roll) - Math.Sin(SINSState.GyroHeading) * Math.Sin(SINSState.Pitch) * Math.Cos(SINSState.Roll);
MatrixResult[2, 1] = -Math.Sin(SINSState.GyroHeading) * Math.Sin(SINSState.Roll) - Math.Cos(SINSState.GyroHeading) * Math.Sin(SINSState.Pitch) * Math.Cos(SINSState.Roll);
MatrixResult[2, 2] = Math.Cos(SINSState.Pitch) * Math.Cos(SINSState.Roll);
return(MatrixResult);
}
public static Matrix C_convultion_Angles(SINS_State SINSstate)
{
Matrix MatrixResult = new Matrix(3, SimpleData.iMx);
MatrixResult[0, SINSstate.value_iMx_alphaBeta + 0] = -Math.Cos(SINSstate.Heading);
MatrixResult[0, SINSstate.value_iMx_alphaBeta + 1] = Math.Sin(SINSstate.Heading);
MatrixResult[1, SINSstate.value_iMx_alphaBeta + 0] = -Math.Sin(SINSstate.Heading) / Math.Cos(SINSstate.Pitch);
MatrixResult[1, SINSstate.value_iMx_alphaBeta + 1] = -Math.Cos(SINSstate.Heading) / Math.Cos(SINSstate.Pitch);
MatrixResult[2, SINSstate.value_iMx_alphaBeta + 0] = -Math.Sin(SINSstate.Heading) * Math.Tan(SINSstate.Pitch);
MatrixResult[2, SINSstate.value_iMx_alphaBeta + 1] = -Math.Cos(SINSstate.Heading) * Math.Tan(SINSstate.Pitch);
MatrixResult[2, SINSstate.value_iMx_alphaBeta + 2] = 1.0;
MatrixResult[2, 0] = 1.0 / SINSstate.R_e * Math.Tan(SINSstate.Latitude);
return(MatrixResult);
}
public static Matrix A_xs(SINS_State SINSState)
{
Matrix MatrixResult = new Matrix(3, 3);
MatrixResult[0, 0] = Math.Cos(SINSState.Roll);
MatrixResult[0, 1] = 0.0;
MatrixResult[0, 2] = Math.Sin(SINSState.Roll);
MatrixResult[1, 0] = Math.Sin(SINSState.Pitch) * Math.Sin(SINSState.Roll);
MatrixResult[1, 1] = Math.Cos(SINSState.Pitch);
MatrixResult[1, 2] = -Math.Sin(SINSState.Pitch) * Math.Cos(SINSState.Roll);
MatrixResult[2, 0] = -Math.Cos(SINSState.Pitch) * Math.Sin(SINSState.Roll);
MatrixResult[2, 1] = Math.Sin(SINSState.Pitch);
MatrixResult[2, 2] = Math.Cos(SINSState.Pitch) * Math.Cos(SINSState.Roll);
return(MatrixResult);
}
