Exemple #1
0
        public FARRungeKutta4(double endTime, double dt, FARMatrix eqns, double[] initCond)
        {
//            b.Add(0.5f, 0, 1);
//            b.Add(0.5f, 1, 2);
//            b.Add(1, 2, 3);
            this.endTime        = endTime;
            this.dt             = dt;
            this.stateEquations = eqns;
            this.initCond       = initCond;
            soln = new double[initCond.Length, (int)Math.Ceiling(endTime / dt)];
            time = new double[(int)Math.Ceiling(endTime / dt)];
        }
        private void RunTransientSimLongitudinal(double endTime, double initDt, double[] InitCond)
        {
            FARMatrix A = new FARMatrix(4, 4);
            FARMatrix B = new FARMatrix(1, 4);
            MonoBehaviour.print("Init Dump...");

            A.PrintToConsole();

            MonoBehaviour.print("Deriv Dump...");
            int i = 0;
            int j = 0;
            int num = 0;
            foreach (double f in MOI_stabDerivs)
            {
                if (num < 3 || num >= 15)
                {
                    num++;              //Avoid Ix, Iy, Iz
                    continue;
                }
                else
                    num++;
                MonoBehaviour.print(i + "," + j);
                if (i <= 2)
                    if (num == 10)
                        A.Add(f + u0, i, j);
                    else
                        A.Add(f, i, j);
                else
                    B.Add(f, 0, j);
                if (j < 2)
                    j++;
                else
                {
                    j = 0;
                    i++;
                }

            }
            A.Add(-9.81f, 3, 1);
            A.Add(1, 2, 3);

            A.PrintToConsole();                //We should have an array that looks like this:
            B.PrintToConsole();

            /*             i --------------->
             *       j  [ Z w , Z u , Z q ,  0 ]
             *       |  [ X w , X u , X q , -g ]
             *       |  [ M w , M u , M q ,  0 ]
             *      \ / [  0  ,  0  ,  1  ,  0 ]
             *       V                              //And one that looks like this:
             *
             *          [ Z e ]
             *          [ X e ]
             *          [ M e ]
             *          [  0  ]
             *
             *
             */
            FARRungeKutta4 transSolve = new FARRungeKutta4(endTime, initDt, A, InitCond);
            MonoBehaviour.print("Runge-Kutta 4 init...");
            transSolve.Solve();
            MonoBehaviour.print("Solved...");
            graph.Clear();
            double[] yVal = transSolve.GetSolution(0);
            MonoBehaviour.print("Got 0");
            for (int k = 0; k < yVal.Length; k++)
                if (yVal[k] > 50)
                    yVal[k] = 50;
                else if (yVal[k] < -50)
                    yVal[k] = -50;
            graph.AddLine("w", transSolve.time, yVal, Color.green);
            yVal = transSolve.GetSolution(1);
            MonoBehaviour.print("Got 1");
            for (int k = 0; k < yVal.Length; k++)
                if (yVal[k] > 50)
                    yVal[k] = 50;
                else if (yVal[k] < -50)
                    yVal[k] = -50;
            graph.AddLine("u", transSolve.time, yVal, Color.yellow);
            yVal = transSolve.GetSolution(2);
            for (int k = 0; k < yVal.Length; k++)
            {
                yVal[k] = yVal[k] * 180 / Mathf.PI;
                if (yVal[k] > 50)
                    yVal[k] = 50;
                else if (yVal[k] < -50)
                    yVal[k] = -50;
            }
            MonoBehaviour.print("Got 2");
            graph.AddLine("q", transSolve.time, yVal, Color.blue);
            yVal = transSolve.GetSolution(3);
            for (int k = 0; k < yVal.Length; k++)
            {
                yVal[k] = yVal[k] * 180 / Mathf.PI;
                if (yVal[k] > 50)
                    yVal[k] = 50;
                else if (yVal[k] < -50)
                    yVal[k] = -50;
            }
            MonoBehaviour.print("Got 3");
            graph.AddLine("θ", transSolve.time, yVal, Color.cyan);
            graph.SetBoundaries(0, endTime, -10, 10);
            graph.SetGridScaleUsingValues(1, 5);
            graph.horizontalLabel = "time";
            graph.verticalLabel = "value";
            graph.Update();
        }
 public FARRungeKutta4(double endTime, double dt, FARMatrix eqns, double[] initCond)
 {
     //            b.Add(0.5f, 0, 1);
     //            b.Add(0.5f, 1, 2);
     //            b.Add(1, 2, 3);
     this.endTime = endTime;
     this.dt = dt;
     this.stateEquations = eqns;
     this.initCond = initCond;
     soln = new double[initCond.Length, (int)Math.Ceiling(endTime / dt)];
     time = new double[(int)Math.Ceiling(endTime / dt)];
 }
        private void RunTransientSimLateral(double endTime, double initDt, double[] InitCond)
        {
            FARMatrix A = new FARMatrix(4, 4);
            FARMatrix B = new FARMatrix(1, 4);
            MonoBehaviour.print("Init Dump...");

            A.PrintToConsole();

            MonoBehaviour.print("Deriv Dump...");
            int i = 0;
            int j = 0;
            int num = 0;
            double[] Derivs = new double[27];

            MOI_stabDerivs.CopyTo(Derivs, 0);

            Derivs[15] = Derivs[15] / u0;
            Derivs[18] = Derivs[18] / u0;
            Derivs[21] = Derivs[21] / u0 - 1;

            double Lb = Derivs[16] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Nb = Derivs[17] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            double Lp = Derivs[19] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Np = Derivs[20] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            double Lr = Derivs[22] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));
            double Nr = Derivs[23] / (1 - Derivs[26] * Derivs[26] / (Derivs[0] * Derivs[2]));

            Derivs[16] = Lb + Derivs[26] / Derivs[0] * Nb;
            Derivs[17] = Nb + Derivs[26] / Derivs[2] * Lb;

            Derivs[19] = Lp + Derivs[26] / Derivs[0] * Np;
            Derivs[20] = Np + Derivs[26] / Derivs[2] * Lp;

            Derivs[22] = Lr + Derivs[26] / Derivs[0] * Nr;
            Derivs[23] = Nr + Derivs[26] / Derivs[2] * Lr;

            foreach (double f in Derivs)
            {
                if (num < 15)
                {
                    num++;              //Avoid Ix, Iy, Iz and long derivs
                    continue;
                }
                else
                    num++;
                MonoBehaviour.print(i + "," + j);
                if (i <= 2)
                    A.Add(f, i, j);

                if (j < 2)
                    j++;
                else
                {
                    j = 0;
                    i++;
                }

            }
            A.Add(9.81 * Math.Cos(stable_AoA * FARMathUtil.deg2rad) / u0, 3, 0);
            A.Add(1, 1, 3);

            A.PrintToConsole();                //We should have an array that looks like this:
            B.PrintToConsole();

            /*             i --------------->
             *       j  [ Yb / u0 , Yp / u0 , -(1 - Yr/ u0) ,  g Cos(θ0) / u0 ]
             *       |  [   Lb    ,    Lp   ,      Lr       ,          0          ]
             *       |  [   Nb    ,    Np   ,      Nr       ,          0          ]
             *      \ / [    0    ,    1    ,      0        ,          0          ]
             *       V                              //And one that looks like this:
             *
             *          [ Z e ]
             *          [ X e ]
             *          [ M e ]
             *          [  0  ]
             *
             *
             */
            FARRungeKutta4 transSolve = new FARRungeKutta4(endTime, initDt, A, InitCond);
            MonoBehaviour.print("Runge-Kutta 4 init...");
            transSolve.Solve();
            MonoBehaviour.print("Solved...");
            graph.Clear();
            double[] yVal = transSolve.GetSolution(0);
            MonoBehaviour.print("Got 0");
            for (int k = 0; k < yVal.Length; k++)
            {
                yVal[k] = yVal[k] * 180 / Mathf.PI;
                if (yVal[k] > 50)
                    yVal[k] = 50;
                else if (yVal[k] < -50)
                    yVal[k] = -50;
            }
            graph.AddLine("β", transSolve.time, yVal, Color.green);
            yVal = transSolve.GetSolution(1);
            MonoBehaviour.print("Got 1");
            for (int k = 0; k < yVal.Length; k++)
            {
                yVal[k] = yVal[k] * 180 / Mathf.PI;
                if (yVal[k] > 50)
                    yVal[k] = 50;
                else if (yVal[k] < -50)
                    yVal[k] = -50;
            }
            graph.AddLine("p", transSolve.time, yVal, Color.yellow);
            yVal = transSolve.GetSolution(2);
            for (int k = 0; k < yVal.Length; k++)
            {
                yVal[k] = yVal[k] * 180 / Mathf.PI;
                if (yVal[k] > 50)
                    yVal[k] = 50;
                else if (yVal[k] < -50)
                    yVal[k] = -50;
            }
            MonoBehaviour.print("Got 2");
            graph.AddLine("r", transSolve.time, yVal, Color.blue);
            yVal = transSolve.GetSolution(3);
            for (int k = 0; k < yVal.Length; k++)
            {
                yVal[k] = yVal[k] * 180 / Mathf.PI;
                if (yVal[k] > 50)
                    yVal[k] = 50;
                else if (yVal[k] < -50)
                    yVal[k] = -50;
            }
            MonoBehaviour.print("Got 3");
            graph.AddLine("φ", transSolve.time, yVal, Color.cyan);
            graph.SetBoundaries(0, endTime, -10, 10);
            graph.SetGridScaleUsingValues(1, 5);
            graph.horizontalLabel = "time";
            graph.verticalLabel = "value";
            graph.Update();
        }