コード例 #1
0
ファイル: FINS.cs プロジェクト: i-svetlana/FINS
        double[] v;                                    //три проэкции вектора абсолютной скорости ПО на оси СИСК {Х0} (м/с)

        public FINS(ConsumerParameters inputPars)
        {
            input = new ConsumerParameters(inputPars);
            T     = 0;
            Omega = new double[3];
            q     = MathExpansion.a / (Math.Sqrt(1 - MathExpansion.epsilon * MathExpansion.epsilon
                                                 * Math.Sin(input.Phi) * Math.Sin(input.Phi)));
            v_z = MathExpansion.om * q * Math.Cos(input.Phi);
            r   = (q + input.H) * Math.Cos(input.Phi);
            n0  = Quaternion.Proud(new Quaternion(Math.Cos(input.Lambda / 2), Math.Sin(input.Lambda / 2), 0, 0),
                                   new Quaternion(Math.Cos(input.Phi / 2), 0, 0, (-1) * Math.Sin(input.Phi / 2)));

            v    = new double[3];
            v[0] = input.V_n;
            v[1] = input.V_h;
            v[2] = input.V_e + v_z;

            V = Quaternion.InverseBasisTransform(n0, v);

            R_vec    = new double[3];
            R_vec[0] = ((1 - MathExpansion.epsilon * MathExpansion.epsilon) * q + input.H) * Math.Sin(input.Phi);
            R_vec[1] = r * Math.Cos(input.Lambda);
            R_vec[2] = r * Math.Sin(input.Lambda);

            L = Quaternion.Proud(new Quaternion(Math.Cos(input.Psi / 2), 0, -Math.Sin(input.Psi / 2), 0),
                                 new Quaternion(Math.Cos(input.Theta / 2), 0, 0, Math.Sin(input.Theta / 2)));
            L = Quaternion.Proud(L, new Quaternion(Math.Cos(input.Gama / 2), Math.Sin(input.Gama / 2), 0, 0));
        }
コード例 #2
0
 public ConsumerParameters(ConsumerParameters cp)
 {
     Phi    = cp.Phi;
     H      = cp.H;
     Lambda = cp.Lambda;
     V_n    = cp.V_n;
     V_h    = cp.V_h;
     V_e    = cp.V_e;
     Psi    = cp.Psi;
     Theta  = cp.Theta;
     Gama   = cp.Gama;
 }
コード例 #3
0
ファイル: FINS.cs プロジェクト: i-svetlana/FINS
        public void Go(double[] nTheta, double[] nV, double nT, double tEx, List <ConsumerParameters> consParms)
        {
            double[] g = GravitationalAcceleratioVector(R_vec);

            double[] vScecified = MathExpansion.Sum(nTheta,
                                                    MathExpansion.Multiplication(1 / 2, MathExpansion.VectorProud(nTheta, nV)));

            double[] v1 = new double[3];            //3-х мерный вектор абсолютной скорости с предыдущего такта
            for (int i = 0; i < v1.Length; i++)
            {
                v1[i] = V[i];
            }

            double[] nTheta1 = MathExpansion.Multiplication(nT, Omega);
            Omega = MathExpansion.Division(nTheta, nT);

            Quaternion s = Quaternion.Proud(n0, L);

            double[] deltaV = MathExpansion.Sum(Quaternion.InverseBasisTransform(s, vScecified),
                                                MathExpansion.Multiplication(nT, g));
            V = MathExpansion.Sum(V, deltaV);

            R_vec = MathExpansion.Sum(R_vec, MathExpansion.Multiplication(nT,
                                                                          MathExpansion.Multiplication(1 / 2, MathExpansion.Sum(v1, V))));

            double dl_0 = 1 - MathExpansion.Module(nTheta) * MathExpansion.Module(nTheta) / 8;

            double[] dl = MathExpansion.Multiplication(1 / 2, nTheta);
            dl = MathExpansion.Sum(dl, MathExpansion.Multiplication(-MathExpansion.Module(nTheta)
                                                                    * MathExpansion.Module(nTheta) / 48, nTheta));
            dl = MathExpansion.Sum(dl, MathExpansion.Multiplication(-1 / 24, MathExpansion.VectorProud(nTheta, nTheta1)));
            Quaternion deltaL = new Quaternion(dl_0, dl);

            deltaL.Multiplication(1 / Math.Sqrt(dl_0 * dl_0 + MathExpansion.ScalarProud(dl, dl)));
            L = Quaternion.Proud(L, deltaL);

            T += nT;

            ConsumerParameters            result = new ConsumerParameters();
            ConsumerParametersCalculation cpCalc = new ConsumerParametersCalculation();

            result = cpCalc.Calculate(T, R_vec, V, n0, L);
            consParms.Add(result);

            if (T < tEx)
            {
                Go(nTheta, nV, nT, tEx, consParms);
            }
        }
