Exemplo n.º 1
0
        public static double CalculateHeadingByTwoDots(double Lat1, double Lon1, double Alt1, double Lat2, double Lon2, double Alt2)
        {
            double lat_dif_true  = (Lat2 - Lat1) * SimpleOperations.RadiusN(Lat2, Alt2);
            double long_dif_true = (Lon2 - Lon1) * SimpleOperations.RadiusE(Lat2, Alt2) * Math.Cos(Lat2);

            return(Math.Atan2(long_dif_true, lat_dif_true));
        }
Exemplo n.º 2
0
        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);
            }
        }
Exemplo n.º 3
0
        //---------------------------------Задается модель поведения движения-------------------------------------------

        public static double CalculateDistanceBtwDots(double Lat1, double Lon1, double Alt1, double Lat2, double Lon2, double Alt2)
        {
            double lat_dif_true  = (Lat2 - Lat1) * SimpleOperations.RadiusN(Lat2, Alt2);
            double long_dif_true = (Lon2 - Lon1) * SimpleOperations.RadiusE(Lat2, Alt2) * Math.Cos(Lat2);

            return(Math.Sqrt(lat_dif_true * lat_dif_true + long_dif_true * long_dif_true));
        }
Exemplo n.º 4
0
        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);
        }
Exemplo n.º 5
0
        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];
                    }
                }
            }
        }
Exemplo n.º 6
0
        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;
            }
        }
Exemplo n.º 7
0
 public static void KalmanForecast_Align(Kalman_Align KalmanAlign)
 {
     unsafe
     {
         fixed(double *_xm = KalmanAlign.ErrorConditionVector_m, _xp = KalmanAlign.ErrorConditionVector_p, _sm = KalmanAlign.CovarianceMatrixS_m, _sp = KalmanAlign.CovarianceMatrixS_p, _f = KalmanAlign.TransitionMatrixF, _sq = KalmanAlign.CovarianceMatrixNoise)
         {
             KalmanProcs.dgq0b(_xp, _sp, _f, _sq, _xm, _sm, SimpleData.iMx_Align, SimpleData.iMq_Align);
         }
     }
     SimpleOperations.CopyArray(KalmanAlign.CovarianceMatrixS_p, KalmanAlign.CovarianceMatrixS_m);
     SimpleOperations.CopyArray(KalmanAlign.ErrorConditionVector_p, KalmanAlign.ErrorConditionVector_m);
 }
Exemplo n.º 8
0
        public static void ApplyMatrixStartCondition(SINS_State SINSstate)
        {
            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.A_nxi = SimpleOperations.A_ne(SINSstate.Time, SINSstate.Longitude_Start);
            SINSstate.AT    = Matrix.Multiply(SINSstate.A_sx0, SINSstate.A_x0n);
            SINSstate.AT    = Matrix.Multiply(SINSstate.AT, SINSstate.A_nxi);

            SINSstate.R_e = RadiusE(SINSstate.Latitude, SINSstate.Height);
            SINSstate.R_n = RadiusN(SINSstate.Latitude, SINSstate.Height);
        }
Exemplo n.º 9
0
        //--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
        //--------------------------------------------------------------------Коррекция дрейфа на стоянке-----------------------------------------------------------------------------------------------------------
        //--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
        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;
        }
Exemplo n.º 10
0
        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;
            }
        }
