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 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 void InitOfCovarianceMatrixes(SINS_State SINSstate, Kalman_Vars KalmanVars) { SINSprocessing.InitOfCovarianceMatrixes(SINSstate, KalmanVars); KalmanVars.CovarianceMatrixS_m[SINSstate.iMx_r12_odo * SimpleData.iMx + SINSstate.iMx_r12_odo] = KalmanVars.CovarianceMatrixS_p[SINSstate.iMx_r12_odo * SimpleData.iMx + SINSstate.iMx_r12_odo] = SINSstate.stdOdoR; KalmanVars.CovarianceMatrixS_m[(SINSstate.iMx_r12_odo + 1) * SimpleData.iMx + (SINSstate.iMx_r12_odo + 1)] = KalmanVars.CovarianceMatrixS_p[(SINSstate.iMx_r12_odo + 1) * SimpleData.iMx + (SINSstate.iMx_r12_odo + 1)] = SINSstate.stdOdoR; if (SINSstate.flag_Using_iMx_r_odo_3) { KalmanVars.CovarianceMatrixS_m[(SINSstate.iMx_r12_odo + 2) * SimpleData.iMx + (SINSstate.iMx_r12_odo + 2)] = KalmanVars.CovarianceMatrixS_p[(SINSstate.iMx_r12_odo + 2) * SimpleData.iMx + (SINSstate.iMx_r12_odo + 2)] = SINSstate.stdOdoR; } }
public static void MatrixNoise_ReDef(SINS_State SINSstate, Kalman_Vars KalmanVars, bool AlignmentFLG) { int iMx = SimpleData.iMx, iMq = SimpleData.iMq, iMx_r3_dV3 = SINSstate.iMx_r3_dV3, iMx_odo_model = SINSstate.iMx_odo_model, iMx_r12_odo = SINSstate.iMx_r12_odo; int tmpCounter = SINSprocessing.MatrixNoise_ReDef(SINSstate, KalmanVars, SINSstate.flag_Alignment); double sqrt_freq = Math.Sqrt(SINSstate.Freq); if (SINSstate.flag_iMqDeltaRodo) { KalmanVars.CovarianceMatrixNoise[(iMx_r12_odo + 0) * iMq + tmpCounter + 0] = KalmanVars.Noise_Pos * sqrt_freq; KalmanVars.CovarianceMatrixNoise[(iMx_r12_odo + 1) * iMq + tmpCounter + 1] = KalmanVars.Noise_Pos * sqrt_freq; if (SINSstate.flag_Using_iMx_r_odo_3) { KalmanVars.CovarianceMatrixNoise[(iMx_r12_odo + 2) * iMq + tmpCounter + 2] = KalmanVars.Noise_Pos * sqrt_freq; } } //SimpleOperations.PrintMatrixToFile(KalmanVars.CovarianceMatrixNoise, iMx, iMq); }
public static void StateCorrection(double[] ErrorVector, SINS_State SINSstate, SINS_State SINSstate_OdoMod, Kalman_Vars KalmanVars) { SINSstate.Latitude = SINSstate.Latitude - SINSstate.DeltaLatitude; SINSstate.Longitude = SINSstate.Longitude - SINSstate.DeltaLongitude; SINSstate.Vx_0[0] = SINSstate.Vx_0[0] - SINSstate.DeltaV_1; SINSstate.Vx_0[1] = SINSstate.Vx_0[1] - SINSstate.DeltaV_2; SINSstate.Roll = SINSstate.Roll - SINSstate.DeltaRoll; SINSstate.Pitch = SINSstate.Pitch - SINSstate.DeltaPitch; SINSstate.Heading = SINSstate.Heading - SINSstate.DeltaHeading; // --- Применение обновленных значений углов ориентации (переопределение матриц ориентации) SINSprocessing.ApplyCompensatedErrorsToSolution(SINSstate); //--- Ведем расчет оценки ошибок модели одометра в случае обратных связей ---// if (SINSstate.value_iMx_kappa_3_ds > 0) { SINSstate.Cumulative_KappaEst[2] += ErrorVector[SINSstate.value_iMx_kappa_3_ds + 0]; SINSstate.Cumulative_KappaEst[1] += ErrorVector[SINSstate.value_iMx_kappa_3_ds + 1]; } //--- Кумулируем ошибки вектора ошибок "x" для вывода в файлы ---// //--- Суммируются все компоненты вектора ошибок x, но используются только те компоненты, которые соответствуют датчикам и одометру for (int i = 0; i < SimpleData.iMx; i++) { SINSstate.Cumulative_KalmanErrorVector[i] += ErrorVector[i]; } //--- Кумулируем ошибки в большом для вывода в файлы, чисто для возможности визуализации---// SINSstate.Cumulative_StateErrorVector[0] += SINSstate.DeltaLatitude; SINSstate.Cumulative_StateErrorVector[1] += SINSstate.DeltaLongitude; SINSstate.Cumulative_StateErrorVector[2] += SINSstate.DeltaAltitude; SINSstate.Cumulative_StateErrorVector[3] += SINSstate.DeltaV_1; SINSstate.Cumulative_StateErrorVector[4] += SINSstate.DeltaV_2; SINSstate.Cumulative_StateErrorVector[5] += SINSstate.DeltaV_3; SINSstate.Cumulative_StateErrorVector[6] += SINSstate.DeltaHeading; SINSstate.Cumulative_StateErrorVector[7] += SINSstate.DeltaRoll; SINSstate.Cumulative_StateErrorVector[8] += SINSstate.DeltaPitch; //--- Поправки к одометрическому счисления --- SINSstate_OdoMod.Latitude = SINSstate_OdoMod.Latitude - SINSstate_OdoMod.DeltaLatitude; SINSstate_OdoMod.Longitude = SINSstate_OdoMod.Longitude - SINSstate_OdoMod.DeltaLongitude; SINSstate_OdoMod.A_x0n = A_x0n(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Longitude); SINSstate_OdoMod.A_nx0 = SINSstate_OdoMod.A_x0n.Transpose(); // ----------------------------------------------------------// // --------------Поправки к вертикальному каналу----------------// // ----------------------------------------------------------// SINSstate.Vx_0[2] = SINSstate.Vx_0[2] - SINSstate.DeltaV_3; SINSstate.Height = SINSstate.Height - SINSstate.DeltaAltitude; SINSstate_OdoMod.Height = SINSstate_OdoMod.Height - SINSstate_OdoMod.DeltaAltitude; for (int i = 0; i < SimpleData.iMx_Vertical; i++) { SINSstate.Vertical_Cumulative_KalmanErrorVector[i] += KalmanVars.Vertical_ErrorConditionVector_p[i]; } }
public static void Make_A(SINS_State SINSstate, Kalman_Vars KalmanVars, 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 ; SINSprocessing.Make_A(SINSstate, KalmanVars, SINSstate_OdoMod); /*-----------МОДИФИЦИРОВАННЫЙ СЛАБОСВЗАННЫЙ ВАРИАНТ----------------*/ if (SINSstate.flag_OdoSINSWeakConnect_MODIF) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + (iMx_alphaBeta + 2)] = SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 1] = SINSstate_OdoMod.Omega_x[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + (iMx_alphaBeta + 2)] = -SINSstate_OdoMod.Vx_0[0]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 0] = -SINSstate_OdoMod.Omega_x[2]; if (SINSstate.flag_iMx_r3_dV3) { if (SINSstate.existRelationHoriz_VS_Vertical) { KalmanVars.Matrix_A[(iMx_r12_odo + 0) * iMx + 0] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_e; KalmanVars.Matrix_A[(iMx_r12_odo + 0) * iMx + (iMx_alphaBeta + 1)] += -SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 0) * iMx + iMx_r_odo_3] = -SINSstate_OdoMod.Omega_x[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 1] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_n; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + (iMx_alphaBeta + 0)] += SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r_odo_3] = SINSstate_OdoMod.Omega_x[0]; } } if (SINSstate.flag_iMx_r3_dV3) { if (SINSstate.existRelationHoriz_VS_Vertical) { KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + 0] = -SINSstate_OdoMod.Vx_0[0] / SINSstate_OdoMod.R_e; KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + 1] = -SINSstate_OdoMod.Vx_0[1] / SINSstate_OdoMod.R_n; KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + iMx_r12_odo + 0] = SINSstate_OdoMod.Omega_x[1]; KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + iMx_r12_odo + 1] = -SINSstate_OdoMod.Omega_x[0]; KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + (iMx_alphaBeta + 0)] = -SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + (iMx_alphaBeta + 1)] = SINSstate_OdoMod.Vx_0[0]; } } } /*-----------СЛАБОСВЗАННЫЙ ВАРИАНТ----------------*/ else if (SINSstate.flag_OdoSINSWeakConnect) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + (iMx_alphaBeta + 2)] = SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + 0] = -SINSstate_OdoMod.Omega_x[0] * Math.Tan(SINSstate_OdoMod.Latitude); KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 0] = SINSstate_OdoMod.Omega_x[0] * Math.Tan(SINSstate_OdoMod.Latitude); KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 1] = SINSstate_OdoMod.Omega_x[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + (iMx_alphaBeta + 2)] = -SINSstate_OdoMod.Vx_0[0]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 0] = -SINSstate_OdoMod.Omega_x[2]; if (SINSstate.flag_iMx_r3_dV3) { if (SINSstate.existRelationHoriz_VS_Vertical) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + (iMx_alphaBeta + 1)] += -SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 0] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_e; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r_odo_3 + 0] = -SINSstate_OdoMod.Omega_x[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + (iMx_alphaBeta + 1)] += SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 1] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_n; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r_odo_3 + 0] = SINSstate_OdoMod.Omega_x[0]; } } if (SINSstate.flag_iMx_r3_dV3) { if (SINSstate.existRelationHoriz_VS_Vertical) { KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + (iMx_alphaBeta + 0)] = -SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + (iMx_alphaBeta + 1)] = SINSstate_OdoMod.Vx_0[0]; } } } if (SINSstate.flag_iMx_kappa_13_ds) { if (SINSstate.existRelationHoriz_VS_Vertical || !SINSstate.flag_iMx_r3_dV3) { if (iMx_kappa_1 > 0) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_kappa_1 + 0] = SINSstate_OdoMod.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[0, 2] - SINSstate_OdoMod.OdoSpeed_s[2] * SINSstate_OdoMod.A_x0s[0, 1]; } } KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_kappa_3_ds + 0] = -SINSstate_OdoMod.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[0, 0] + SINSstate_OdoMod.OdoSpeed_s[0] * SINSstate_OdoMod.A_x0s[0, 1]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_kappa_3_ds + 1] = SINSstate_OdoMod.OdoSpeed_x0[0]; if (SINSstate.existRelationHoriz_VS_Vertical || !SINSstate.flag_iMx_r3_dV3) { if (iMx_kappa_1 > 0) { KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_kappa_1 + 0] = SINSstate_OdoMod.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[1, 2] - SINSstate_OdoMod.OdoSpeed_s[2] * SINSstate_OdoMod.A_x0s[1, 1]; } } KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_kappa_3_ds + 0] = -SINSstate_OdoMod.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[1, 0] + SINSstate_OdoMod.OdoSpeed_s[0] * SINSstate_OdoMod.A_x0s[1, 1]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_kappa_3_ds + 1] = SINSstate_OdoMod.OdoSpeed_x0[1]; if (SINSstate.flag_iMx_r3_dV3) { if (iMx_kappa_1 > 0) { KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + iMx_kappa_1 + 0] = SINSstate_OdoMod.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[2, 2] - SINSstate_OdoMod.OdoSpeed_s[2] * SINSstate_OdoMod.A_x0s[2, 1]; } if (SINSstate.existRelationHoriz_VS_Vertical || !SINSstate.flag_iMx_r3_dV3) { KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + iMx_kappa_3_ds + 0] = -SINSstate_OdoMod.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[2, 0] + SINSstate_OdoMod.OdoSpeed_s[0] * SINSstate_OdoMod.A_x0s[2, 1]; KalmanVars.Matrix_A[(iMx_r_odo_3) * iMx + iMx_kappa_3_ds + 1] = SINSstate_OdoMod.OdoSpeed_x0[2]; } } } // ----------------------------------------------------------// if (SINSstate.flag_SeparateHorizVSVertical == true) { if (SINSstate.Vertical_kappa1 > 0) { KalmanVars.Vertical_Matrix_A[SINSstate.Vertical_rOdo3 * SimpleData.iMx_Vertical + SINSstate.Vertical_kappa1 + 0] = SINSstate_OdoMod.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[2, 2] - SINSstate_OdoMod.OdoSpeed_s[2] * SINSstate_OdoMod.A_x0s[2, 1]; if (SINSstate.Vertical_kappa3Scale > 0) { KalmanVars.Vertical_Matrix_A[SINSstate.Vertical_rOdo3 * SimpleData.iMx_Vertical + SINSstate.Vertical_kappa3Scale + 0] = -SINSstate_OdoMod.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[2, 0] + SINSstate_OdoMod.OdoSpeed_s[0] * SINSstate_OdoMod.A_x0s[2, 1]; KalmanVars.Vertical_Matrix_A[SINSstate.Vertical_rOdo3 * SimpleData.iMx_Vertical + SINSstate.Vertical_kappa3Scale + 1] = SINSstate_OdoMod.OdoSpeed_x0[2]; } } } // ----------------------------------------------------------// }
public static void Make_A(SINS_State SINSstate, Kalman_Vars KalmanVars, 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; SINSprocessing.Make_A(SINSstate, KalmanVars, SINSstate_OdoMod); /*-----------МОДИФИЦИРОВАННЫЙ СЛАБОСВЗАННЫЙ ВАРИАНТ----------------*/ if (SINSstate.flag_OdoSINSWeakConnect_MODIF) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + 6] = SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 1] = SINSstate_OdoMod.Omega_x[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 6] = -SINSstate_OdoMod.Vx_0[0]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 0] = -SINSstate_OdoMod.Omega_x[2]; if (SINSstate.flag_iMx_r3_dV3) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + 0] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_e; KalmanVars.Matrix_A[iMx_r12_odo * iMx + 5] += -SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 2] = -SINSstate_OdoMod.Omega_x[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 1] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_n; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 4] += SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 2] = SINSstate_OdoMod.Omega_x[0]; } if (SINSstate.flag_Using_iMx_r_odo_3) { KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 0] = -SINSstate_OdoMod.Vx_0[0] / SINSstate_OdoMod.R_e; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 1] = -SINSstate_OdoMod.Vx_0[1] / SINSstate_OdoMod.R_n; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 4] = -SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 5] = SINSstate_OdoMod.Vx_0[0]; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_r12_odo + 0] = SINSstate_OdoMod.Omega_x[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_r12_odo + 1] = -SINSstate_OdoMod.Omega_x[0]; } } /*-----------СЛАБОСВЗАННЫЙ ВАРИАНТ----------------*/ else if (SINSstate.flag_OdoSINSWeakConnect) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + 6] = SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + 0] = -SINSstate_OdoMod.Omega_x[0] * Math.Tan(SINSstate_OdoMod.Latitude); KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 0] = SINSstate_OdoMod.Omega_x[0] * Math.Tan(SINSstate_OdoMod.Latitude); KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 1] = SINSstate_OdoMod.Omega_x[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 6] = -SINSstate_OdoMod.Vx_0[0]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 0] = -SINSstate_OdoMod.Omega_x[2]; if (SINSstate.flag_iMx_r3_dV3) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + 5] += -SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 0] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_e; KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 2] = -SINSstate_OdoMod.Omega_x[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 4] += SINSstate_OdoMod.Vx_0[2]; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 1] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_n; KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 2] = SINSstate_OdoMod.Omega_x[0]; } if (SINSstate.flag_Using_iMx_r_odo_3) { KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 4] = -SINSstate_OdoMod.Vx_0[1]; KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 5] = SINSstate_OdoMod.Vx_0[0]; } } if (SINSstate.flag_iMx_kappa_13_ds) { KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_odo_model + 0] = -SINSstate_OdoMod.A_x0s[0, 2] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_odo_model + 1] = SINSstate_OdoMod.A_x0s[0, 0] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_odo_model + 2] = SINSstate_OdoMod.A_x0s[0, 1] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_odo_model + 0] = -SINSstate_OdoMod.A_x0s[1, 2] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_odo_model + 1] = SINSstate_OdoMod.A_x0s[1, 0] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_odo_model + 2] = SINSstate_OdoMod.A_x0s[1, 1] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); if (SINSstate.flag_Using_iMx_r_odo_3) { KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_odo_model + 0] = -SINSstate_OdoMod.A_x0s[2, 2] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_odo_model + 1] = SINSstate_OdoMod.A_x0s[2, 0] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_odo_model + 2] = SINSstate_OdoMod.A_x0s[2, 1] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0); } } //if (SINSstate.Count % 1000 == 0) //{ // SimpleOperations.PrintMatrixToFile(KalmanVars.Matrix_A, SimpleData.iMx, SimpleData.iMx); // SimpleOperations.PrintMatrixToFile(KalmanVars.CovarianceMatrixS_m, SimpleData.iMx, SimpleData.iMx); //} }