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