Exemplo n.º 11
0
        public static void OutPutInfo(int i, int start_i, Proc_Help ProcHelp, SINS_State SINSstate, SINS_State SINSstate_OdoMod, Kalman_Vars KalmanVars,
                                      StreamWriter Nav_FeedbackSolution,
                                      StreamWriter Nav_StateErrorsVector,
                                      StreamWriter Nav_Errors,
                                      StreamWriter GRTV_output,
                                      StreamWriter Nav_CovarianceDiagonal,
                                      StreamWriter KMLFileOut, StreamWriter KMLFileOut_PNPPK
                                      )
        {
            double Lat = 0.0, Long = 0.0;

            double[] Vx_0 = new double[3];

            Lat  = SINSstate.Latitude;
            Long = SINSstate.Longitude;
            SimpleOperations.CopyArray(Vx_0, SINSstate.Vx_0);

            ProcHelp.distance = Math.Sqrt(Math.Pow((Lat - ProcHelp.LatSNS * SimpleData.ToRadian) * SimpleOperations.RadiusN(Lat, SINSstate.Height), 2) +
                                          Math.Pow((Long - ProcHelp.LongSNS * SimpleData.ToRadian) * SimpleOperations.RadiusE(Lat, SINSstate.Height) * Math.Cos(Lat), 2));
            ProcHelp.distance_from_start = Math.Sqrt(Math.Pow((Lat - SINSstate.Latitude_Start) * SimpleOperations.RadiusN(Lat, SINSstate.Height), 2) +
                                                     Math.Pow((Long - SINSstate.Longitude_Start) * SimpleOperations.RadiusE(Lat, SINSstate.Height) * Math.Cos(Lat), 2));


            int iMx_kappa_3_ds = SINSstate.value_iMx_kappa_3_ds, iMx_alphaBeta = SINSstate.value_iMx_alphaBeta;


            // --- Вывод в файл данных, необходимых для формирования GRTV бинарного фалйа --- //
            if (SINSstate.flag_GRTV_output == true)
            {
                if (i == start_i)
                {
                    StreamWriter GRTV_init_output = new StreamWriter(SimpleData.PathOutputString + "\\TXT for GRTV\\S_GRTV_init_output.txt");

                    GRTV_init_output.WriteLine(
                        "fSamplingInterval " + SINSstate.timeStep + "\n"
                        + "nInstallationModel 0\n"                               // 0 – продольная ось прибора совпадает с продольной осью объекта
                        + "flLatitudeID " + SINSstate.Latitude_Start + " 1\n"
                        + "flLongitudeID " + SINSstate.Longitude_Start + " 1\n"
                        + "flHeightID " + SINSstate.Altitude_Start + " 1\n"
                        + "flTrueHeadID " + SINSstate.Heading + " 1\n"     //начальная долгота, заданная с пульта [рад]
                        + "flAzimuthMisalignment 0.0 0\n"                  // Угол азимутального рассогласования
                        + "flElevation 0.0 0"                              //начальный угол возвышения ИНС [рад]
                        );

                    GRTV_init_output.Close();
                }

                int modeGRTV = 16;
                if (i < ProcHelp.AlignmentCounts)
                {
                    modeGRTV = 8;
                }

                GRTV_output.WriteLine(
                    SINSstate.Count
                    + " " + modeGRTV + " "
                    + " " + 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" + " 0" + " 0" + " 0" + " 0" + " 0" + " 0" + " 0" + " 0" + " 0"


                    + " " + " 0 0" //Скорость GPS вертикальная
                    );
            }


            /*----------------------------------OUTPUT FEEDBACK------------------------------------------------------*/
            if (i % SINSstate.FreqOutput == 0)
            {
                ProcHelp.datastring = (SINSstate.Time + SINSstate.Time_Alignment) + " " + SINSstate.Count
                                      + " " + SINSstate.OdoTimeStepCount + " " + SimpleOperations.AbsoluteVectorValue(SINSstate.OdoSpeed_s)
                                      + " " + Math.Round(((SINSstate.Latitude - SINSstate.Latitude_Start) * SINSstate.R_n), 2)
                                      + " " + Math.Round(((SINSstate.Longitude - SINSstate.Longitude_Start) * SINSstate.R_e * Math.Cos(SINSstate.Latitude)), 2) + " " + SINSstate.Height
                                      + " " + ((SINSstate.Latitude) * SimpleData.ToDegree) + " " + ((SINSstate.Longitude) * SimpleData.ToDegree)
                                      + " " + Math.Round(SINSstate.Vx_0[0], 3) + " " + Math.Round(SINSstate.Vx_0[1], 3) + " " + Math.Round(SINSstate.Vx_0[2], 5)
                                      + " " + Math.Round((SINSstate.Heading * SimpleData.ToDegree), 8)
                                      + " " + Math.Round((SINSstate.Roll * SimpleData.ToDegree), 8) + " " + Math.Round((SINSstate.Pitch * SimpleData.ToDegree), 8)
                                      + " " + ProcHelp.distance
                                      + " " + (SINSstate.Height - ProcHelp.AltSNS)
                                      + " " + SINSstate.OdometerData.odometer_left.Value
                                      + " " + SINSstate.GPS_Data.gps_Latitude_pnppk_sol.Value * SimpleData.ToDegree
                                      + " " + SINSstate.GPS_Data.gps_Longitude_pnppk_sol.Value * SimpleData.ToDegree
                                      + " " + SINSstate.GPS_Data.gps_Altitude_pnppk_sol.Value
                ;

                Nav_FeedbackSolution.WriteLine(ProcHelp.datastring);
            }



            if (i % SINSstate.FreqOutput == 0)
            {
                /*----------------------------------OUTPUT ERRORS------------------------------------------------------*/
                ProcHelp.datastring = (SINSstate.Time + SINSstate.Time_Alignment)
                                      + " " + SINSstate.Cumulative_StateErrorVector[0] * SimpleOperations.RadiusN(Lat, SINSstate.Height)
                                      + " " + SINSstate.Cumulative_StateErrorVector[1] * SimpleOperations.RadiusE(Lat, SINSstate.Height) * Math.Cos(Lat)
                                      + " " + SINSstate.Cumulative_StateErrorVector[2]
                                      + " " + SINSstate.Cumulative_StateErrorVector[3]
                                      + " " + SINSstate.Cumulative_StateErrorVector[4]
                                      + " " + SINSstate.Cumulative_StateErrorVector[5]
                                      + " " + SINSstate.Cumulative_StateErrorVector[6] * SimpleData.ToDegree
                                      + " " + SINSstate.Cumulative_StateErrorVector[7] * SimpleData.ToDegree
                                      + " " + SINSstate.Cumulative_StateErrorVector[8] * SimpleData.ToDegree
                ;

                Nav_Errors.WriteLine(ProcHelp.datastring);



                /*----------------------------------OUTPUT StateErrors------------------------------------------------------*/
                int iMx = SimpleData.iMx, iMz = SimpleData.iMz, iMq = SimpleData.iMq, iMx_r12_odo = SINSstate.value_iMx_r_odo_12;

                int iMx_dV_12 = SINSstate.value_iMx_dV_12,
                    iMx_Nu0   = SINSstate.value_iMx_Nu0,
                    f0_12     = SINSstate.value_iMx_f0_12,
                    f0_3      = SINSstate.value_iMx_f0_3
                ;

                double[] Vertical_ErrorConditionVector = new double[KalmanVars.Vertical_ErrorConditionVector_p.Length];
                SimpleOperations.CopyArray(Vertical_ErrorConditionVector, SINSstate.Vertical_Cumulative_KalmanErrorVector);

                ProcHelp.datastring = (SINSstate.Time + SINSstate.Time_Alignment)
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[0]
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[1]
                                      + " " + Vertical_ErrorConditionVector[0]
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_dV_12 + 0)]
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_dV_12 + 1)]
                                      + " " + Vertical_ErrorConditionVector[1]
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_alphaBeta + 0)] * SimpleData.ToDegree
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_alphaBeta + 1)] * SimpleData.ToDegree
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_alphaBeta + 2)] * SimpleData.ToDegree
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_Nu0 + 0)] * SimpleData.ToDegree * 3600.0
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_Nu0 + 1)] * SimpleData.ToDegree * 3600.0
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[(iMx_Nu0 + 2)] * SimpleData.ToDegree * 3600.0
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[(f0_12 + 0)]
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[(f0_12 + 1)]
                                      + " " + Vertical_ErrorConditionVector[SINSstate.Vertical_f0_3 + 0]
                ;

                ProcHelp.datastring = ProcHelp.datastring
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[iMx_r12_odo]
                                      + " " + SINSstate.Cumulative_KalmanErrorVector[iMx_r12_odo + 1]
                                      + " " + Vertical_ErrorConditionVector[SINSstate.Vertical_rOdo3]
                ;

                if (SINSstate.Vertical_kappa1 > 0)
                {
                    ProcHelp.datastring = ProcHelp.datastring + " " + Vertical_ErrorConditionVector[SINSstate.Vertical_kappa1] * SimpleData.ToDegree;
                }
                else
                {
                    ProcHelp.datastring = ProcHelp.datastring + " 0";
                }

                if (iMx_kappa_3_ds > 0)
                {
                    ProcHelp.datastring = ProcHelp.datastring
                                          + " " + SINSstate.Cumulative_KalmanErrorVector[iMx_kappa_3_ds + 0] * SimpleData.ToDegree
                                          + " " + SINSstate.Cumulative_KalmanErrorVector[iMx_kappa_3_ds + 1]
                                          + " " + (-SINSstate.Cumulative_KalmanErrorVector[(iMx_alphaBeta + 2)] + SINSstate.Cumulative_KalmanErrorVector[iMx_kappa_3_ds + 0]) * SimpleData.ToDegree;
                }
                else
                {
                    ProcHelp.datastring = ProcHelp.datastring + " 0 0";
                }

                Nav_StateErrorsVector.WriteLine(ProcHelp.datastring);


                // ----------------------------------------------------------//
                // ----------Вывод в файл для GoogleEarth---------------------//
                // ----------------------------------------------------------//
                double[] PhiLambdaH_WGS84 = GeodesicVsGreenwich.Geodesic2Geodesic(SINSstate.Latitude, SINSstate.Longitude, SINSstate.Height, 1);

                KMLFileOut.WriteLine(PhiLambdaH_WGS84[1] * SimpleData.ToDegree
                                     + "," + PhiLambdaH_WGS84[0] * SimpleData.ToDegree
                                     + "," + (SINSstate.Time + SINSstate.Time_Alignment)
                                     );

                double[] PhiLambdaH_WGS84_PNPPK = GeodesicVsGreenwich.Geodesic2Geodesic(SINSstate.GPS_Data.gps_Latitude_pnppk_sol.Value, SINSstate.GPS_Data.gps_Longitude_pnppk_sol.Value, SINSstate.GPS_Data.gps_Altitude_pnppk_sol.Value, 1);

                KMLFileOut_PNPPK.WriteLine(PhiLambdaH_WGS84_PNPPK[1] * SimpleData.ToDegree
                                           + "," + PhiLambdaH_WGS84_PNPPK[0] * SimpleData.ToDegree
                                           + "," + (SINSstate.Time + SINSstate.Time_Alignment)
                                           );
            }


            if (i % Convert.ToInt32(Math.Round(1.0 / SINSstate.Freq)) == 0)
            {
                int iMx = SimpleData.iMx, iMx_r12_odo = SINSstate.value_iMx_r_odo_12;
                int iMx_dV_12 = SINSstate.value_iMx_dV_12, iMx_Nu0 = SINSstate.value_iMx_Nu0, f0_12 = SINSstate.value_iMx_f0_12, f0_3 = SINSstate.value_iMx_f0_3;
                int iMxV = SimpleData.iMx_Vertical, vert_f0_3 = SINSstate.Vertical_f0_3, vert_kappa1 = SINSstate.Vertical_kappa1, Vertical_rOdo3 = SINSstate.Vertical_rOdo3;

                ProcHelp.datastring = (SINSstate.Time + SINSstate.Time_Alignment)
                                      + " " + KalmanVars.CovarianceMatrixS_p[0 * iMx + 0]
                                      + " " + KalmanVars.CovarianceMatrixS_p[1 * iMx + 1]

                                      + " " + KalmanVars.Vertical_CovarianceMatrixS_m[0 * iMxV + 0]

                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_r12_odo + 0) * iMx + (iMx_r12_odo + 0)]
                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_r12_odo + 1) * iMx + (iMx_r12_odo + 1)]

                                      + " " + KalmanVars.Vertical_CovarianceMatrixS_m[Vertical_rOdo3 * iMxV + Vertical_rOdo3]

                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_dV_12 + 0) * iMx + (iMx_dV_12 + 0)]
                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_dV_12 + 1) * iMx + (iMx_dV_12 + 1)]

                                      + " " + KalmanVars.Vertical_CovarianceMatrixS_m[1 * iMxV + 1]

                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_alphaBeta + 0) * iMx + (iMx_alphaBeta + 0)]
                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_alphaBeta + 1) * iMx + (iMx_alphaBeta + 1)]
                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_alphaBeta + 2) * iMx + (iMx_alphaBeta + 2)]
                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_Nu0 + 0) * iMx + (iMx_Nu0 + 0)]
                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_Nu0 + 1) * iMx + (iMx_Nu0 + 1)]
                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_Nu0 + 2) * iMx + (iMx_Nu0 + 2)]
                                      + " " + KalmanVars.CovarianceMatrixS_p[(f0_12 + 0) * iMx + (f0_12 + 0)]
                                      + " " + KalmanVars.CovarianceMatrixS_p[(f0_12 + 1) * iMx + (f0_12 + 1)]

                                      + " " + KalmanVars.Vertical_CovarianceMatrixS_m[(vert_f0_3 + 0) * iMxV + (vert_f0_3 + 0)]
                                      + " " + KalmanVars.Vertical_CovarianceMatrixS_m[(vert_kappa1 + 0) * iMxV + (vert_kappa1 + 0)]

                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_kappa_3_ds + 0) * iMx + (iMx_kappa_3_ds + 0)]
                                      + " " + KalmanVars.CovarianceMatrixS_p[(iMx_kappa_3_ds + 1) * iMx + (iMx_kappa_3_ds + 1)]
                ;
                Nav_CovarianceDiagonal.WriteLine(ProcHelp.datastring);
            }
        }
