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 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 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 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 void KalmanCorrection(Kalman_Vars KalmanVars, SINS_State SINSstate, SINS_State SINSstate_OdoMod)
{
SimpleOperations.NullingOfArray(KalmanVars.KalmanFactor);
SimpleOperations.NullingOfArray(KalmanVars.StringOfMeasure);
if (!SINSstate.MyOwnKalman_Korrection)
{
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];
}
}
}
}
}
}
}
}
else
{
myOwn_f0b(
KalmanVars.cnt_measures, SimpleData.iMx,
KalmanVars.Matrix_H, KalmanVars.Measure, KalmanVars.Noize_Z,
KalmanVars.CovarianceMatrixS_p, KalmanVars.CovarianceMatrixS_m,
KalmanVars.ErrorConditionVector_m, KalmanVars.ErrorConditionVector_p
);
}
// ----------------------------------------------------------//
// ----------------------------------------------------------//
// ----------------------------------------------------------//
if (SINSstate.flag_SeparateHorizVSVertical == true)
{
SimpleOperations.NullingOfArray(KalmanVars.KalmanFactor);
SimpleOperations.NullingOfArray(KalmanVars.StringOfMeasure);
if (!SINSstate.MyOwnKalman_Korrection)
{
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];
}
}
}
}
}
}
}
}
else
{
myOwn_f0b(
KalmanVars.Vertical_cnt_measures, SimpleData.iMx_Vertical,
KalmanVars.Vertical_Matrix_H, KalmanVars.Vertical_Measure, KalmanVars.Vertical_Noize_Z,
KalmanVars.Vertical_CovarianceMatrixS_p, KalmanVars.Vertical_CovarianceMatrixS_m,
KalmanVars.Vertical_ErrorConditionVector_m, KalmanVars.Vertical_ErrorConditionVector_p
);
}
}
}