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