Exemplo n.º 12
0
        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];
            }
        }
Exemplo n.º 13
0
        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;
        }
Exemplo n.º 14
0
        private static void myOwn_f0b(
            int in_cnt_measures, int in_iMx,
            double[] in_Matrix_H, double[] in_Measure, double[] in_Noize_Z,
            double[] in_CovarianceMatrixS_p, double[] in_CovarianceMatrixS_m,
            double[] in_ErrorConditionVector_m, double[] in_ErrorConditionVector_p
            )
        {
            double d_k = 0.0, b_k = 0.0, c_k = 0.0;

            double[] f, e, S_k_m, S_k_p;
            Matrix   CovarianceMatrixS_p = new Matrix(in_iMx, in_iMx),
                     CovarianceMatrixS_m = new Matrix(in_iMx, in_iMx);

            double[] StringOfMeasure = new double[in_iMx];

            for (int t = 0; t < in_cnt_measures; t++)
            {
                d_k = in_Noize_Z[t] * in_Noize_Z[t];

                e     = new double[in_iMx];
                S_k_m = new double[in_iMx];
                f     = new double[in_iMx];
                S_k_p = new double[in_iMx];

                SimpleOperations.MakeMatrixFromVector(CovarianceMatrixS_p, in_CovarianceMatrixS_p, in_iMx);
                SimpleOperations.MakeMatrixFromVector(CovarianceMatrixS_m, in_CovarianceMatrixS_m, in_iMx);

                for (int i = 0; i < in_iMx; i++)
                {
                    StringOfMeasure[i] = in_Matrix_H[t * in_iMx + i];
                }

                SimpleOperations.CopyArray(f, CovarianceMatrixS_m.Transpose() * StringOfMeasure);

                for (int i = 0; i < in_iMx; i++)
                {
                    b_k = Math.Sqrt(d_k / (d_k + f[i] * f[i]));
                    c_k = f[i] / (Math.Sqrt(d_k * (d_k + f[i] * f[i])));
                    d_k = d_k + f[i] * f[i];

                    for (int j = 0; j < in_iMx; j++)
                    {
                        S_k_m[j] = in_CovarianceMatrixS_m[j * in_iMx + i];
                    }

                    for (int j = 0; j < in_iMx; j++)
                    {
                        S_k_p[j] = S_k_m[j] * b_k - e[j] * c_k;
                        in_CovarianceMatrixS_p[j * in_iMx + i] = S_k_p[j];
                    }

                    for (int j = 0; j < in_iMx; j++)
                    {
                        e[j] = e[j] + S_k_m[j] * f[i];
                    }
                }

                double h_x_ = 0.0;
                for (int j = 0; j < in_iMx; j++)
                {
                    h_x_ += StringOfMeasure[j] * in_ErrorConditionVector_m[j];
                }

                for (int j = 0; j < in_iMx; j++)
                {
                    in_ErrorConditionVector_p[j] = in_ErrorConditionVector_m[j] + (in_Measure[t] - h_x_) * e[j] / d_k;
                }


                if (t < in_cnt_measures - 1)
                {
                    for (int j = 0; j < in_iMx; j++)
                    {
                        in_ErrorConditionVector_m[j] = in_ErrorConditionVector_p[j];
                    }
                    for (int j = 0; j < in_iMx * in_iMx; j++)
                    {
                        in_CovarianceMatrixS_m[j] = in_CovarianceMatrixS_p[j];
                    }
                }
            }
        }
Exemplo n.º 15
0
        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
                        );
                }
            }
        }
Exemplo n.º 16
0
        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);
        }
