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 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 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 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 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 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 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 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 double Sigmf_Disp(int numb_i, Kalman_Vars KalmanVars)
{
double _out = 0.0;
unsafe
{
fixed(double *_s = KalmanVars.CovarianceMatrixS_m)
{
_out = sigmbi(_s, numb_i, SimpleData.iMx);
}
}
return(_out);
}
//--------------------------------------------------------------------------
//-------------------------------СПУТНИК---------------------------------------
//--------------------------------------------------------------------------
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 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 NullingOfCorrectedErrors(SINS_State SINSstate, Kalman_Vars KalmanVars)
{
for (int i = 0; i < SimpleData.iMx; i++)
{
KalmanVars.ErrorConditionVector_p[i] = 0.0;
KalmanVars.ErrorConditionVector_m[i] = 0.0;
}
// ----------------------------------------------------------//
// -------------Для вертикального канала----------------------//
for (int i = 0; i < SimpleData.iMx_Vertical; i++)
{
KalmanVars.Vertical_ErrorConditionVector_p[i] = 0.0;
KalmanVars.Vertical_ErrorConditionVector_m[i] = 0.0;
}
}
public static void Make_Vertical_H_KNS(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod)
{
KalmanVars.Vertical_Matrix_H[(KalmanVars.Vertical_cnt_measures + 0) * SimpleData.iMx_Vertical + 1] = 1.0;
KalmanVars.Vertical_Measure[KalmanVars.Vertical_cnt_measures] = SINSstate.Vx_0[2];
KalmanVars.Vertical_Noize_Z[(KalmanVars.Vertical_cnt_measures + 0)] = KalmanVars.OdoNoise_STOP;
KalmanVars.Vertical_cnt_measures += 1;
// --- Корректируем также по нулевой разнице между высотами БИНС и одометра --- //
{
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)] = 0;
KalmanVars.Vertical_Noize_Z[(KalmanVars.Vertical_cnt_measures + 0)] = KalmanVars.OdoNoise_Dist / 10.0;
KalmanVars.Vertical_cnt_measures += 1;
}
}
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------Коррекция дрейфа на стоянке-----------------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
public static void Make_H_DRIFTS(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 = 0.00001 * SimpleData.ToRadian;
//---Разбиение на три составляющие---
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_Nu0 + 0] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + iMx_Nu0 + 1] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 2) * iMx + iMx_Nu0 + 2] = 1.0;
SimpleOperations.CopyArray(SINSstate.u_s, SINSstate.A_sx0 * SINSstate.u_x);
double[] W_z_avg = new double[3];
W_z_avg[0] = SINSstate.forDriftMeasureWsAvg[0] / SINSstate.forDriftMeasureWsAvg_cnt;
W_z_avg[1] = SINSstate.forDriftMeasureWsAvg[1] / SINSstate.forDriftMeasureWsAvg_cnt;
W_z_avg[2] = SINSstate.forDriftMeasureWsAvg[2] / SINSstate.forDriftMeasureWsAvg_cnt;
// --- Формирование измерений по разности координат БИНС и одометрического счисления
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = W_z_avg[0] - SINSstate.u_s[0];
KalmanVars.Measure[(KalmanVars.cnt_measures + 1)] = W_z_avg[1] - SINSstate.u_s[1];
KalmanVars.Measure[(KalmanVars.cnt_measures + 2)] = W_z_avg[2] - SINSstate.u_s[2];
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = Noize;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = Noize;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 2)] = Noize;
KalmanVars.cnt_measures += 3;
SINSstate.flag_UsingCorrection = true;
}
public static void Make_F(double timeStep, Kalman_Vars KalmanVars, SINS_State SINSstate)
{
unsafe
{
fixed(double *_a = KalmanVars.Matrix_A, _f = KalmanVars.TransitionMatrixF)
{
func(timeStep, _a, _f, SimpleData.iMx);
}
}
// ----------------------------------------------------------//
unsafe
{
fixed(double *_a = KalmanVars.Vertical_Matrix_A, _f = KalmanVars.Vertical_TransitionMatrixF)
{
func(timeStep, _a, _f, SimpleData.iMx_Vertical);
}
}
}
public static void Make_H_VELOCITY_Mz13(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[] Vz = new double[3];
SimpleOperations.CopyArray(Vz, SINSstate.A_sx0 * SINSstate.Vx_0);
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = Vz[0];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + (iMx_dV_12 + 0)] = SINSstate.A_sx0[0, 0];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + (iMx_dV_12 + 1)] = SINSstate.A_sx0[0, 1];
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 0.01;
KalmanVars.cnt_measures += 1;
if (SINSstate.flag_iMx_r3_dV3)
{
KalmanVars.Matrix_H[(KalmanVars.cnt_measures - 1) * iMx + value_iMx_dV3] = SINSstate.A_sx0[0, 2];
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = Vz[2];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + (iMx_dV_12 + 0)] = SINSstate.A_sx0[2, 0];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + (iMx_dV_12 + 1)] = SINSstate.A_sx0[2, 1];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + value_iMx_dV3] = SINSstate.A_sx0[2, 2];
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 0.01;
KalmanVars.cnt_measures += 1;
}
}
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);
}
//--------------------------------------------------------------------------
//--------------------------ПОЗИЦИОННАЯ КОРЕКЦИЯ CHECKPOINTS-----------------------------
//--------------------------------------------------------------------------
public static void Make_H_CONTROLPOINTS(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod, double Latitude_CP, double Longitude_CP, double Altitude_CP, double NoiseValue)
{
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];
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 0] = 1.0;
KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 1) * iMx + 1] = 1.0;
//Формирование измерений по географическим координатам
KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = (SINSstate.Longitude - Longitude_CP) * SINSstate.R_e * Math.Cos(SINSstate.Latitude);
KalmanVars.Measure[(KalmanVars.cnt_measures + 1)] = (SINSstate.Latitude - Latitude_CP) * SINSstate.R_n;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = NoiseValue;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = NoiseValue;
KalmanVars.cnt_measures += 2;
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 - Altitude_CP;
KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = NoiseValue;
KalmanVars.cnt_measures += 1;
}
SINSstate.flag_UsingCorrection = true;
}
//--------------------------------------------------------------------------
//-------------------------------КНС---------------------------------------
//--------------------------------------------------------------------------
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_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
;
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;
// ----------------------------------------------------------//
// --------Измерение для коррекции вертикального канала-------------//
// ----------------------------------------------------------//
{
KalmanVars.Vertical_Matrix_H[(KalmanVars.Vertical_cnt_measures + 0) * SimpleData.iMx_Vertical + 1] = 1.0;
KalmanVars.Vertical_Measure[(KalmanVars.Vertical_cnt_measures + 0)] = SINSstate.Vx_0[2];
KalmanVars.Vertical_Noize_Z[(KalmanVars.Vertical_cnt_measures + 0)] = KalmanVars.OdoNoise_STOP;
KalmanVars.Vertical_cnt_measures += 1;
}
SINSstate.flag_UsingCorrection = true;
}
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 KalmanCorrection(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod)
{
SimpleOperations.NullingOfArray(KalmanVars.KalmanFactor);
SimpleOperations.NullingOfArray(KalmanVars.StringOfMeasure);
// --- Запускается коррекция по циклу по одной строке из матрицы H
unsafe
{
fixed(double *_xm = KalmanVars.ErrorConditionVector_m, _xp = KalmanVars.ErrorConditionVector_p, _sm = KalmanVars.CovarianceMatrixS_m, _sp = KalmanVars.CovarianceMatrixS_p, _kf = KalmanVars.KalmanFactor)
{
for (int t = 0; t < KalmanVars.cnt_measures; t++)
{
for (int i = 0; i < SimpleData.iMx; i++)
{
KalmanVars.StringOfMeasure[i] = KalmanVars.Matrix_H[t * SimpleData.iMx + i];
fixed(double *_h = KalmanVars.StringOfMeasure)
{
// --- Коррекция по измерениям
f0b(KalmanVars.Measure[t], _xm, _sm, _h, KalmanVars.Noize_Z[t] * KalmanVars.Noize_Z[t], _xp, _sp, _kf, SimpleData.iMx);
if (t < KalmanVars.cnt_measures - 1)
{
for (int i = 0; i < SimpleData.iMx; i++)
{
_xm[i] = _xp[i];
for (int j = 0; j < SimpleData.iMx; j++)
{
_sm[i * SimpleData.iMx + j] = _sp[i * SimpleData.iMx + j];
}
}
}
}
}
}
}
// ----------------------------------------------------------//
// --------------Шаг коррекции для вертикального фильтра-----------------//
// ----------------------------------------------------------//
SimpleOperations.NullingOfArray(KalmanVars.KalmanFactor);
SimpleOperations.NullingOfArray(KalmanVars.StringOfMeasure);
unsafe
{
fixed(double *_xm = KalmanVars.Vertical_ErrorConditionVector_m, _xp = KalmanVars.Vertical_ErrorConditionVector_p, _sm = KalmanVars.Vertical_CovarianceMatrixS_m,
_sp = KalmanVars.Vertical_CovarianceMatrixS_p, _kf = KalmanVars.KalmanFactor)
{
for (int t = 0; t < KalmanVars.Vertical_cnt_measures; t++)
{
for (int i = 0; i < SimpleData.iMx_Vertical; i++)
{
KalmanVars.StringOfMeasure[i] = KalmanVars.Vertical_Matrix_H[t * SimpleData.iMx_Vertical + i];
fixed(double *_h = KalmanVars.StringOfMeasure)
{
//Коррекция по измерениям
f0b(KalmanVars.Vertical_Measure[t], _xm, _sm, _h, KalmanVars.Vertical_Noize_Z[t] * KalmanVars.Vertical_Noize_Z[t], _xp, _sp, _kf, SimpleData.iMx_Vertical);
if (t < KalmanVars.Vertical_cnt_measures - 1)
{
for (int i = 0; i < SimpleData.iMx_Vertical; i++)
{
_xm[i] = _xp[i];
for (int j = 0; j < SimpleData.iMx_Vertical; j++)
{
_sm[i * SimpleData.iMx_Vertical + j] = _sp[i * SimpleData.iMx_Vertical + j];
}
}
}
}
}
}
}
}
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_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_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
;
for (int i = 0; i < iMx * iMx; i++)
{
KalmanVars.Matrix_A[i] = 0;
}
SINSstate.W_x[0] = SINSstate.Omega_x[0];
SINSstate.W_x[1] = SINSstate.Omega_x[1] + SimpleData.U * Math.Cos(SINSstate.Latitude);
SINSstate.W_x[2] = SINSstate.Omega_x[2] + SimpleData.U * Math.Sin(SINSstate.Latitude);
/*----------- Далее компоненты для матрицы части БИНС в горизонтальном канале ----------------*/
// --- блок по позиционным ошибкам БИНС
KalmanVars.Matrix_A[0 * iMx + 1] = SINSstate.Omega_x[2];
KalmanVars.Matrix_A[0 * iMx + (iMx_dV_12 + 0)] = 1.0;
KalmanVars.Matrix_A[0 * iMx + (iMx_alphaBeta + 2)] = SINSstate.Vx_0[1];
KalmanVars.Matrix_A[1 * iMx + 0] = -SINSstate.Omega_x[2];
KalmanVars.Matrix_A[1 * iMx + (iMx_dV_12 + 1)] = 1.0;
KalmanVars.Matrix_A[1 * iMx + (iMx_alphaBeta + 2)] = -SINSstate.Vx_0[0];
// --- блок по скоростным ошибкам
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + 1] = SINSstate.u_x[1] * SINSstate.Vx_0[1] / SINSstate.R_n;
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (iMx_dV_12 + 1)] = SINSstate.Omega_x[2] + 2 * SINSstate.u_x[2];
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (iMx_alphaBeta + 0)] = SINSstate.u_x[1] * SINSstate.Vx_0[1];
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (iMx_alphaBeta + 1)] = -SINSstate.g;
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (iMx_Nu0 + 0)] = -SINSstate.Vx_0[1] * SINSstate.A_x0s[2, 0];
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (iMx_Nu0 + 1)] = -SINSstate.Vx_0[1] * SINSstate.A_x0s[2, 1];
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (f0_12 + 0)] = SINSstate.A_x0s[0, 0];
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (f0_12 + 1)] = SINSstate.A_x0s[0, 1];
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (f0_3 + 0)] = SINSstate.A_x0s[0, 2];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + 1] = -SINSstate.u_x[1] * SINSstate.Vx_0[0] / SINSstate.R_n;
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (iMx_dV_12 + 0)] = -SINSstate.Omega_x[2] - 2 * SINSstate.u_x[2];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (iMx_alphaBeta + 0)] = -SINSstate.u_x[1] * SINSstate.Vx_0[0] + SINSstate.g;
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (iMx_Nu0 + 0)] = SINSstate.Vx_0[0] * SINSstate.A_x0s[2, 0];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (iMx_Nu0 + 1)] = SINSstate.Vx_0[0] * SINSstate.A_x0s[2, 1];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (f0_12 + 0)] = SINSstate.A_x0s[1, 0];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (f0_12 + 1)] = SINSstate.A_x0s[1, 1];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (f0_3 + 0)] = SINSstate.A_x0s[1, 2];
// --- блок по угловым ошибкам ориентации
KalmanVars.Matrix_A[(iMx_alphaBeta + 0) * iMx + 0] = -SINSstate.u_x[2] / SINSstate.R_e;
KalmanVars.Matrix_A[(iMx_alphaBeta + 0) * iMx + (iMx_dV_12 + 1)] = -1.0 / SINSstate.R_n;
KalmanVars.Matrix_A[(iMx_alphaBeta + 0) * iMx + (iMx_alphaBeta + 1)] = SINSstate.u_x[2];
KalmanVars.Matrix_A[(iMx_alphaBeta + 0) * iMx + (iMx_alphaBeta + 2)] = -SINSstate.u_x[1];
KalmanVars.Matrix_A[(iMx_alphaBeta + 0) * iMx + (iMx_Nu0 + 0)] = -SINSstate.A_x0s[0, 0];
KalmanVars.Matrix_A[(iMx_alphaBeta + 0) * iMx + (iMx_Nu0 + 1)] = -SINSstate.A_x0s[0, 1];
KalmanVars.Matrix_A[(iMx_alphaBeta + 0) * iMx + (iMx_Nu0 + 2)] = -SINSstate.A_x0s[0, 2];
KalmanVars.Matrix_A[(iMx_alphaBeta + 1) * iMx + 1] = -SINSstate.u_x[2] / SINSstate.R_n;
KalmanVars.Matrix_A[(iMx_alphaBeta + 1) * iMx + (iMx_dV_12 + 0)] = 1.0 / SINSstate.R_e;
KalmanVars.Matrix_A[(iMx_alphaBeta + 1) * iMx + (iMx_alphaBeta + 0)] = -SINSstate.u_x[2];
KalmanVars.Matrix_A[(iMx_alphaBeta + 1) * iMx + (iMx_Nu0 + 0)] = -SINSstate.A_x0s[1, 0];
KalmanVars.Matrix_A[(iMx_alphaBeta + 1) * iMx + (iMx_Nu0 + 1)] = -SINSstate.A_x0s[1, 1];
KalmanVars.Matrix_A[(iMx_alphaBeta + 1) * iMx + (iMx_Nu0 + 2)] = -SINSstate.A_x0s[1, 2];
KalmanVars.Matrix_A[(iMx_alphaBeta + 2) * iMx + 0] = SINSstate.Omega_x[0] / SINSstate.R_e;
KalmanVars.Matrix_A[(iMx_alphaBeta + 2) * iMx + 1] = (SINSstate.Omega_x[1] + SINSstate.u_x[1]) / SINSstate.R_n;
KalmanVars.Matrix_A[(iMx_alphaBeta + 2) * iMx + (iMx_alphaBeta + 0)] = SINSstate.Omega_x[1] + SINSstate.u_x[1];
KalmanVars.Matrix_A[(iMx_alphaBeta + 2) * iMx + (iMx_alphaBeta + 1)] = -SINSstate.Omega_x[0];
KalmanVars.Matrix_A[(iMx_alphaBeta + 2) * iMx + (iMx_Nu0 + 0)] = -SINSstate.A_x0s[2, 0];
KalmanVars.Matrix_A[(iMx_alphaBeta + 2) * iMx + (iMx_Nu0 + 1)] = -SINSstate.A_x0s[2, 1];
KalmanVars.Matrix_A[(iMx_alphaBeta + 2) * iMx + (iMx_Nu0 + 2)] = -SINSstate.A_x0s[2, 2];
/*-----------Компоненты при ошибках одометрического счисления----------------*/
// --- блок по горизонтальным ошибкам одометрического счисления
KalmanVars.Matrix_A[(iMx_r12_odo + 0) * iMx + (iMx_alphaBeta + 2)] = SINSstate_OdoMod.Vx_0[1];
KalmanVars.Matrix_A[(iMx_r12_odo + 0) * iMx + iMx_r12_odo + 1] = SINSstate_OdoMod.Omega_x[2];
if (iMx_kappa_3_ds > 0)
{
KalmanVars.Matrix_A[(iMx_r12_odo + 0) * 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 + 0) * iMx + iMx_kappa_3_ds + 1] = SINSstate_OdoMod.OdoSpeed_x0[0];
}
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 (iMx_kappa_3_ds > 0)
{
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];
}
// ----------------------------------------------------------//
// ----------------ВЕРТИКАЛЬНЫЙ КАНАЛ ОТДЕЛЬНО----------------------//
// ----------------------------------------------------------//
int iMxV = SimpleData.iMx_Vertical, Vertical_rOdo3 = SINSstate.Vertical_rOdo3, Vertical_kappa1 = SINSstate.Vertical_kappa1;
SimpleOperations.NullingOfArray(KalmanVars.Vertical_Matrix_A);
KalmanVars.Vertical_Matrix_A[0 * iMxV + 1] = 1.0;
KalmanVars.Vertical_Matrix_A[1 * iMxV + 0] = 2 * 0.000001538;
KalmanVars.Vertical_Matrix_A[1 * iMxV + SINSstate.Vertical_f0_3] = SINSstate.A_x0s[2, 2];
if (Vertical_kappa1 > 0)
{
KalmanVars.Vertical_Matrix_A[Vertical_rOdo3 * iMxV + Vertical_kappa1]
= SINSstate_OdoMod.OdoSpeed_s[1] * SINSstate_OdoMod.A_x0s[2, 2] - SINSstate_OdoMod.OdoSpeed_s[2] * SINSstate_OdoMod.A_x0s[2, 1];
}
// ----------------------------------------------------------//
// --- Дополняем горизонтальный канал компонентами, при которых стоят либо высота, либо вертикальная скорость --- //
if (true)
{
KalmanVars.Matrix_A[0 * iMx + 0] += SINSstate.Vx_0[2] / SINSstate.R_e;
KalmanVars.Matrix_A[0 * iMx + (iMx_alphaBeta + 1)] += -SINSstate.Vx_0[2];
KalmanVars.Matrix_A[1 * iMx + 1] += SINSstate.Vx_0[2] / SINSstate.R_n;
KalmanVars.Matrix_A[1 * iMx + (iMx_alphaBeta + 0)] += SINSstate.Vx_0[2];
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + 1] += SINSstate.u_x[2] * SINSstate.Vx_0[2] / SINSstate.R_n;
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (iMx_alphaBeta + 0)] += SINSstate.u_x[2] * SINSstate.Vx_0[2];
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (iMx_Nu0 + 0)] += SINSstate.Vx_0[2] * SINSstate.A_x0s[1, 0];
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (iMx_Nu0 + 1)] += SINSstate.Vx_0[2] * SINSstate.A_x0s[1, 1];
KalmanVars.Matrix_A[(iMx_dV_12 + 0) * iMx + (iMx_Nu0 + 2)] += SINSstate.Vx_0[2] * SINSstate.A_x0s[1, 2];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + 0] += -SINSstate.u_x[2] * SINSstate.Vx_0[2] / SINSstate.R_e;
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (iMx_alphaBeta + 1)] += SINSstate.u_x[2] * SINSstate.Vx_0[2];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (iMx_alphaBeta + 2)] += -SINSstate.u_x[1] * SINSstate.Vx_0[2];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (iMx_Nu0 + 0)] += -SINSstate.Vx_0[2] * SINSstate.A_x0s[0, 0];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (iMx_Nu0 + 1)] += -SINSstate.Vx_0[2] * SINSstate.A_x0s[0, 1];
KalmanVars.Matrix_A[(iMx_dV_12 + 1) * iMx + (iMx_Nu0 + 2)] += -SINSstate.Vx_0[2] * SINSstate.A_x0s[0, 2];
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 + 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];
}
}
public static void StateIntegration_AT(SINS_State SINSstate, Kalman_Vars KalmanVars, SINS_State SINSstate_OdoMod)
{
double[] fz = new double[3], Wz = new double[3], u = new double[3], tempV = new double[3], Wz_avg = new double[3];
double[] Vx_0 = new double[3], Vx_0_prev = new double[3];
Matrix AT_z_xi = new Matrix(3, 3); Matrix B_x_eta = new Matrix(3, 3);
Matrix dAT = new Matrix(3, 3); Matrix D_x_z = new Matrix(3, 3);
Matrix W_x_xi = new Matrix(3, 3); Matrix C_eta_xi = new Matrix(3, 3);
Matrix Hat1 = new Matrix(3, 3);
Matrix Hat2 = new Matrix(3, 3);
Matrix E = Matrix.UnitMatrix(3);
Matrix dMatrix = new Matrix(3, 3);
double W_z_abs, Omega_x_abs, dlt, dlt2, Altitude, Altitude_prev, dh, dVx, dVy, dVh, Azimth;
CopyMatrix(AT_z_xi, SINSstate.AT);
CopyMatrix(B_x_eta, SINSstate.A_x0n);
SINSstate.A_nxi = SimpleOperations.A_ne(SINSstate.Time, SINSstate.Longitude_Start);
Altitude = SINSstate.Height;
Altitude_prev = SINSstate.Altitude_prev;
fz[1] = SINSstate.F_z[1];
fz[2] = SINSstate.F_z[2];
fz[0] = SINSstate.F_z[0];
Wz[1] = SINSstate.W_z[1];
Wz[2] = SINSstate.W_z[2];
Wz[0] = SINSstate.W_z[0];
// --- в обратной связи используется накопленные оценки ошибок ньютонометров в горизонте
fz[0] -= SINSstate.Cumulative_KalmanErrorVector[(SINSstate.value_iMx_f0_12 + 0)];
fz[1] -= SINSstate.Cumulative_KalmanErrorVector[(SINSstate.value_iMx_f0_12 + 1)];
// --- в обратной связи используется накопленные оценка ошибки ньютонометра f_3 в вертикальном канале
fz[2] -= SINSstate.Vertical_Cumulative_KalmanErrorVector[SINSstate.Vertical_f0_3];
// --- то же самое для ДУС, только все с горизонтального канала
for (int i = 0; i < 3; i++)
{
Wz[i] += SINSstate.Cumulative_KalmanErrorVector[SINSstate.value_iMx_Nu0 + i];
}
// --- вычитание из показаний датчиков значений нулей, полученных алгебраической выставкой на начальной выставке
for (int i = 0; i < 3; i++)
{
fz[i] = fz[i] - SINSstate.AlignAlgebraZeroF[i];
}
for (int i = 0; i < 3; i++)
{
Wz[i] = Wz[i] + SINSstate.AlignAlgebraDrifts[i];
}
CopyArray(SINSstate.F_z, fz);
CopyArray(SINSstate.W_z, Wz);
CopyArray(Vx_0, SINSstate.Vx_0);
CopyArray(Vx_0_prev, SINSstate.Vx_0_prev);
SINSstate.R_e = RadiusE(SINSstate.Latitude, SINSstate.Height);
SINSstate.R_n = RadiusN(SINSstate.Latitude, SINSstate.Height);
SINSstate.u_x = U_x0(SINSstate.Latitude);
u[0] = 0.0;
u[1] = SimpleData.U * Math.Cos(SINSstate.Latitude);
u[2] = SimpleData.U * Math.Sin(SINSstate.Latitude);
//-------------ИНТЕГРИРОВАНИЕ МАТРИЦЫ AT_Z_XI И ПЕРВОЕ ВЫЧИСЛЕНИЕ МАТРИЦЫ D_X_Z---------
for (int i = 0; i < 3; i++)
{
fz[i] = (fz[i] + SINSstate.F_z_prev[i]) / 2.0;
Wz[i] = (Wz[i] + SINSstate.W_z_prev[i]) / 2.0;
}
W_z_abs = Math.Sqrt(Wz[0] * Wz[0] + Wz[1] * Wz[1] + Wz[2] * Wz[2]);
dlt = Math.Sin(W_z_abs * SINSstate.timeStep) / W_z_abs;
dlt2 = (1.0 - Math.Cos(W_z_abs * SINSstate.timeStep)) / (W_z_abs * W_z_abs);
Hat1 = Matrix.SkewSymmetricMatrix(Wz);
Hat2 = Matrix.SkewSymmetricMatrixSquare(Wz);
// --- Интегрирование матрицы ориентации AT_z_xi приборного относительно инерциального трехгранника
CopyMatrix(dMatrix, (E + Hat1 * dlt + Hat2 * dlt2));
CopyMatrix(AT_z_xi, (dMatrix * AT_z_xi));
//Нормировка
for (int i = 0; i < 3; i++)
{
tempV[i] = Math.Sqrt(AT_z_xi[i, 0] * AT_z_xi[i, 0] + AT_z_xi[i, 1] * AT_z_xi[i, 1] + AT_z_xi[i, 2] * AT_z_xi[i, 2]);
for (int j = 0; j < 3; j++)
{
AT_z_xi[i, j] = AT_z_xi[i, j] / tempV[i];
}
}
CopyMatrix(SINSstate.AT, AT_z_xi);
// --- Вычисление матрицы ориентации D_x_z приборного относительно географии
CopyMatrix(W_x_xi, B_x_eta * SINSstate.A_nxi);
CopyMatrix(D_x_z, W_x_xi * SINSstate.AT.Transpose());
//--------------------------------------------------------------------------------------
//---------------------------------ИНТЕГРИРОВАНИЕ СКОРОСТЕЙ----------------------------
CopyArray(SINSstate.F_x, D_x_z * fz);
SINSstate.g = SimpleData.Gravity_Normal * (1.0 + 0.0053020 * Math.Pow(Math.Sin(SINSstate.Latitude), 2) - 0.000007 * Math.Pow(Math.Sin(2 * SINSstate.Latitude), 2)) - 0.00014 - 2 * 0.000001538 * Altitude;
dVx = SINSstate.F_x[0] + Vx_0[1] * (2.0 * u[2] + SINSstate.Omega_x[2]) - Vx_0[2] * (2.0 * u[1] + SINSstate.Omega_x[1]);
dVy = SINSstate.F_x[1] - Vx_0[0] * (2.0 * u[2] + SINSstate.Omega_x[2]) + Vx_0[2] * (2.0 * u[0] + SINSstate.Omega_x[0]);
Vx_0[0] += dVx * SINSstate.timeStep;
Vx_0[1] += dVy * SINSstate.timeStep;
if (SimpleOperations.AbsoluteVectorValue(Vx_0) > 0.1)
{
double[] Vs = new double[3], Vx0 = new double[3];
Vx0[0] = Vx_0[0];
Vx0[1] = Vx_0[1];
SimpleOperations.CopyArray(Vs, SINSstate.A_sx0 * Vx0);
SINSstate.distance_by_SINS += Vs[1] * SINSstate.timeStep;
}
//--------------------------------------------------------------------------------------
//--- Интегрируем вертикальную скорость ---//
if (SINSstate.flag_AutonomouseSolution == false)
{
dVh = SINSstate.F_x[2] - SINSstate.g + (Vx_0[0] + Vx_0_prev[0]) / 2.0 * (2 * u[1] + SINSstate.Omega_x[1]) - (Vx_0[1] + Vx_0_prev[1]) / 2.0 * (2 * u[0] + SINSstate.Omega_x[0]);
Vx_0[2] += dVh * SINSstate.timeStep;
dh = (Vx_0[2] + Vx_0_prev[2]) / 2.0;
Altitude += dh * SINSstate.timeStep;
}
//---------ИНТЕГРИРОВАНИЕ МАТРИЦЫ B_X_ETA И ВТОРОЕ ВЫЧИСЛЕНИЕ МАТРИЦЫ D_X_Z--------------
SINSstate.Omega_x[0] = -(Vx_0[1] + Vx_0_prev[1]) / 2.0 / SINSstate.R_n;
SINSstate.Omega_x[1] = (Vx_0[0] + Vx_0_prev[0]) / 2.0 / SINSstate.R_e;
SINSstate.Omega_x[2] = Math.Tan(SINSstate.Latitude) * SINSstate.Omega_x[1];
Omega_x_abs = Math.Sqrt(SINSstate.Omega_x[0] * SINSstate.Omega_x[0] + SINSstate.Omega_x[1] * SINSstate.Omega_x[1] + SINSstate.Omega_x[2] * SINSstate.Omega_x[2]);
if (Omega_x_abs != 0)
{
dlt = Math.Sin(Omega_x_abs * SINSstate.timeStep) / Omega_x_abs;
dlt2 = (1.0 - Math.Cos(Omega_x_abs * SINSstate.timeStep)) / (Omega_x_abs * Omega_x_abs);
}
else
{
dlt = 1.0;
dlt2 = 0.0;
}
Hat1 = Matrix.SkewSymmetricMatrix(SINSstate.Omega_x);
Hat2 = Matrix.SkewSymmetricMatrixSquare(SINSstate.Omega_x);
CopyMatrix(dMatrix, E + Hat1 * dlt + Hat2 * dlt2);
CopyMatrix(B_x_eta, dMatrix * B_x_eta);
//Нормировка
for (int i = 0; i < 3; i++)
{
tempV[i] = Math.Sqrt(B_x_eta[i, 0] * B_x_eta[i, 0] + B_x_eta[i, 1] * B_x_eta[i, 1] + B_x_eta[i, 2] * B_x_eta[i, 2]);
for (int j = 0; j < 3; j++)
{
B_x_eta[i, j] = B_x_eta[i, j] / tempV[i];
}
}
// --- Повторное вычисление матрицы ориентации D_x_z приборного относительно географии
CopyMatrix(W_x_xi, B_x_eta * SINSstate.A_nxi);
CopyMatrix(D_x_z, W_x_xi * SINSstate.AT.Transpose());
//----------------Вычисление углов и переприсвоение матриц---------------------------
CopyMatrix(SINSstate.A_sx0, D_x_z.Transpose());
CopyMatrix(SINSstate.A_x0s, D_x_z);
CopyMatrix(SINSstate.A_x0n, B_x_eta);
CopyMatrix(SINSstate.A_nx0, B_x_eta.Transpose());
//---ОПРЕДЕЛЕНИЕ ГЕОГРАФИЧЕСКИХ КООРДИНАТ---
SINSstate.Longitude = Math.Atan2(SINSstate.A_x0n[2, 1], SINSstate.A_x0n[2, 0]);
SINSstate.Latitude = Math.Atan2(SINSstate.A_x0n[2, 2], Math.Sqrt(SINSstate.A_x0n[0, 2] * SINSstate.A_x0n[0, 2] + SINSstate.A_x0n[1, 2] * SINSstate.A_x0n[1, 2]));
Azimth = Math.Atan2(SINSstate.A_x0n[0, 2], SINSstate.A_x0n[1, 2]);
SINSstate.Altitude_prev = SINSstate.Height;
SINSstate.Height = Altitude;
//SINSstate.Heading = gkurs - Azimth;
SINSstate.Heading = Math.Atan2(SINSstate.A_sx0[1, 0], SINSstate.A_sx0[1, 1]);
SINSstate.Roll = -Math.Atan2(SINSstate.A_sx0[0, 2], SINSstate.A_sx0[2, 2]);
SINSstate.Pitch = Math.Atan2(SINSstate.A_sx0[1, 2], Math.Sqrt(SINSstate.A_sx0[0, 2] * SINSstate.A_sx0[0, 2] + SINSstate.A_sx0[2, 2] * SINSstate.A_sx0[2, 2]));
//SINSstate.Azimth = Azimth;
SINSstate.R_e = RadiusE(SINSstate.Latitude, SINSstate.Height);
SINSstate.R_n = RadiusN(SINSstate.Latitude, SINSstate.Height);
SINSstate.u_x = U_x0(SINSstate.Latitude);
CopyArray(SINSstate.Vx_0_prev, SINSstate.Vx_0);
CopyArray(SINSstate.Vx_0, Vx_0);
CopyArray(SINSstate.F_z_prev, SINSstate.F_z);
CopyArray(SINSstate.W_z_prev, SINSstate.W_z);
CopyArray(SINSstate.W_x, SINSstate.A_x0s * Wz);
//--------------------------------------------------------------------------------------
//--------------------------------------------------------------------------------------
//--------- ДЛЯ SINSstate_OdoModel - одометрического счисления ---------//
SimpleOperations.CopyArray(SINSstate_OdoMod.Vx_0_prev, SINSstate_OdoMod.Vx_0);
SimpleOperations.CopyArray(SINSstate.OdoSpeed_x0, SINSstate.A_x0s * SINSstate.OdoSpeed_s);
SimpleOperations.CopyArray(SINSstate_OdoMod.OdoSpeed_s, SINSstate.OdoSpeed_s);
//--- считаем для одометрического счисления свою матрицу ориентации SINSstate_OdoMod.A_x0s ---//
SimpleOperations.CopyMatrix(W_x_xi, SINSstate_OdoMod.A_x0n * SINSstate.A_nxi);
SimpleOperations.CopyMatrix(SINSstate_OdoMod.A_x0s, W_x_xi * SINSstate.AT.Transpose());
SimpleOperations.CopyArray(SINSstate_OdoMod.OdoSpeed_x0, SINSstate_OdoMod.A_x0s * SINSstate.OdoSpeed_s);
//---------ВЫЧИСЛЕНИЕ МАТРИЦЫ B_X_ETA И ВТОРОЕ ВЫЧИСЛЕНИЕ МАТРИЦЫ D_X_Z для одометрического счисления--------------
{
SimpleOperations.CopyArray(SINSstate_OdoMod.Vx_0, SINSstate_OdoMod.OdoSpeed_x0);
SINSstate_OdoMod.Omega_x[0] = -(SINSstate_OdoMod.Vx_0[1] + SINSstate_OdoMod.Vx_0_prev[1]) / 2.0 / SINSstate_OdoMod.R_n;
SINSstate_OdoMod.Omega_x[1] = (SINSstate_OdoMod.Vx_0[0] + SINSstate_OdoMod.Vx_0_prev[0]) / 2.0 / SINSstate_OdoMod.R_e;
SINSstate_OdoMod.Omega_x[2] = Math.Tan(SINSstate_OdoMod.Latitude) * SINSstate_OdoMod.Omega_x[1];
//--- Производим одометрическое счисление координат, если пришло обновление показаний одометра ---//
if (SINSstate.OdometerData.odometer_left.isReady == 1)
{
double[] dS_x = new double[3];
SimpleOperations.CopyArray(dS_x, SINSstate_OdoMod.A_x0s * SINSstate.OdometerVector);
SINSstate_OdoMod.Latitude = SINSstate_OdoMod.Latitude + dS_x[1] / SimpleOperations.RadiusN(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Height);
SINSstate_OdoMod.Longitude = SINSstate_OdoMod.Longitude + dS_x[0] / SimpleOperations.RadiusE(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Height) / Math.Cos(SINSstate_OdoMod.Latitude);
SINSstate_OdoMod.Height = SINSstate_OdoMod.Height + dS_x[2];
}
//----------------Вычисление углов и переприсвоение матриц---------------------------
SINSstate_OdoMod.A_x0n = SimpleOperations.A_x0n(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Longitude);
SimpleOperations.CopyMatrix(SINSstate_OdoMod.A_nx0, SINSstate_OdoMod.A_x0n.Transpose());
SimpleOperations.CopyMatrix(W_x_xi, SINSstate_OdoMod.A_x0n * SINSstate.A_nxi);
SimpleOperations.CopyMatrix(SINSstate_OdoMod.A_x0s, W_x_xi * SINSstate.AT.Transpose());
SimpleOperations.CopyMatrix(SINSstate_OdoMod.A_sx0, SINSstate_OdoMod.A_x0s.Transpose());
}
SINSstate_OdoMod.R_e = RadiusE(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Height);
SINSstate_OdoMod.R_n = RadiusN(SINSstate_OdoMod.Latitude, SINSstate_OdoMod.Height);
//--------------------------------------------------------------------------------------
}
public static void MatrixNoise_ReDef(SINS_State SINSstate, Kalman_Vars KalmanVars)
{
int iMx = SimpleData.iMx, iMz = SimpleData.iMz, iMq = SimpleData.iMq, iMx_r12_odo = SINSstate.value_iMx_r_odo_12, iMx_kappa_3_ds = SINSstate.value_iMx_kappa_3_ds;
int iMx_dV_12 = SINSstate.value_iMx_dV_12, iMx_alphaBeta = SINSstate.value_iMx_alphaBeta, iMx_Nu0 = SINSstate.value_iMx_Nu0;
double sqrt_freq_vert = Math.Sqrt(Math.Abs(SINSstate.Freq));
double sqrt_freq = Math.Sqrt(Math.Abs(SINSstate.Freq));
double Noise_Pos = KalmanVars.Noise_Pos, Noise_Pos_Vertical = KalmanVars.Noise_Pos_Vertical;
if (Math.Abs(SimpleOperations.AbsoluteVectorValue(SINSstate.Vx_0)) < 0.2)
{
//sqrt_freq = 1.0;
sqrt_freq_vert = 1.0;
Noise_Pos = 0.1;
Noise_Pos_Vertical = 0.0;
}
double[] Noise_Vel_in_Mx = new double[3], Noise_Angl_in_Mx = new double[3];
for (int i = 0; i < iMx * iMq; i++)
{
KalmanVars.CovarianceMatrixNoise[i] = 0.0;
}
// --- На основе шумовых параметров, полученных на выставке в приборной системе координат, формируем шумовые параметры в проекции на географическую СК
for (int j = 0; j < 3; j++)
{
Noise_Vel_in_Mx[j] = Math.Sqrt(Math.Pow(SINSstate.A_x0s[j, 0] * KalmanVars.Noise_Vel[0], 2) +
Math.Pow(SINSstate.A_x0s[j, 1] * KalmanVars.Noise_Vel[1], 2) +
Math.Pow(SINSstate.A_x0s[j, 2] * KalmanVars.Noise_Vel[2], 2));
Noise_Angl_in_Mx[j] = Math.Sqrt(Math.Pow(SINSstate.A_x0s[j, 0] * KalmanVars.Noise_Angl[0], 2) +
Math.Pow(SINSstate.A_x0s[j, 1] * KalmanVars.Noise_Angl[1], 2) +
Math.Pow(SINSstate.A_x0s[j, 2] * KalmanVars.Noise_Angl[2], 2));
}
// --- шумы по горизонтальному каналу БИНС
KalmanVars.CovarianceMatrixNoise[0 * iMq + 0] = Noise_Pos * sqrt_freq;
KalmanVars.CovarianceMatrixNoise[1 * iMq + 1] = Noise_Pos * sqrt_freq;
// --- Проставляются параметры шумов датчиков в матриц Q //
KalmanVars.CovarianceMatrixNoise[(iMx_dV_12 + 0) * iMq + iMx_dV_12 + 0] = Noise_Vel_in_Mx[0] * sqrt_freq;
//KalmanVars.CovarianceMatrixNoise[(iMx_dV_12 + 0) * iMq + iMx_alphaBeta + 0] = SINSstate.Vx_0[1] * Noise_Angl_in_Mx[0] * sqrt_freq;
KalmanVars.CovarianceMatrixNoise[(iMx_dV_12 + 1) * iMq + iMx_dV_12 + 1] = Noise_Vel_in_Mx[1] * sqrt_freq;
//KalmanVars.CovarianceMatrixNoise[(iMx_dV_12 + 1) * iMq + iMx_alphaBeta + 1] = SINSstate.Vx_0[0] * Noise_Angl_in_Mx[1] * sqrt_freq;
KalmanVars.CovarianceMatrixNoise[(iMx_alphaBeta + 0) * iMq + iMx_alphaBeta + 0] = Noise_Angl_in_Mx[0] * sqrt_freq;
KalmanVars.CovarianceMatrixNoise[(iMx_alphaBeta + 1) * iMq + iMx_alphaBeta + 1] = Noise_Angl_in_Mx[1] * sqrt_freq;
KalmanVars.CovarianceMatrixNoise[(iMx_alphaBeta + 2) * iMq + iMx_alphaBeta + 2] = Noise_Angl_in_Mx[2] * sqrt_freq;
// --- шумы по горизонтальному одометрическому решению
KalmanVars.CovarianceMatrixNoise[(iMx_r12_odo + 0) * iMq + iMx_r12_odo + 0] = Noise_Pos * sqrt_freq;
KalmanVars.CovarianceMatrixNoise[(iMx_r12_odo + 1) * iMq + iMx_r12_odo + 1] = Noise_Pos * sqrt_freq;
//KalmanVars.CovarianceMatrixNoise[(iMx_Nu0 + 0) * iMq + iMx_Nu0 + 0] = 0.001 * SimpleData.ToRadian / 3600.0;
//KalmanVars.CovarianceMatrixNoise[(iMx_Nu0 + 1) * iMq + iMx_Nu0 + 1] = 0.001 * SimpleData.ToRadian / 3600.0;
//KalmanVars.CovarianceMatrixNoise[(iMx_Nu0 + 2) * iMq + iMx_Nu0 + 2] = 0.001 * SimpleData.ToRadian / 3600.0;
//SimpleOperations.PrintMatrixToFile(KalmanVars.CovarianceMatrixNoise, SimpleData.iMx, SimpleData.iMx, "CovarianceMatrixNoise");
// ----------------------------------------------------------//
// --- Матрица шумов для вертикального канала ---
SimpleOperations.NullingOfArray(KalmanVars.Vertical_CovarianceMatrixNoise);
KalmanVars.Vertical_CovarianceMatrixNoise[0 * SimpleData.iMq_Vertical + 0] = Noise_Pos_Vertical * sqrt_freq_vert;
KalmanVars.Vertical_CovarianceMatrixNoise[1 * SimpleData.iMq_Vertical + 1] = Noise_Vel_in_Mx[2] * sqrt_freq_vert;
KalmanVars.Vertical_CovarianceMatrixNoise[SINSstate.Vertical_rOdo3 * SimpleData.iMq_Vertical + SINSstate.Vertical_rOdo3] = Noise_Pos_Vertical * sqrt_freq_vert;
}
public static void InitOfCovarianceMatrixes(SINS_State SINSstate, 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
;
SimpleOperations.NullingOfArray(KalmanVars.CovarianceMatrixS_m);
SimpleOperations.NullingOfArray(KalmanVars.CovarianceMatrixS_p);
// --- нач. ковариации для ошибки координат --- //
KalmanVars.CovarianceMatrixS_m[0 * iMx + 0] = KalmanVars.CovarianceMatrixS_p[0 * iMx + 0] = SINSstate.stdR; // позиционные ошибки
KalmanVars.CovarianceMatrixS_m[1 * iMx + 1] = KalmanVars.CovarianceMatrixS_p[1 * iMx + 1] = SINSstate.stdR;
// --- нач. ковариации для ошибки скорости --- //
KalmanVars.CovarianceMatrixS_m[(iMx_dV_12 + 0) * iMx + (iMx_dV_12 + 0)] = KalmanVars.CovarianceMatrixS_p[(iMx_dV_12 + 0) * iMx + (iMx_dV_12 + 0)] = SINSstate.stdV; // 0.01 м/с
KalmanVars.CovarianceMatrixS_m[(iMx_dV_12 + 1) * iMx + (iMx_dV_12 + 1)] = KalmanVars.CovarianceMatrixS_p[(iMx_dV_12 + 1) * iMx + (iMx_dV_12 + 1)] = SINSstate.stdV;
// --- нач. ковариации для ошибок углов ориентации --- //
KalmanVars.CovarianceMatrixS_m[(iMx_alphaBeta + 0) * iMx + (iMx_alphaBeta + 0)] = KalmanVars.CovarianceMatrixS_p[(iMx_alphaBeta + 0) * iMx + (iMx_alphaBeta + 0)]
= Math.Max(Math.Abs(SINSstate.stdAlpha1), 1E-6); // 5 угл. минут
KalmanVars.CovarianceMatrixS_m[(iMx_alphaBeta + 1) * iMx + (iMx_alphaBeta + 1)] = KalmanVars.CovarianceMatrixS_p[(iMx_alphaBeta + 1) * iMx + (iMx_alphaBeta + 1)]
= Math.Max(Math.Abs(SINSstate.stdAlpha2), 1E-6);
KalmanVars.CovarianceMatrixS_m[(iMx_alphaBeta + 2) * iMx + (iMx_alphaBeta + 2)] = KalmanVars.CovarianceMatrixS_p[(iMx_alphaBeta + 2) * iMx + (iMx_alphaBeta + 2)]
= Math.Max(Math.Abs(SINSstate.stdBeta3), 1E-6);
// --- нач. ковариации для дрейфов ДУС --- //
KalmanVars.CovarianceMatrixS_m[(iMx_Nu0 + 0) * iMx + (iMx_Nu0 + 0)] = KalmanVars.CovarianceMatrixS_p[(iMx_Nu0 + 0) * iMx + (iMx_Nu0 + 0)]
= Math.Max(Math.Abs(SINSstate.stdNu) * SimpleData.ToRadian / 3600.0, 1E-10);
KalmanVars.CovarianceMatrixS_m[(iMx_Nu0 + 1) * iMx + (iMx_Nu0 + 1)] = KalmanVars.CovarianceMatrixS_p[(iMx_Nu0 + 1) * iMx + (iMx_Nu0 + 1)]
= Math.Max(Math.Abs(SINSstate.stdNu) * SimpleData.ToRadian / 3600.0, 1E-10);
KalmanVars.CovarianceMatrixS_m[(iMx_Nu0 + 2) * iMx + (iMx_Nu0 + 2)] = KalmanVars.CovarianceMatrixS_p[(iMx_Nu0 + 2) * iMx + (iMx_Nu0 + 2)]
= Math.Max(Math.Abs(SINSstate.stdNu) * SimpleData.ToRadian / 3600.0, 1E-10);
// --- нач. ковариации для горизонтальных ньютонометров --- //
KalmanVars.CovarianceMatrixS_m[(f0_12 + 0) * iMx + (f0_12 + 0)] = KalmanVars.CovarianceMatrixS_p[(f0_12 + 0) * iMx + (f0_12 + 0)]
= Math.Max(Math.Abs(SINSstate.stdF[0]), 1E-6); // м/с^2
KalmanVars.CovarianceMatrixS_m[(f0_12 + 1) * iMx + (f0_12 + 1)] = KalmanVars.CovarianceMatrixS_p[(f0_12 + 1) * iMx + (f0_12 + 1)]
= Math.Max(Math.Abs(SINSstate.stdF[1]), 1E-6);
// --- нач. ковариации для вертикального ньютонометра, если он включен в вектор ошибок --- //
if (SINSstate.value_iMx_f0_3 > 0)
{
KalmanVars.CovarianceMatrixS_m[(f0_3 + 0) * iMx + (f0_3 + 0)] = KalmanVars.CovarianceMatrixS_p[(f0_3 + 0) * iMx + (f0_3 + 0)]
= Math.Max(Math.Abs(SINSstate.stdF[2]), 1E-6);
}
// --- нач. ковариации для ошибок масштаба и ошибок углов установки БИНС на корпусе --- //
if (iMx_kappa_3_ds > 0)
{
KalmanVars.CovarianceMatrixS_m[(iMx_kappa_3_ds + 0) * iMx + (iMx_kappa_3_ds + 0)] = KalmanVars.CovarianceMatrixS_p[(iMx_kappa_3_ds + 0) * iMx + (iMx_kappa_3_ds + 0)] = SINSstate.stdKappa3 * SimpleData.ToRadian_min;
KalmanVars.CovarianceMatrixS_m[(iMx_kappa_3_ds + 1) * iMx + (iMx_kappa_3_ds + 1)] = KalmanVars.CovarianceMatrixS_p[(iMx_kappa_3_ds + 1) * iMx + (iMx_kappa_3_ds + 1)] = SINSstate.stdScale;
}
// --- нач. ковариации горизонтальных ошибок координат одометрического счисления --- //
KalmanVars.CovarianceMatrixS_m[(iMx_r12_odo + 0) * iMx + (iMx_r12_odo + 0)] = KalmanVars.CovarianceMatrixS_p[(iMx_r12_odo + 0) * iMx + (iMx_r12_odo + 0)] = SINSstate.stdOdoR;
KalmanVars.CovarianceMatrixS_m[(iMx_r12_odo + 1) * iMx + (iMx_r12_odo + 1)] = KalmanVars.CovarianceMatrixS_p[(iMx_r12_odo + 1) * iMx + (iMx_r12_odo + 1)] = SINSstate.stdOdoR;
SimpleOperations.PrintMatrixToFile(KalmanVars.CovarianceMatrixS_m, SimpleData.iMx, SimpleData.iMx, "StartCovariance");
// ---------------------ВЕРТИКАЛЬНЫЙ КАНАЛ ОТДЕЛЬНО-----------------------------//
int iMxV = SimpleData.iMx_Vertical,
vert_f0_3 = SINSstate.Vertical_f0_3,
Vertical_kappa1 = SINSstate.Vertical_kappa1,
Vertical_rOdo3 = SINSstate.Vertical_rOdo3
;
SimpleOperations.NullingOfArray(KalmanVars.Vertical_CovarianceMatrixS_m);
SimpleOperations.NullingOfArray(KalmanVars.Vertical_CovarianceMatrixS_p);
// --- нач. ковариации ошибок высоты и верт. скорости --- //
KalmanVars.Vertical_CovarianceMatrixS_m[0 * iMxV + 0] = KalmanVars.Vertical_CovarianceMatrixS_p[0 * iMxV + 0] = SINSstate.stdR;
KalmanVars.Vertical_CovarianceMatrixS_m[1 * iMxV + 1] = KalmanVars.Vertical_CovarianceMatrixS_p[1 * iMxV + 1] = SINSstate.stdV;
// --- нач. ковариации ошибки верт. ньютонометра --- //
KalmanVars.Vertical_CovarianceMatrixS_m[vert_f0_3 * iMxV + vert_f0_3] = KalmanVars.Vertical_CovarianceMatrixS_p[vert_f0_3 * iMxV + vert_f0_3]
= Math.Max(Math.Abs(SINSstate.stdF[2]), 1E-6);
// --- нач. ковариации ошибок мастаба одометра и углов установки БИНС на корпусе --- //
if (Vertical_kappa1 > 0)
{
KalmanVars.Vertical_CovarianceMatrixS_m[Vertical_kappa1 * iMxV + Vertical_kappa1]
= KalmanVars.Vertical_CovarianceMatrixS_p[Vertical_kappa1 * iMxV + Vertical_kappa1] = SINSstate.stdKappa1 * SimpleData.ToRadian_min;
}
KalmanVars.Vertical_CovarianceMatrixS_m[Vertical_rOdo3 * iMxV + Vertical_rOdo3]
= KalmanVars.Vertical_CovarianceMatrixS_p[Vertical_rOdo3 * iMxV + Vertical_rOdo3] = SINSstate.stdOdoR;
}
public static int RougthAlignment(Proc_Help ProcHelp, SINS_State SINSstate, StreamReader myFile, Kalman_Vars KalmanVars, SINS_State SINSstate_OdoMod, StreamWriter GRTV_output)
{
int k = 0, i = 0;
double[] f_avg = new double[3]; double[] w_avg = new double[3]; double[] w_avg_x = new double[3]; double[] U_s = new double[3];
Matrix A_xs = new Matrix(3, 3);
StreamWriter Alignment_avg_rougth = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_avg_rougth.txt");
StreamWriter Alignment_avg_rougthMovingAVG = new StreamWriter(SimpleData.PathOutputString + "Alignment//Alignment_avg_rougth_MovingAVG.txt");
StreamWriter myFileWithSmoothedCoord = new StreamWriter(SimpleData.PathInputString + "DataWithSmoothedCoord//Align_InputDataWithSmoothedCoordinates.txt");
// --- вспомогательные массивы для определения сигмы шумов
double[] array_f_1 = new double[200000], array_sigma_f_1 = new double[200000];
double[] array_f_2 = new double[200000], array_sigma_f_2 = new double[200000];
double[] array_f_3 = new double[200000], array_sigma_f_3 = new double[200000];
double[] array_w_1 = new double[200000], array_sigma_w_1 = new double[200000];
double[] array_w_2 = new double[200000], array_sigma_w_2 = new double[200000];
double[] array_w_3 = new double[200000], array_sigma_w_3 = new double[200000];
double[] sigma_f = new double[3];
double[] sigma_w = new double[3];
Alignment_avg_rougth.WriteLine("count f_1 f_2 f_3 w_1 w_2 w_3 heading roll pitch Latitude");
Alignment_avg_rougthMovingAVG.WriteLine("count MA_f_1 MA_f_2 MA_f_3 MA_w_1 MA_w_2 MA_w_3");
while (true)
{
i++;
if (i < 1)
{
myFile.ReadLine(); continue;
}
if (SINSstate.FLG_Stop == 0 && false)
{
ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, myFileWithSmoothedCoord, SINSstate, SINSstate_OdoMod, true);
}
else
{
i--;
break;
}
}
int t = i;
double Latitude = 0.0, Pitch = 0.0, Roll = 0.0, Heading = 0.0;
int MovingWindow = 500;
double[] MovingAverageAccGyro = new double[6];
// --- k_f, k_nu - отдельные счетчики сколько обновлений соответствующих датчиков были использованы для осреднения (в некоторых заездах
// --- почему-то ньютонометры на начальной выставке поставляют константное значение)
int k_f = 0, k_nu = 0;
for (i = t; ; i++)
{
ProcessingHelp.ReadSINSStateFromString(ProcHelp, myFile, myFileWithSmoothedCoord, SINSstate, SINSstate_OdoMod, true);
if (/*SINSstate.FLG_Stop == 0 || */ (ProcHelp.AlignmentCounts != 0 && i == ProcHelp.AlignmentCounts))
{
break;
}
//if (i == 1000)
// break;
array_f_1[k] = SINSstate.F_z[0];
array_f_2[k] = SINSstate.F_z[1];
array_f_3[k] = SINSstate.F_z[2];
array_w_1[k] = SINSstate.W_z[0];
array_w_2[k] = SINSstate.W_z[1];
array_w_3[k] = SINSstate.W_z[2];
double array_sigma_f_1_tmp_sum = 0.0, array_sigma_w_1_tmp_sum = 0.0;
int u = 0;
for (u = 1; u <= Math.Min(i, 50); u++)
{
array_sigma_f_1_tmp_sum += array_f_1[i - u];
}
array_sigma_f_1_tmp_sum /= (u - 1);
if (Math.Abs(array_sigma_f_1_tmp_sum - array_f_1[i]) > 1E-9)
{
array_sigma_f_1[k_f] = SINSstate.F_z[0];
array_sigma_f_2[k_f] = SINSstate.F_z[1];
array_sigma_f_3[k_f] = SINSstate.F_z[2];
f_avg[0] += SINSstate.F_z[0];
f_avg[1] += SINSstate.F_z[1];
f_avg[2] += SINSstate.F_z[2];
k_f++;
}
u = 0;
for (u = 1; u <= Math.Min(i, 50); u++)
{
array_sigma_w_1_tmp_sum += array_w_1[i - u];
}
array_sigma_w_1_tmp_sum /= (u - 1);
if (Math.Abs(array_sigma_w_1_tmp_sum - array_w_1[i]) > 1E-9)
{
array_sigma_w_1[k_nu] = SINSstate.W_z[0];
array_sigma_w_2[k_nu] = SINSstate.W_z[1];
array_sigma_w_3[k_nu] = SINSstate.W_z[2];
w_avg[0] += SINSstate.W_z[0];
w_avg[1] += SINSstate.W_z[1];
w_avg[2] += SINSstate.W_z[2];
k_nu++;
}
k++;
for (int u1 = 1; u1 <= Math.Min(k, MovingWindow); u1++)
{
MovingAverageAccGyro[0] += array_f_1[k - u1];
MovingAverageAccGyro[1] += array_f_2[k - u1];
MovingAverageAccGyro[2] += array_f_3[k - u1];
MovingAverageAccGyro[3] += array_w_1[k - u1];
MovingAverageAccGyro[4] += array_w_2[k - u1];
MovingAverageAccGyro[5] += array_w_3[k - u1];
}
for (int u1 = 0; u1 < 6; u1++)
{
MovingAverageAccGyro[u1] = MovingAverageAccGyro[u1] / MovingWindow;
}
//-------
Pitch = Math.Atan2(f_avg[1], Math.Sqrt(f_avg[0] * f_avg[0] + f_avg[2] * f_avg[2]));
Roll = -Math.Atan2(f_avg[0], f_avg[2]);
A_xs = SimpleOperations.A_xs(Heading, Roll, Pitch);
w_avg_x = Matrix.Multiply(A_xs, w_avg);
Heading = -Math.Atan2(w_avg_x[0], w_avg_x[1]);
Latitude = Math.Atan2(w_avg_x[2], Math.Sqrt(w_avg_x[1] * w_avg_x[1] + w_avg_x[0] * w_avg_x[0]));
SINSstate.A_sx0 = SimpleOperations.A_sx0(Heading, Roll, Pitch);
U_s = SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude);
//FinalAlignment.WriteLine(k + " " + SINSstate.Count + " " + Heading + " " + Roll + " " + Pitch + " " + Latitude
// + " " + SINSstate.F_z[0] + " " + SINSstate.F_z[1] + " " + SINSstate.F_z[2]
// + " " + SINSstate.W_z[0] + " " + SINSstate.W_z[1] + " " + SINSstate.W_z[2]
// + " " + U_s[0] + " " + U_s[1] + " " + U_s[2]);
if (Math.Abs(w_avg[0] / k - U_s[0]) < 0.000005)
{
}
else
{
Heading = Heading - Math.PI;
SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate);
U_s = SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude);
}
for (int j = 0; j < 3; j++)
{
SINSstate.AlignAlgebraDrifts[j] = w_avg[j] / k_nu - U_s[j];
}
if (k > MovingWindow && k % 10 == 0)
{
Alignment_avg_rougth.WriteLine(SINSstate.Count.ToString()
+ " " + (f_avg[0] / k_f).ToString() + " " + (f_avg[1] / k_f).ToString() + " " + (f_avg[2] / k_f).ToString()
+ " " + (w_avg[0] / k_nu).ToString() + " " + (w_avg[1] / k_nu).ToString() + " " + (w_avg[2] / k_nu).ToString()
+ " " + (Heading * SimpleData.ToDegree).ToString() + " " + (Roll * SimpleData.ToDegree).ToString() + " " + (Pitch * SimpleData.ToDegree).ToString() + " " + Latitude.ToString()
+ " " + (w_avg_x[0] / k_nu).ToString() + " " + (w_avg_x[1] / k_nu).ToString() + " " + (w_avg_x[2] / k_nu).ToString()
);
Alignment_avg_rougthMovingAVG.WriteLine(SINSstate.Time.ToString() + " " + MovingAverageAccGyro[0] + " " + MovingAverageAccGyro[1] + " " + MovingAverageAccGyro[2] + " " + MovingAverageAccGyro[3] + " " + MovingAverageAccGyro[4]
+ " " + MovingAverageAccGyro[5]);
}
// --- Вывод данных для формирования GRTV файла --- //
if (SINSstate.flag_GRTV_output)
{
GRTV_output.WriteLine(
SINSstate.Count
+ " " + "4" + " "
+ " " + SINSstate.F_z_orig[1] + " " + SINSstate.F_z_orig[2] + " " + SINSstate.F_z_orig[0]
+ " " + SINSstate.W_z_orig[1] + " " + SINSstate.W_z_orig[2] + " " + SINSstate.W_z_orig[0]
+ " " + SINSstate.Latitude + " " + SINSstate.Longitude + " " + SINSstate.Height
+ " " + SINSstate.Vx_0[1] + " " + SINSstate.Vx_0[0] + " " + SINSstate.Vx_0[2]
+ " " + SINSstate.Heading + " " + SINSstate.Pitch + " " + SINSstate.Roll
+ " " + SINSstate.Latitude + " 1 " + SINSstate.Longitude + " 1 " + SINSstate.Height + " 1"
+ " " + SINSstate.Vx_0[1] + " 1 " + SINSstate.Vx_0[0] + " 1 " + SINSstate.Vx_0[2] + " 1"
+ " " + SINSstate.OdometerData.odometer_left.Value_orig + " " + SINSstate.OdometerData.odometer_left.isReady_orig
//метка времени - отмечает момент времени формирования пакета СНС-данных
+ " " + SINSstate.GPS_Data.gps_Latitude.isReady_orig
+ " " + SINSstate.GPS_Data.gps_Latitude.Value_orig + " " + SINSstate.GPS_Data.gps_Latitude.isReady_orig
+ " " + SINSstate.GPS_Data.gps_Longitude.Value_orig + " " + SINSstate.GPS_Data.gps_Longitude.isReady_orig
+ " " + SINSstate.GPS_Data.gps_Altitude.Value_orig + " " + SINSstate.GPS_Data.gps_Altitude.isReady_orig
+ " " + SINSstate.GPS_Data.gps_Vn.Value_orig + " " + SINSstate.GPS_Data.gps_Vn.isReady_orig
+ " " + SINSstate.GPS_Data.gps_Ve.Value_orig + " " + SINSstate.GPS_Data.gps_Vn.isReady_orig
+ " " + " 0 0" //Скорость GPS вертикальная
);
}
}
f_avg[0] = f_avg[0] / k_f; w_avg[0] = w_avg[0] / k_nu;
f_avg[1] = f_avg[1] / k_f; w_avg[1] = w_avg[1] / k_nu;
f_avg[2] = f_avg[2] / k_f; w_avg[2] = w_avg[2] / k_nu;
for (int j = 1; j < k_f; j++)
{
sigma_f[0] += Math.Pow((array_sigma_f_1[j] - f_avg[0]), 2);
sigma_f[1] += Math.Pow((array_sigma_f_2[j] - f_avg[1]), 2);
sigma_f[2] += Math.Pow((array_sigma_f_3[j] - f_avg[2]), 2);
}
for (int j = 1; j < k_nu; j++)
{
sigma_w[0] += Math.Pow((array_sigma_w_1[j] - w_avg[0]), 2);
sigma_w[1] += Math.Pow((array_sigma_w_2[j] - w_avg[1]), 2);
sigma_w[2] += Math.Pow((array_sigma_w_3[j] - w_avg[2]), 2);
}
sigma_f[0] = Math.Sqrt(sigma_f[0] / k_f);
sigma_f[1] = Math.Sqrt(sigma_f[1] / k_f);
sigma_f[2] = Math.Sqrt(sigma_f[2] / k_f);
sigma_w[0] = Math.Sqrt(sigma_w[0] / k_nu);
sigma_w[1] = Math.Sqrt(sigma_w[1] / k_nu);
sigma_w[2] = Math.Sqrt(sigma_w[2] / k_nu);
//шумы ньютонометров и дусов
for (int j = 0; j < 3; j++)
{
if (SimpleOperations.AbsoluteVectorValue(sigma_f) > 1E-5)
{
KalmanVars.Noise_Vel[j] = sigma_f[j] / 1.0;
}
if (SimpleOperations.AbsoluteVectorValue(sigma_w) > 1E-9)
{
KalmanVars.Noise_Angl[j] = sigma_w[j] / 1.0;
}
}
// === По вертикальному шум всегда будет меньше на выставке, поэтому мы немного сглаживаем === //
if (SINSstate.flag_equalizeVertNoise == true)
{
KalmanVars.Noise_Vel[2] = (KalmanVars.Noise_Vel[0] + KalmanVars.Noise_Vel[1]) / 2.0;
KalmanVars.Noise_Angl[2] = (KalmanVars.Noise_Angl[0] + KalmanVars.Noise_Angl[1]) / 2.0;
}
SINSstate.Pitch = Math.Atan2(f_avg[1], Math.Sqrt(f_avg[0] * f_avg[0] + f_avg[2] * f_avg[2]));
SINSstate.Roll = -Math.Atan2(f_avg[0], f_avg[2]);
//if (SINSstate.Global_file == "GRTV_Ekat_151029_1_zaezd")
//{
// KalmanVars.Noise_Vel[0] = 0.007814; KalmanVars.Noise_Angl[0] = 0.0000888;
// KalmanVars.Noise_Vel[1] = 0.003528; KalmanVars.Noise_Angl[1] = 0.0002505;
// KalmanVars.Noise_Vel[2] = 0.005671; KalmanVars.Noise_Angl[2] = 0.0001697;
//}
//if (SINSstate.Global_file == "GRTVout_GCEF_format (070715выезд завод)" || SINSstate.Global_file == "GRTVout_GCEF_format (070715выезд куликовка)")
//{
// KalmanVars.Noise_Vel[0] = 0.0018; KalmanVars.Noise_Angl[0] = 0.000020;
// KalmanVars.Noise_Vel[1] = 0.0018; KalmanVars.Noise_Angl[1] = 0.000020;
// KalmanVars.Noise_Vel[2] = 0.0018; KalmanVars.Noise_Angl[2] = 0.000020;
//}
A_xs = SimpleOperations.A_xs(SINSstate);
w_avg_x = Matrix.Multiply(A_xs, w_avg);
if (true)
{
SINSstate.Heading = -Math.Atan2(w_avg_x[0], w_avg_x[1]);
Latitude = Math.Atan2(w_avg_x[2], Math.Sqrt(w_avg_x[1] * w_avg_x[1] + w_avg_x[0] * w_avg_x[0]));
}
// -- следующий способ дает то же самое с разницей в несколько секунд -- //
else
{
double[] l1 = new double[3], l2 = new double[3], l3 = new double[3];
l3[0] = A_xs[2, 0]; l3[1] = A_xs[2, 1]; l3[2] = A_xs[2, 2];
for (int ij = 0; ij < 3; ij++)
{
l2[ij] = (w_avg[ij] - SimpleData.U * l3[ij] * Math.Sin(SINSstate.Latitude)) / (SimpleData.U * Math.Cos(SINSstate.Latitude));
}
double l2_mod = Math.Sqrt(l2[0] * l2[0] + l2[1] * l2[1] + l2[2] * l2[2]);
l2[0] = l2[0] / l2_mod;
l2[1] = l2[1] / l2_mod;
l2[2] = l2[2] / l2_mod;
l1[0] = -l2[2] * l3[1] + l2[1] * l3[2];
l1[1] = l2[2] * l3[0] - l2[0] * l3[2];
l1[2] = -l2[1] * l3[0] + l2[0] * l3[1];
SINSstate.Heading = -Math.Atan2(w_avg_x[0], w_avg_x[1]);
SINSstate.Heading = Math.Atan2(l1[1], l2[1]);
}
SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate);
U_s = SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude);
//if (Math.Abs(w_avg[0] - U_s[0]) < 0.000005) { }
//else
//{
// SINSstate.Heading = SINSstate.Heading - Math.PI;
// SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate);
// U_s = SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude);
//}
double[] gilmertF = new double[3];
gilmertF[2] = SimpleOperations.GilmertGravityForce(SINSstate.Latitude, SINSstate.Height);
SimpleOperations.CopyArray(gilmertF, SINSstate.A_sx0 * gilmertF);
for (int j = 0; j < 3; j++)
{
SINSstate.AlignAlgebraDrifts[j] = w_avg[j] - U_s[j];
}
if (SINSstate.AlgebraicCalibration_F_Zero == true)
{
for (int j = 0; j < 3; j++)
{
SINSstate.AlignAlgebraZeroF[j] = f_avg[j] - gilmertF[j];
}
}
SINSstate.Time_Alignment = SINSstate.Time;
// --- Если задан курс в настройках ---//
if (SINSstate.Alignment_HeadingDetermined == true)
{
SINSstate.Heading = SINSstate.Alignment_HeadingValue + SINSstate.alpha_kappa_3;
}
if (SINSstate.Alignment_RollDetermined == true)
{
SINSstate.Roll = SINSstate.Alignment_RollValue;
}
if (SINSstate.Alignment_PitchDetermined == true)
{
SINSstate.Pitch = SINSstate.Alignment_PitchValue - SINSstate.alpha_kappa_1;
}
if (Math.Abs(SINSstate.WRONG_alpha_kappa_1) > 0.0)
{
SINSstate.Pitch = SINSstate.Pitch - SINSstate.WRONG_alpha_kappa_1;
}
if (Math.Abs(SINSstate.WRONG_alpha_kappa_3) > 0.0)
{
SINSstate.Heading = SINSstate.Heading + SINSstate.WRONG_alpha_kappa_3;
}
if (SINSstate.Global_file == "Saratov_run_2014_07_23")
{
double lat_dif_true = (49.99452656 * SimpleData.ToRadian - SINSstate.Latitude_Start) * SimpleOperations.RadiusN(49.99452656 * SimpleData.ToRadian, SINSstate.Height_Start);
double long_dif_true = (46.87201806 * SimpleData.ToRadian - SINSstate.Longitude_Start) * SimpleOperations.RadiusE(49.99452656 * SimpleData.ToRadian, SINSstate.Height_Start) * Math.Cos(49.99452656 * SimpleData.ToRadian);
double SettedHeading = Math.Atan2(long_dif_true, lat_dif_true);
if (SINSstate.Time > 10000.0)
{
SettedHeading = SimpleOperations.CalculateHeadingByTwoDots(49.80892188 * SimpleData.ToRadian, 45.3817334 * SimpleData.ToRadian, SINSstate.GPS_Data.gps_Altitude_prev.Value,
49.80906066 * SimpleData.ToRadian, 45.38113053 * SimpleData.ToRadian, SINSstate.GPS_Data.gps_Altitude.Value);
SINSstate.Heading = SettedHeading;
SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate);
SimpleOperations.CopyArray(U_s, SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude));
}
else
{
double difHeadings = SettedHeading - SINSstate.Heading;
SINSstate.Heading = SettedHeading;
SINSstate.A_sx0 = SimpleOperations.A_sx0(SINSstate);
SimpleOperations.CopyArray(U_s, SINSstate.A_sx0 * SimpleOperations.U_x0(SINSstate.Latitude));
}
for (int j = 0; j < 3; j++)
{
SINSstate.AlignAlgebraDrifts[j] = w_avg[j] - U_s[j];
}
for (int j = 0; j < 3; j++)
{
KalmanVars.Noise_Vel[j] = KalmanVars.Noise_Vel[j] * 5.0;
KalmanVars.Noise_Angl[j] = KalmanVars.Noise_Angl[j] * 5.0;
//KalmanVars.Noise_Vel[j] = KalmanVars.Noise_Vel.Max();
//KalmanVars.Noise_Angl[j] = KalmanVars.Noise_Angl.Max();
}
}
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! //
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.AT = Matrix.Multiply(SINSstate.A_sx0, SINSstate.A_x0n);
SINSstate.A_nxi = SimpleOperations.A_ne(SINSstate.Time - SINSstate.Time_Alignment, SINSstate.Longitude_Start);
//Далее произойдет обнуление SINSstate.Time
SINSstate.AT = Matrix.Multiply(SINSstate.AT, SINSstate.A_nxi);
Alignment_avg_rougth.Close();
Alignment_avg_rougthMovingAVG.Close();
myFileWithSmoothedCoord.Close();
return(i);
}
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 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);
}
}
