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); }