Exemplo n.º 17
0
        public static void Make_A(SINS_State SINSstate, Kalman_Vars KalmanVars, SINS_State SINSstate_OdoMod)
        {
            int iMx = SimpleData.iMx, iMz = SimpleData.iMz, iMq = SimpleData.iMq, iMx_r3_dV3 = SINSstate.iMx_r3_dV3, iMx_odo_model = SINSstate.iMx_odo_model,
                iMx_r12_odo = SINSstate.iMx_r12_odo;


            SINSprocessing.Make_A(SINSstate, KalmanVars, SINSstate_OdoMod);


            /*-----------МОДИФИЦИРОВАННЫЙ СЛАБОСВЗАННЫЙ ВАРИАНТ----------------*/
            if (SINSstate.flag_OdoSINSWeakConnect_MODIF)
            {
                KalmanVars.Matrix_A[iMx_r12_odo * iMx + 6] = SINSstate_OdoMod.Vx_0[1];
                KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 1] = SINSstate_OdoMod.Omega_x[2];

                KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 6] = -SINSstate_OdoMod.Vx_0[0];
                KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 0] = -SINSstate_OdoMod.Omega_x[2];


                if (SINSstate.flag_iMx_r3_dV3)
                {
                    KalmanVars.Matrix_A[iMx_r12_odo * iMx + 0] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_e;
                    KalmanVars.Matrix_A[iMx_r12_odo * iMx + 5] += -SINSstate_OdoMod.Vx_0[2];
                    KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 2] = -SINSstate_OdoMod.Omega_x[1];

                    KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 1] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_n;
                    KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 4] += SINSstate_OdoMod.Vx_0[2];
                    KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 2] = SINSstate_OdoMod.Omega_x[0];
                }

                if (SINSstate.flag_Using_iMx_r_odo_3)
                {
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 0] = -SINSstate_OdoMod.Vx_0[0] / SINSstate_OdoMod.R_e;
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 1] = -SINSstate_OdoMod.Vx_0[1] / SINSstate_OdoMod.R_n;
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 4] = -SINSstate_OdoMod.Vx_0[1];
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 5] = SINSstate_OdoMod.Vx_0[0];
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_r12_odo + 0] = SINSstate_OdoMod.Omega_x[1];
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_r12_odo + 1] = -SINSstate_OdoMod.Omega_x[0];
                }
            }
            /*-----------СЛАБОСВЗАННЫЙ ВАРИАНТ----------------*/
            else if (SINSstate.flag_OdoSINSWeakConnect)
            {
                KalmanVars.Matrix_A[iMx_r12_odo * iMx + 6] = SINSstate_OdoMod.Vx_0[1];
                KalmanVars.Matrix_A[iMx_r12_odo * iMx + 0] = -SINSstate_OdoMod.Omega_x[0] * Math.Tan(SINSstate_OdoMod.Latitude);
                KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 0] = SINSstate_OdoMod.Omega_x[0] * Math.Tan(SINSstate_OdoMod.Latitude);
                KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 1] = SINSstate_OdoMod.Omega_x[2];

                KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 6] = -SINSstate_OdoMod.Vx_0[0];
                KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 0] = -SINSstate_OdoMod.Omega_x[2];

                if (SINSstate.flag_iMx_r3_dV3)
                {
                    KalmanVars.Matrix_A[iMx_r12_odo * iMx + 5] += -SINSstate_OdoMod.Vx_0[2];
                    KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 0] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_e;
                    KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_r12_odo + 2]  = -SINSstate_OdoMod.Omega_x[1];

                    KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + 4] += SINSstate_OdoMod.Vx_0[2];
                    KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 1] += SINSstate_OdoMod.Vx_0[2] / SINSstate_OdoMod.R_n;
                    KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_r12_odo + 2]  = SINSstate_OdoMod.Omega_x[0];
                }

                if (SINSstate.flag_Using_iMx_r_odo_3)
                {
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 4] = -SINSstate_OdoMod.Vx_0[1];
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + 5] = SINSstate_OdoMod.Vx_0[0];
                }
            }



            if (SINSstate.flag_iMx_kappa_13_ds)
            {
                KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_odo_model + 0] = -SINSstate_OdoMod.A_x0s[0, 2] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0);
                KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_odo_model + 1] = SINSstate_OdoMod.A_x0s[0, 0] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0);
                KalmanVars.Matrix_A[iMx_r12_odo * iMx + iMx_odo_model + 2] = SINSstate_OdoMod.A_x0s[0, 1] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0);

                KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_odo_model + 0] = -SINSstate_OdoMod.A_x0s[1, 2] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0);
                KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_odo_model + 1] = SINSstate_OdoMod.A_x0s[1, 0] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0);
                KalmanVars.Matrix_A[(iMx_r12_odo + 1) * iMx + iMx_odo_model + 2] = SINSstate_OdoMod.A_x0s[1, 1] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0);

                if (SINSstate.flag_Using_iMx_r_odo_3)
                {
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_odo_model + 0] = -SINSstate_OdoMod.A_x0s[2, 2] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0);
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_odo_model + 1] = SINSstate_OdoMod.A_x0s[2, 0] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0);
                    KalmanVars.Matrix_A[(iMx_r12_odo + 2) * iMx + iMx_odo_model + 2] = SINSstate_OdoMod.A_x0s[2, 1] * SimpleOperations.AbsoluteVectorValue(SINSstate_OdoMod.OdoSpeed_x0);
                }
            }


            //if (SINSstate.Count % 1000 == 0)
            //{
            //    SimpleOperations.PrintMatrixToFile(KalmanVars.Matrix_A, SimpleData.iMx, SimpleData.iMx);
            //    SimpleOperations.PrintMatrixToFile(KalmanVars.CovarianceMatrixS_m, SimpleData.iMx, SimpleData.iMx);
            //}
        }
Exemplo n.º 18
0
        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;
        }
