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