コード例 #4
0
ファイル: Program.cs プロジェクト: i-svetlana/FINS
        static void Main()
        {
            List <ConsumerParameters> consumerParameters = new List <ConsumerParameters>();

            double phi0, h0, lambda0, vn0, vh0, ve0, psi0, theta0, gama0;

            #region Ввод начальных данных
            do
            {
                Console.WriteLine("Initial value of latitude:");
            } while (!Double.TryParse(Console.ReadLine(), out phi0));

            do
            {
                Console.WriteLine("Initial value of longitude:");
            } while (!Double.TryParse(Console.ReadLine(), out lambda0));

            do
            {
                Console.WriteLine("Initial value of height:");
            } while (!Double.TryParse(Console.ReadLine(), out h0));

            do
            {
                Console.WriteLine("Initial value of north velocity component:");
            } while (!Double.TryParse(Console.ReadLine(), out vn0));

            do
            {
                Console.WriteLine("Initial value of vertical velocity component:");
            } while (!Double.TryParse(Console.ReadLine(), out vh0));

            do
            {
                Console.WriteLine("Initial value of east velocity component:");
            } while (!Double.TryParse(Console.ReadLine(), out ve0));

            do
            {
                Console.WriteLine("Initial value of course angle:");
            } while (!Double.TryParse(Console.ReadLine(), out psi0));

            do
            {
                Console.WriteLine("Initial value of pitch angle:");
            } while (!Double.TryParse(Console.ReadLine(), out theta0));

            do
            {
                Console.WriteLine("Initial value of roll angle:");
            } while (!Double.TryParse(Console.ReadLine(), out gama0));
            #endregion

            ConsumerParameters initial = new ConsumerParameters(phi0, h0, lambda0, vn0, vh0, ve0, psi0, theta0, gama0);
            consumerParameters.Add(initial);

            FINS fins = new FINS(initial);

            double[] nTheta = new double[3];
            double[] nV = new double[3];
            double   dt, tEx;

            #region Ввод начальных данных для циклической части алгоритма
loopTheta:
            Console.WriteLine("Increment of the angle of apparent rotation (like \"x y z\"):");
            string   s    = Console.ReadLine();
            string[] prms = s.Split(' ');
            if (nTheta.Length != prms.Length)
            {
                Console.WriteLine("Incorrect data! Try again.");
                goto loopTheta;
            }
            for (int i = 0; i < nTheta.Length; i++)
            {
                if (!Double.TryParse(prms[i], out nTheta[i]))
                {
                    Console.WriteLine("Incorrect data! Try again.");
                    goto loopTheta;
                }
            }

loopV:
            Console.WriteLine("Increment of apparent velocity vector (like \"x y z\"):");
            s    = Console.ReadLine();
            prms = s.Split(' ');
            if (nV.Length != prms.Length)
            {
                Console.WriteLine("Incorrect data! Try again.");
                goto loopV;
            }
            for (int i = 0; i < nV.Length; i++)
            {
                if (!Double.TryParse(prms[i], out nV[i]))
                {
                    Console.WriteLine("Incorrect data! Try again.");
                    goto loopV;
                }
            }

            do
            {
                Console.WriteLine("Time between updates:");
            } while (!Double.TryParse(Console.ReadLine(), out dt));

            do
            {
                Console.WriteLine("Exit time:");
            } while (!Double.TryParse(Console.ReadLine(), out tEx));
            #endregion

            fins.Go(nTheta, nV, dt, tEx, consumerParameters);

            #region Вывод полученных данных в файл
            using (StreamWriter sw = new StreamWriter("D:\\result.txt"))
            {
                sw.WriteLine("Phi:");
                foreach (ConsumerParameters cp in consumerParameters)
                {
                    sw.WriteLine(cp.Phi);
                }
                sw.WriteLine();

                sw.WriteLine("Lambda:");
                foreach (ConsumerParameters cp in consumerParameters)
                {
                    sw.WriteLine(cp.Lambda);
                }
                sw.WriteLine();

                sw.WriteLine("V_n:");
                foreach (ConsumerParameters cp in consumerParameters)
                {
                    sw.WriteLine(cp.V_n);
                }
                sw.WriteLine();

                sw.WriteLine("V_h:");
                foreach (ConsumerParameters cp in consumerParameters)
                {
                    sw.WriteLine(cp.V_h);
                }
                sw.WriteLine();

                sw.WriteLine("V_e:");
                foreach (ConsumerParameters cp in consumerParameters)
                {
                    sw.WriteLine(cp.V_e);
                }
                sw.WriteLine();

                sw.WriteLine("Psi:");
                foreach (ConsumerParameters cp in consumerParameters)
                {
                    sw.WriteLine(cp.Psi);
                }
                sw.WriteLine();

                sw.WriteLine("Theta:");
                foreach (ConsumerParameters cp in consumerParameters)
                {
                    sw.WriteLine(cp.Theta);
                }
                sw.WriteLine();

                sw.WriteLine("Gama:");
                foreach (ConsumerParameters cp in consumerParameters)
                {
                    sw.WriteLine(cp.Gama);
                }
                sw.WriteLine();
            }
            #endregion

            Console.ReadLine();
        }
コード例 #5
0
 public ConsumerParametersCalculation()
 {
     parameters = new ConsumerParameters();
 }