Exemplo n.º 19
0
        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)
        {
            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;

            SINSstate.flag_UsingCorrection = true;

            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)] = SINSstate.Imitator_GPS_PositionError;
            KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = SINSstate.Imitator_GPS_PositionError;


            KalmanVars.cnt_measures += 2;

            if (SINSstate.flag_iMx_r3_dV3)
            {
                KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r3_dV3] = 1.0;
                KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Altitude - Altitude_CP;
                KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = SINSstate.Imitator_GPS_PositionError;

                KalmanVars.cnt_measures += 1;
            }


            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_OdoMod.Longitude - Longitude_CP) * SINSstate_OdoMod.R_e * Math.Cos(SINSstate_OdoMod.Latitude);
            KalmanVars.Measure[(KalmanVars.cnt_measures + 1)] = (SINSstate_OdoMod.Latitude - Latitude_CP) * SINSstate_OdoMod.R_n;

            KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = SINSstate.Imitator_GPS_PositionError;
            KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 1)] = SINSstate.Imitator_GPS_PositionError;

            KalmanVars.cnt_measures += 2;

            if (SINSstate.flag_Using_iMx_r_odo_3)
            {
                KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_r12_odo + 2] = 1.0;
                KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate_OdoMod.Altitude - Altitude_CP;
                KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = SINSstate.Imitator_GPS_PositionError;

                KalmanVars.cnt_measures += 1;
            }


            //---АЛГЕБРАИЧЕСКАЯ КАЛИБРОВКА---//
            if (SINSstate.flag_using_GoCalibrInCP == true && SINSstate.flag_iMx_kappa_13_ds)
            {
                double lat_dif_calc  = (SINSstate.Latitude - SINSstate.Latitude_Start) * SINSstate.R_n;
                double long_dif_calc = (SINSstate.Longitude - SINSstate.Longitude_Start) * SINSstate.R_e * Math.Cos(SINSstate.Latitude);
                double lat_dif_true  = (Latitude_CP - SINSstate.Latitude_Start) * SimpleOperations.RadiusN(Latitude_CP, Altitude_CP);
                double long_dif_true = (Longitude_CP - SINSstate.Longitude_Start) * SimpleOperations.RadiusE(Latitude_CP, Altitude_CP) * Math.Cos(Latitude_CP);


                double l_calc = Math.Sqrt(Math.Pow(lat_dif_calc, 2) + Math.Pow(long_dif_calc, 2));
                double l_true = Math.Sqrt(Math.Pow(lat_dif_true, 2) + Math.Pow(long_dif_true, 2));

                KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_odo_model + 2] = 1.0;
                if (SINSstate.flag_FeedbackExist)
                {
                    KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = (SINSstate.OdometerData.odometer_left.Value / (1 + SINSstate.ComulativeKappaEst[1]) - l_true) / l_true;
                }
                else
                {
                    KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = (SINSstate.OdometerData.odometer_left.Value - l_true) / l_true;
                }
                KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 0.01;
                KalmanVars.cnt_measures += 1;

                double tmp  = (SINSstate.OdometerData.odometer_left.Value / (1 + SINSstate.ComulativeKappaEst[1]) - l_true) / l_true;
                double tst  = Math.Atan2(long_dif_calc, lat_dif_calc);
                double tst2 = Math.Atan2(long_dif_true, lat_dif_true);

                KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 0] = Math.Tan(SINSstate.Latitude) / SINSstate.R_e;
                KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 4] = -Math.Sin(SINSstate.Heading) * Math.Tan(SINSstate.Pitch);
                KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 5] = -Math.Cos(SINSstate.Heading) * Math.Tan(SINSstate.Pitch);
                KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 6] = 1.0;
                KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_odo_model + 1] = 1.0;
                if (SINSstate.flag_FeedbackExist)
                {
                    KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Heading - SINSstate.ComulativeKappaEst[2] - Math.Atan2(long_dif_true, lat_dif_true);
                }
                else
                {
                    KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Heading - Math.Atan2(long_dif_true, lat_dif_true);
                }
                KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 1.0 * SimpleData.ToRadian_min;
                KalmanVars.cnt_measures += 1;

                double tmp2 = SINSstate.Heading - SINSstate.ComulativeKappaEst[2] - Math.Atan2(long_dif_true, lat_dif_true);

                if (SINSstate.flag_Using_iMx_r_odo_3)
                {
                    KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 4] = -Math.Cos(SINSstate.Heading);
                    KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + 5] = Math.Sin(SINSstate.Heading);
                    KalmanVars.Matrix_H[(KalmanVars.cnt_measures + 0) * iMx + iMx_odo_model + 0] = 1.0;
                    KalmanVars.Measure[(KalmanVars.cnt_measures + 0)] = SINSstate.Altitude - Altitude_CP;
                    KalmanVars.Noize_Z[(KalmanVars.cnt_measures + 0)] = 0.1;
                    KalmanVars.cnt_measures += 1;
                }
            }
        }
Exemplo n.º 20
0
        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);
            //--------------------------------------------------------------------------------------
        }
Exemplo n.º 21
0
        public static void ReadDataForAAStudent()
        {
            StreamWriter Result_DataForAAStudent = new StreamWriter("D://Ботва//Postgraduate//1#_Scientific work//1#_Software//1#_Mine//MovingImitator_Azimut//SINS motion processing_new data//DataForAAStudent_Result.txt");
            StreamReader read = new StreamReader("D://Ботва//Postgraduate//1#_Scientific work//1#_Software//1#_Mine//MovingImitator_Azimut//SINS motion processing_new data//DataForAAStudent.txt");

            SINS_State SINSstate  = new SINS_State();
            string     datastring = "";

            double[] dS_x = new double[3];
            string[] tt;
            char[]   separator = new char[] { ' ' };


            List <double> b = new List <double>();

            double[] a = new double[] { 1, 4, 8 };
            a = a.Where(o => o < 6).ToArray();
            b = b.Where(o => o < 6).ToList();

            var myFile = new StreamReader("D://Ботва//Postgraduate//1#_Scientific work//1#_Software//1#_Mine//MovingImitator_Azimut//SINS motion processing_new data//AzimutB_210530_Other_120814_Autolab_10-31-26_2.dat");


            Matrix A_sx0 = new Matrix(3, 3);
            Matrix A_x0s = new Matrix(3, 3);

            datastring = read.ReadLine();

            SINSstate.Latitude  = 57.9987987 * SimpleData.ToRadian;
            SINSstate.Longitude = 56.268466 * SimpleData.ToRadian;
            SINSstate.Height    = 173.8157;

            datastring = myFile.ReadLine();

            for (int i = 0; i < 200000; i++)
            {
                datastring = myFile.ReadLine();

                //datastring = read.ReadLine();
                tt = datastring.Split(separator, StringSplitOptions.RemoveEmptyEntries);

                SINSstate.Time    = Convert.ToDouble(tt[0]);
                SINSstate.Heading = Convert.ToDouble(tt[1]) * SimpleData.ToRadian;
                SINSstate.Roll    = Convert.ToDouble(tt[2]) * SimpleData.ToRadian;
                SINSstate.Pitch   = Convert.ToDouble(tt[3]) * SimpleData.ToRadian;
                SINSstate.OdometerData.odometer_left.Value = Convert.ToDouble(tt[4]);

                b.Add(SINSstate.Time);
                b.Add(SINSstate.Heading);
                b.Add(SINSstate.Roll);
                b.Add(SINSstate.Pitch);
                b.Add(Convert.ToDouble(tt[5]));
                b.Add(SINSstate.OdometerData.odometer_left.Value);
                b.Add(Convert.ToDouble(tt[6]));
                b.Add(Convert.ToDouble(tt[7]));
                b.Add(Convert.ToDouble(tt[8]));
                b.Add(Convert.ToDouble(tt[9]));
                b.Add(Convert.ToDouble(tt[10]));
                b.Add(Convert.ToDouble(tt[11]));
                b.Add(Convert.ToDouble(tt[12]));
                b.Add(Convert.ToDouble(tt[13]));
                b.Add(SINSstate.Time);
                b.Add(SINSstate.Heading);
                b.Add(SINSstate.Roll);
                b.Add(SINSstate.Pitch);
                b.Add(Convert.ToDouble(tt[5]));
                b.Add(SINSstate.OdometerData.odometer_left.Value);
                b.Add(Convert.ToDouble(tt[6]));
                b.Add(Convert.ToDouble(tt[7]));
                b.Add(Convert.ToDouble(tt[8]));
                b.Add(Convert.ToDouble(tt[9]));
                b.Add(Convert.ToDouble(tt[10]));
                b.Add(Convert.ToDouble(tt[11]));
                b.Add(Convert.ToDouble(tt[12]));
                b.Add(Convert.ToDouble(tt[13]));
                b.Add(SINSstate.Time);
                b.Add(SINSstate.Heading);
                b.Add(SINSstate.Roll);
                b.Add(SINSstate.Pitch);
                b.Add(Convert.ToDouble(tt[5]));
                b.Add(SINSstate.OdometerData.odometer_left.Value);
                b.Add(Convert.ToDouble(tt[6]));
                b.Add(Convert.ToDouble(tt[7]));
                b.Add(Convert.ToDouble(tt[8]));
                b.Add(Convert.ToDouble(tt[9]));
                b.Add(Convert.ToDouble(tt[10]));
                b.Add(Convert.ToDouble(tt[11]));
                b.Add(Convert.ToDouble(tt[12]));
                b.Add(Convert.ToDouble(tt[13]));
                b.Add(SINSstate.Time);
                b.Add(SINSstate.Heading);
                b.Add(SINSstate.Roll);
                b.Add(SINSstate.Pitch);
                b.Add(Convert.ToDouble(tt[5]));
                b.Add(SINSstate.OdometerData.odometer_left.Value);
                b.Add(Convert.ToDouble(tt[6]));
                b.Add(Convert.ToDouble(tt[7]));
                b.Add(Convert.ToDouble(tt[8]));
                b.Add(Convert.ToDouble(tt[9]));
                b.Add(Convert.ToDouble(tt[10]));
                b.Add(Convert.ToDouble(tt[11]));
                b.Add(Convert.ToDouble(tt[12]));
                b.Add(Convert.ToDouble(tt[13]));


                A_sx0 = SimpleOperations.A_sx0(SINSstate);
                A_x0s = A_sx0.Transpose();

                SINSstate.R_e = SimpleOperations.RadiusE(SINSstate.Latitude, SINSstate.Height);
                SINSstate.R_n = SimpleOperations.RadiusN(SINSstate.Latitude, SINSstate.Height);

                SINSstate.OdometerVector[1] = SINSstate.OdometerData.odometer_left.Value - SINSstate.OdometerLeftPrev;
                SimpleOperations.CopyArray(dS_x, A_x0s * SINSstate.OdometerVector);

                SINSstate.Latitude  = SINSstate.Latitude + dS_x[1] / SINSstate.R_n;
                SINSstate.Longitude = SINSstate.Longitude + dS_x[0] / SINSstate.R_e / Math.Cos(SINSstate.Latitude);
                SINSstate.Height    = SINSstate.Height + dS_x[2];

                SINSstate.OdometerLeftPrev = SINSstate.OdometerData.odometer_left.Value;

                string temp = (SINSstate.Latitude * SimpleData.ToDegree).ToString() + " " + (SINSstate.Longitude * SimpleData.ToDegree).ToString() + " " + SINSstate.Height;

                Result_DataForAAStudent.WriteLine(temp);
            }
            Result_DataForAAStudent.Close();

            int           y   = b.Count(o => o < 1000);
            DateTime      t1  = DateTime.Now;
            List <double> bb  = b.Where(o => o * o / 7 < 1000).ToList();
            DateTime      t2  = DateTime.Now;
            List <double> bbb = b.AsParallel().Where(o => o * o / 7 < 1000).ToList();
            DateTime      t3  = DateTime.Now;

            Console.WriteLine((t2 - t1).ToString());
            Console.WriteLine((t3 - t2).ToString());
        }
Exemplo n.º 22
0
        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];
                                        }
                                    }
                                }
                            }
                    }
                }
            }
        }
Exemplo n.º 23
0
        public static void ReadSINSStateFromString(Proc_Help ProcHelp, StreamReader myFile, SINS_State SINSstate, SINS_State SINSstate_OdoMod)
        {
            int t = 0;

            string[] dataArray;

            ProcHelp.datastring = myFile.ReadLine();

            if (ProcHelp.datastring.Contains("Count") || ProcHelp.datastring.Contains("Latitude"))
            {
                ProcHelp.datastring = myFile.ReadLine();
            }

            if (myFile.EndOfStream == false)
            {
                dataArray = ProcHelp.datastring.Split(' ');

                for (int y = 0; y < dataArray.Length; y++)
                {
                    if (dataArray[y] != "")
                    {
                        t++;
                    }
                }
                string[] dataArray2 = new string[t];
                t = 0;

                for (int y = 0; y < dataArray.Length; y++)
                {
                    if (dataArray[y] != "")
                    {
                        dataArray2[t] = dataArray[y];
                        t++;
                    }
                }

                SINSstate.i_global++;
                SINSstate.Count = Convert.ToDouble(dataArray2[0]);

                if (ProcHelp.initCount == false)
                {
                    ProcHelp.initCount = true; SINSstate.initCount = SINSstate.Count - 1;
                }

                SINSstate.Time = (SINSstate.Count - SINSstate.initCount) * Math.Abs(SINSstate.timeStep);

                SINSstate.Input_nMode = Convert.ToInt32(dataArray2[22]);

                SINSstate.F_z[1] = Convert.ToDouble(dataArray2[1]);
                SINSstate.F_z[2] = Convert.ToDouble(dataArray2[2]);
                SINSstate.F_z[0] = Convert.ToDouble(dataArray2[3]);

                SINSstate.W_z[1] = Convert.ToDouble(dataArray2[4]);
                SINSstate.W_z[2] = Convert.ToDouble(dataArray2[5]);
                SINSstate.W_z[0] = Convert.ToDouble(dataArray2[6]);

                // --- Поворот показаний датчиков на заданные углы несоосности БИНС и динамических осей объекта --- //
                {
                    double[] fz = new double[3], Wz = new double[3];

                    fz[0] = SINSstate.F_z[0] - SINSstate.alpha_kappa_3 * SINSstate.F_z[1];
                    fz[1] = SINSstate.F_z[1] + SINSstate.alpha_kappa_3 * SINSstate.F_z[0] - SINSstate.alpha_kappa_1 * SINSstate.F_z[2];
                    fz[2] = SINSstate.F_z[2] + SINSstate.alpha_kappa_1 * SINSstate.F_z[1];

                    Wz[0] = SINSstate.W_z[0] - SINSstate.alpha_kappa_3 * SINSstate.W_z[1];
                    Wz[1] = SINSstate.W_z[1] + SINSstate.alpha_kappa_3 * SINSstate.W_z[0] - SINSstate.alpha_kappa_1 * SINSstate.W_z[2];
                    Wz[2] = SINSstate.W_z[2] + SINSstate.alpha_kappa_1 * SINSstate.W_z[1];

                    SimpleOperations.CopyArray(SINSstate.F_z, fz);
                    SimpleOperations.CopyArray(SINSstate.W_z, Wz);
                }

                /*Поворачиваем на моделируюемую ошибку*/
                if (SINSstate.initError_kappa_1 != 0 || SINSstate.initError_kappa_3 != 0)
                {
                    double[] fz = new double[3], Wz = new double[3];

                    fz[0] = SINSstate.F_z[0] + SINSstate.initError_kappa_3 * SINSstate.F_z[1];
                    fz[1] = SINSstate.F_z[1] - SINSstate.initError_kappa_3 * SINSstate.F_z[0] + SINSstate.initError_kappa_1 * SINSstate.F_z[2];
                    fz[2] = SINSstate.F_z[2] - SINSstate.initError_kappa_1 * SINSstate.F_z[1];

                    Wz[0] = SINSstate.W_z[0] + SINSstate.initError_kappa_3 * SINSstate.W_z[1];
                    Wz[1] = SINSstate.W_z[1] - SINSstate.initError_kappa_3 * SINSstate.W_z[0] + SINSstate.initError_kappa_1 * SINSstate.W_z[2];
                    Wz[2] = SINSstate.W_z[2] - SINSstate.initError_kappa_1 * SINSstate.W_z[1];

                    SimpleOperations.CopyArray(SINSstate.F_z, fz);
                    SimpleOperations.CopyArray(SINSstate.W_z, Wz);
                }


                //---Запоминаем координаты предыдущей контрольной точки
                if (SINSstate.GPS_Data.gps_Latitude.isReady == 1)
                {
                    SINSstate.GPS_Data.gps_Latitude_prev.Value  = SINSstate.GPS_Data.gps_Latitude.Value;
                    SINSstate.GPS_Data.gps_Longitude_prev.Value = SINSstate.GPS_Data.gps_Longitude.Value;
                    SINSstate.GPS_Data.gps_Altitude_prev.Value  = SINSstate.GPS_Data.gps_Altitude.Value;

                    SINSstate.OdometerData.odometer_left_prev.Value  = SINSstate.OdometerData.odometer_left.Value;
                    SINSstate.OdometerData.odometer_right_prev.Value = SINSstate.OdometerData.odometer_right.Value;
                }

                // --- Заполняем структуру показний GPS, если они имеются
                // --- Предполагается, что GPS записываются в WGS84
                SINSstate.GPS_Data.gps_Latitude.Value    = Convert.ToDouble(dataArray2[7]);
                SINSstate.GPS_Data.gps_Latitude.isReady  = Convert.ToInt32(dataArray2[8]);
                SINSstate.GPS_Data.gps_Longitude.Value   = Convert.ToDouble(dataArray2[9]);
                SINSstate.GPS_Data.gps_Longitude.isReady = Convert.ToInt32(dataArray2[10]);
                SINSstate.GPS_Data.gps_Altitude.Value    = Convert.ToDouble(dataArray2[11]);
                SINSstate.GPS_Data.gps_Altitude.isReady  = Convert.ToInt32(dataArray2[12]);

                SINSstate.GPS_Data.gps_Vn.Value   = Convert.ToDouble(dataArray2[13]);
                SINSstate.GPS_Data.gps_Vn.isReady = Convert.ToInt32(dataArray2[14]);
                SINSstate.GPS_Data.gps_Ve.Value   = Convert.ToDouble(dataArray2[15]);
                SINSstate.GPS_Data.gps_Ve.isReady = Convert.ToInt32(dataArray2[16]);

                if (SINSstate.GPS_Data.gps_Latitude.isReady == 2)
                {
                    SINSstate.GPS_Data.gps_Latitude.isReady = 1;
                }
                if (SINSstate.GPS_Data.gps_Longitude.isReady == 2)
                {
                    SINSstate.GPS_Data.gps_Longitude.isReady = 1;
                }
                if (SINSstate.GPS_Data.gps_Altitude.isReady == 2)
                {
                    SINSstate.GPS_Data.gps_Altitude.isReady = 1;
                }

                if (SINSstate.GPS_Data.gps_Vn.isReady == 2)
                {
                    SINSstate.GPS_Data.gps_Vn.isReady = 1;
                }
                if (SINSstate.GPS_Data.gps_Ve.isReady == 2)
                {
                    SINSstate.GPS_Data.gps_Ve.isReady = 1;
                }


                //--- Если режим не Движение, то игнорируем показания СНС ---
                if (SINSstate.Input_nMode != 16)
                {
                    SINSstate.GPS_Data.gps_Latitude.isReady  = 0;
                    SINSstate.GPS_Data.gps_Longitude.isReady = 0;
                    SINSstate.GPS_Data.gps_Altitude.isReady  = 0;
                    SINSstate.GPS_Data.gps_Vn.isReady        = 0;
                    SINSstate.GPS_Data.gps_Ve.isReady        = 0;
                }

                if (SINSstate.GPS_Data.gps_Latitude.isReady == 1)
                {
                    SINSstate.GPS_CounterOfPoints++;
                }

                if (SINSstate.GPS_Data.gps_Latitude.isReady == 1)
                {
                    SINSstate.GPS_Data.gps_Vn.Value_prev = SINSstate.GPS_Data.gps_Vn.Value;
                    SINSstate.GPS_Data.gps_Ve.Value_prev = SINSstate.GPS_Data.gps_Ve.Value;
                }


                SINSstate.FLG_Stop = Convert.ToInt32(dataArray2[17]);

                // --- Обрабатываем ситуацию, когда одометр установлен неправильно и не введен масштабный коэффициент
                double Odo_SINS_VS_distance_coefficient = SINSstate.OdometerData.odometer_left.Value / SINSstate.distance_by_SINS;
                if (SINSstate.distance_by_SINS < 110.0 && Math.Abs(Odo_SINS_VS_distance_coefficient) > 0.3)
                {
                    if (SINSstate.distance_by_SINS > 5.0 && Odo_SINS_VS_distance_coefficient < 0)
                    {
                        SINSstate.OdometerData_Sign = -1;
                        SINSstate.OdometerLeftPrev *= SINSstate.OdometerData_Sign;
                    }
                    if (SINSstate.distance_by_SINS > 100.0 && SINSstate.OdometerData_RoughtScale_flag == 0 && Math.Abs(1 - Odo_SINS_VS_distance_coefficient) > 0.5)
                    {
                        SINSstate.OdometerData_RoughtScale      = Odo_SINS_VS_distance_coefficient;
                        SINSstate.OdometerLeftPrev             /= SINSstate.OdometerData_RoughtScale;
                        SINSstate.OdometerData_RoughtScale_flag = 1;
                    }
                }

                // --- Считываем показания одометров
                SINSstate.OdometerData.odometer_left.Value    = (Convert.ToDouble(dataArray2[18]) - SINSstate.OdometerData.odometer_left.Value_Correction) * SINSstate.OdometerData_Sign / SINSstate.OdometerData_RoughtScale;
                SINSstate.OdometerData.odometer_left.isReady  = Convert.ToInt32(dataArray2[19]);
                SINSstate.OdometerData.odometer_right.Value   = (Convert.ToDouble(dataArray2[20]) - SINSstate.OdometerData.odometer_left.Value_Correction) * SINSstate.OdometerData_Sign / SINSstate.OdometerData_RoughtScale;
                SINSstate.OdometerData.odometer_right.isReady = Convert.ToInt32(dataArray2[21]);


                // --- ЗАГЛУШКА для файла со стоянкой, поскольку здесь всегда для одометра isReady = 0 ---
                if (SINSstate.DataInFileName.Contains("GCEF_format_Azimuth10B_450612_48H_17-Feb-2016") == true && SINSstate.Count % 5 == 0)
                {
                    SINSstate.OdometerData.odometer_left.isReady = 1;
                }
                // --- ЗАГЛУШКА ---


                if (SINSstate.flag_notUseOdometer == true)
                {
                    SINSstate.OdometerData.odometer_left.isReady = 2;
                }

                if (SINSstate.OdometerData.odometer_left.isReady == 1)
                {
                    // Проверка на случай, когда одометр начинает имзерять растояние не с нуля
                    if (SINSstate.firstNotNullOdoValue_flg == 0 && SINSstate.OdometerData.odometer_left.isReady == 1)
                    {
                        SINSstate.firstNotNullOdoValue_flg = 1;
                        if (SINSstate.OdometerData.odometer_left.Value > 1.0)
                        {
                            SINSstate.OdometerData.odometer_left.Value_Correction = SINSstate.OdometerData.odometer_left.Value;
                            SINSstate.OdometerData.odometer_left.Value           -= SINSstate.OdometerData.odometer_left.Value_Correction;
                        }
                    }

                    SINSstate.OdometerData.odometer_left.Value_prev = SINSstate.OdometerData.odometer_left.Value;

                    SINSstate.OdoLimitMeasuresNum_Count++;
                    if (SINSstate.OdoLimitMeasuresNum_Count < SINSstate.OdoLimitMeasuresNum)
                    {
                        SINSstate.OdometerData.odometer_left.isReady  = 2;
                        SINSstate.OdometerData.odometer_right.isReady = 2;
                    }
                    else
                    {
                        SINSstate.OdoLimitMeasuresNum_Count = 0;
                    }
                }

                // --- Делаем поправку на введенное значение ошибки масштаба одометра
                SINSstate.OdometerData.odometer_left.Value  /= (1.0 + SINSstate.alpha_scaleError - SINSstate.initError_scaleError);
                SINSstate.OdometerData.odometer_right.Value /= (1.0 + SINSstate.alpha_scaleError - SINSstate.initError_scaleError);


                // --- Фильтруем всплески в показаниях одометра
                double CurOdoSpeed_s1 = (SINSstate.OdometerData.odometer_left.Value - SINSstate.OdometerLeftPrev) / (SINSstate.OdoTimeStepCount + 1) / SINSstate.timeStep;
                if (SINSstate.OdometerData.odometer_left.isReady == 1 &&
                    SINSstate.OdoSpeed_s[1] > 1.0 &&
                    (CurOdoSpeed_s1 - SINSstate.OdoSpeed_s[1]) > 10.0 &&
                    CurOdoSpeed_s1 > 1.5 * SINSstate.OdoSpeed_s[1])
                {
                    double timeMark = SINSstate.Time + SINSstate.Time_Alignment;
                    SINSstate.OdometerLeftPrev = SINSstate.OdometerData.odometer_left.Value - SINSstate.OdometerVector[1];
                    SINSstate.OdometerData.odometer_left.isReady  = 2;
                    SINSstate.OdometerData.odometer_right.isReady = 2;
                }


                // --- Сохраняем оригинальные считанные значение, если проставлен флаг вывода в GRTV ---//
                if (SINSstate.flag_GRTV_output)
                {
                    for (int y = 0; y < 3; y++)
                    {
                        SINSstate.F_z_orig[y] = SINSstate.F_z[y] / 9.81;
                        SINSstate.W_z_orig[y] = SINSstate.W_z[y];
                    }

                    SINSstate.OdometerData.odometer_left.Value_orig   = Convert.ToDouble(dataArray2[18]);
                    SINSstate.OdometerData.odometer_left.isReady_orig = Convert.ToInt32(dataArray2[19]);
                    if (SINSstate.OdometerData.odometer_left.isReady_orig != 1)
                    {
                        SINSstate.OdometerData.odometer_left.isReady_orig = 0;
                        SINSstate.OdometerData.odometer_left.Value_orig   = SINSstate.OdometerData.odometer_left.Value_prev;
                    }

                    SINSstate.GPS_Data.gps_Vn.Value_orig   = Convert.ToDouble(dataArray2[13]);
                    SINSstate.GPS_Data.gps_Vn.isReady_orig = Convert.ToInt32(dataArray2[14]);
                    if (SINSstate.GPS_Data.gps_Vn.isReady_orig != 1)
                    {
                        SINSstate.GPS_Data.gps_Vn.isReady_orig = 0;
                        SINSstate.GPS_Data.gps_Vn.Value_orig   = SINSstate.GPS_Data.gps_Vn.Value_prev;
                    }
                    SINSstate.GPS_Data.gps_Ve.Value_orig   = Convert.ToDouble(dataArray2[15]);
                    SINSstate.GPS_Data.gps_Ve.isReady_orig = Convert.ToInt32(dataArray2[16]);
                    if (SINSstate.GPS_Data.gps_Ve.isReady_orig != 1)
                    {
                        SINSstate.GPS_Data.gps_Ve.isReady_orig = 0;
                        SINSstate.GPS_Data.gps_Ve.Value_orig   = SINSstate.GPS_Data.gps_Ve.Value_prev;
                    }



                    SINSstate.GPS_Data.gps_Latitude.Value_orig  = Convert.ToDouble(dataArray2[7]);
                    SINSstate.GPS_Data.gps_Longitude.Value_orig = Convert.ToDouble(dataArray2[9]);
                    SINSstate.GPS_Data.gps_Altitude.Value_orig  = Convert.ToDouble(dataArray2[11]);

                    if (SINSstate.GPS_Data.gps_Latitude.isReady != 1)
                    {
                        SINSstate.GPS_Data.gps_Latitude.Value_orig  = SINSstate.GPS_Data.gps_Latitude_prev.Value;
                        SINSstate.GPS_Data.gps_Longitude.Value_orig = SINSstate.GPS_Data.gps_Longitude_prev.Value;
                        SINSstate.GPS_Data.gps_Altitude.Value_orig  = SINSstate.GPS_Data.gps_Altitude_prev.Value;
                    }

                    SINSstate.GPS_Data.gps_Latitude.isReady_orig  = Convert.ToInt32(dataArray2[8]);
                    SINSstate.GPS_Data.gps_Longitude.isReady_orig = Convert.ToInt32(dataArray2[10]);
                    SINSstate.GPS_Data.gps_Altitude.isReady_orig  = Convert.ToInt32(dataArray2[12]);

                    if (SINSstate.GPS_Data.gps_Latitude.isReady_orig != 1)
                    {
                        SINSstate.GPS_Data.gps_Latitude.isReady_orig = 0;
                    }
                    if (SINSstate.GPS_Data.gps_Longitude.isReady_orig != 1)
                    {
                        SINSstate.GPS_Data.gps_Longitude.isReady_orig = 0;
                    }
                    if (SINSstate.GPS_Data.gps_Altitude.isReady_orig != 1)
                    {
                        SINSstate.GPS_Data.gps_Altitude.isReady_orig = 0;
                    }
                }

                SINSstate.GPS_Data.gps_Latitude_pnppk_sol.Value  = Convert.ToDouble(dataArray2[25]);
                SINSstate.GPS_Data.gps_Longitude_pnppk_sol.Value = Convert.ToDouble(dataArray2[26]);
                SINSstate.GPS_Data.gps_Altitude_pnppk_sol.Value  = Convert.ToDouble(dataArray2[27]);


                // --- Проставляем флаги, что перваястрока из файла с данными считана
                if (SINSstate.firstLineRead == false)
                {
                    SINSstate.Roll_prev = SINSstate.Roll;
                    SimpleOperations.CopyArray(SINSstate.F_z_prev, SINSstate.F_z);
                    SimpleOperations.CopyArray(SINSstate.W_z_prev, SINSstate.W_z);
                    SINSstate.OdometerStartValue = SINSstate.OdometerData.odometer_left.Value;
                    SINSstate.OdometerLeftPrev   = SINSstate.OdometerData.odometer_left.Value;
                    SINSstate.OdometerRightPrev  = SINSstate.OdometerData.odometer_right.Value;
                    SINSstate.firstLineRead      = true;
                }
            }
        }