Ejemplo n.º 1
0
        public Matrix <double> p0()
        {
            double          th = 0.000000001;
            Matrix <double> L  = Lambda;

            L.CoerceZero(th);
            Vector <double> Lcs             = L.ColumnSums();
            int             nonZeroLcsCount = 0;
            int             lastNonZero     = -1;

            for (int i = 0; i < Lcs.Count(); i++)
            {
                if (Math.Abs(Lcs[i]) < th)
                {
                    L[i, i] = 1.0;
                }
                else
                {
                    nonZeroLcsCount++;
                    lastNonZero = i;
                }
            }
            if (nonZeroLcsCount == 1)
            {
                return(VBuilder.Dense(N, i => i == lastNonZero ? 1 : 0).ToColumnMatrix());
            }
            else
            {
                Matrix <double> A = MBuilder.Dense(1, N, 1.0);
                A = A.Stack(Lambda.Transpose());
                Vector <double> B      = VBuilder.Dense(N + 1, i => i > 0 ? 0 : 1);
                Vector <double> result = A.Solve(B);
                return(result.ToColumnMatrix());
            }
        }
Ejemplo n.º 2
0
 public void CalculateObservation()
 {
     // 位置控制环
     F1 = vb.Dense(new[] {
         _plane.Vk *(Cos(_plane.Gamma) * Sin(_plane.Chi) - _plane.Chi),
         -_plane.Vk * (Sin(_plane.Gamma) - _plane.Gamma)
     });
     B1 = mb.DenseOfDiagonalArray(new double[] { _plane.Vk, -_plane.Vk });
 }
Ejemplo n.º 3
0
        public Layer(int nodeCount)
        {
            this.NodeCount = nodeCount;

            // All layers have a bias node, even the output (can ignore)
            this.Activations    = V.Dense(nodeCount + 1);
            this.Activations[0] = 1;

            this.RawValues = V.Dense(nodeCount);
            this.Gradients = V.Dense(nodeCount);
            this.Deltas    = V.Dense(nodeCount);
        }
Ejemplo n.º 4
0
        // event RecordShipStateEvent;

        public Ship()
        {
            DeckEnable   = Configuration.IsDeckCompensationEnabled;
            omega_d_2i   = vb.Dense(new double[] { omega_dx_2i, omega_dy_2i, omega_dz_2i });
            DeckPosition = Position;
            if (DeckEnable)
            {
                ForwardFilterState        = MatlabReader.Read <double>("./ForwardFilter.mat", "forward_filter_state");
                CurrentDeckPredict        = MatlabReader.Read <double>("./ForwardFilter.mat", "current_deck_predict").Column(0);
                CurrentDeckControl        = MatlabReader.Read <double>("./ForwardFilter.mat", "current_deck_control").Column(0);
                CurrentDeckLateralControl = MatlabReader.Read <double>("./ForwardFilter.mat", "current_deck_control_lat").Column(0);
            }
        }
        public static Vector <double> Cross(Vector <double> left, Vector <double> right)
        {
            if ((left.Count != 3 || right.Count != 3))
            {
                string message = "Vectors must have a length of 3.";
                throw new Exception(message);
            }
            Vector <double> result = v_builder.Dense(3);

            result[0] = left[1] * right[2] - left[2] * right[1];
            result[1] = -left[0] * right[2] + left[2] * right[0];
            result[2] = left[0] * right[1] - left[1] * right[0];

            return(result);
        }
Ejemplo n.º 6
0
        static Vector <double> MonteKarlo(Matrix <double> A, Vector <double> b, int N, int M)
        {
            int n = b.Count();

            Matrix <double> a = MatrixBuilder.Dense(n, n, (i, j) => i == j ? 0 : -A[i, j] / A[i, i]);
            Vector <double> f = VectorBuilder.Dense(n, i => b[i] / A[i, i]);
            Vector <double> x = VectorBuilder.Dense(n);

            Vector <double> h  = VectorBuilder.Dense(n);
            Vector <double> pi = VectorBuilder.Dense(n, 1.0 / 3);
            Matrix <double> P  = MatrixBuilder.Dense(n, n, (i, j) => 1.0 / 3);

            double[] E           = new double[M];
            int[]    MarkovСhain = new int[N + 1];
            double[] Weights     = new double[N + 1];

            for (int i = 0; i < n; i++)
            {
                h.Clear();
                h[i] = 1;

                Array.Clear(E, 0, E.Length);

                for (int j = 0; j < M; j++)
                {
                    for (int k = 0; k <= N; k++)
                    {
                        MarkovСhain[k] = random.Next(0, 3);
                    }

                    Weights[0] = pi[MarkovСhain[0]] > 0 ? h[MarkovСhain[0]] / pi[MarkovСhain[0]] : 0;

                    for (int k = 1; k <= N; k++)
                    {
                        Weights[k] = P[MarkovСhain[k - 1], MarkovСhain[k]] > 0 ? Weights[k - 1] * a[MarkovСhain[k - 1], MarkovСhain[k]] / P[MarkovСhain[k - 1], MarkovСhain[k]] : 0;
                    }

                    for (int k = 0; k <= N; k++)
                    {
                        E[j] += Weights[k] * f[MarkovСhain[k]];
                    }
                }

                x[i] = E.Sum() / M;
            }

            return(x);
        }
Ejemplo n.º 7
0
        static void Main(string[] args)
        {
            Matrix <double> w0 = M.Random(InputNodes, HiddenNodes, new ContinuousUniform());
            Matrix <double> w1 = M.Random(HiddenNodes, OutputNodes, new ContinuousUniform());

            int iterCount = 0;

            for (int i = 0; i < Iterations; i++)
            {
                iterCount++;
                for (int j = 0; j < TestData.RowCount; j++)
                {
                    //Forwards Propagation
                    Vector <double> l0 = TestData.Row(j);
                    Vector <double> l1 = w0.LeftMultiply(l0).Map(Relu);
                    Vector <double> l2 = w1.LeftMultiply(l1);

                    double goal       = Correct[j];
                    double prediction = l2.Sum();
                    double error      = Math.Pow(goal - prediction, 2);
                    LogPrediction(prediction, error);

                    //Backward Propagation
                    //Calculate delta at each layer
                    Vector <double> l2Delta = V.Dense(1, goal - prediction);
                    Vector <double> l1Delta = w1.Transpose().LeftMultiply(l2Delta).Map2((a, b) => a * Relu2Deriv(b), l1);
                    //Update Weights
                    w0 = w0.Add(l1Delta.ToRowMatrix().StackSelf(InputNodes).PointwiseMultiply(l0.ToRowMatrix().StackSelf(HiddenNodes).Transpose()).Multiply(Alpha));
                    w1 = w1.Add(l1.Multiply(l2Delta.Sum()).Multiply(Alpha).ToColumnMatrix());
                }
            }
            Console.ReadLine();
        }
Ejemplo n.º 8
0
        public static Vector <Complex32> Vector(TestVector vector)
        {
            switch (vector)
            {
            case TestVector.Dense5: return(V.Dense(new[] { new Complex32(1, 1), new Complex32(2, 1), new Complex32(3, 1), new Complex32(4, 1), new Complex32(5, 1) }));

            case TestVector.Dense5WithZeros: return(V.Dense(new[] { new Complex32(2, -1), new Complex32(0, 0), new Complex32(0, 2), new Complex32(-5, 1), new Complex32(0, 0) }));

            case TestVector.Sparse5: return(V.SparseOfEnumerable(new[] { new Complex32(1, 1), new Complex32(2, 1), new Complex32(3, 1), new Complex32(4, 1), new Complex32(5, 1) }));

            case TestVector.Sparse5WithZeros: return(V.SparseOfEnumerable(new[] { new Complex32(2, -1), new Complex32(0, 0), new Complex32(0, 2), new Complex32(-5, 1), new Complex32(0, 0) }));

            case TestVector.Sparse5AllZeros: return(V.Sparse(5));

            case TestVector.SparseMaxLengthAllZeros: return(V.Sparse(int.MaxValue));

            default: throw new NotSupportedException();
            }
        }
Ejemplo n.º 9
0
        public static Vector <float> Vector(TestVector vector)
        {
            switch (vector)
            {
            case TestVector.Dense5: return(V.Dense(new float[] { 1, 2, 3, 4, 5 }));

            case TestVector.Dense5WithZeros: return(V.Dense(new float[] { 2, 0, 0, -5, 0 }));

            case TestVector.Sparse5: return(V.SparseOfEnumerable(new float[] { 1, 2, 3, 4, 5 }));

            case TestVector.Sparse5WithZeros: return(V.SparseOfEnumerable(new float[] { 2, 0, 0, -5, 0 }));

            case TestVector.Sparse5AllZeros: return(V.Sparse(5));

            case TestVector.SparseMaxLengthAllZeros: return(V.Sparse(int.MaxValue));

            default: throw new NotSupportedException();
            }
        }
Ejemplo n.º 10
0
Archivo: Model.cs Proyecto: Venatu/SCL
        public Model(int NumberOfJoints, int NumberOfSupports, int NumberOfMaterials, int NumberOfSections, int NumberOfMembers, int NumberOfLoads)
        {
            JointCoordinates = M.Dense(NumberOfJoints, 2);
            SupportData      = M.Dense(NumberOfSupports, 3);
            MemberData       = M.Dense(NumberOfMembers, 4);
            LoadData         = M.Dense(NumberOfLoads, 2);

            ElasticModulus     = V.Dense(NumberOfMaterials);
            CrossSectionalArea = V.Dense(NumberOfSections);
            JointLoads         = V.Dense(NumberOfLoads);
        }
Ejemplo n.º 11
0
 public L1Adaptive(Ship ship, Plane plane, IControlModule module)
 {
     Ship          = ship;
     Plane         = plane;
     ControlModule = module;
     k_pr          = mb.DenseOfDiagonalArray(new[] { k_p, k_r });
     Amq           = -1 / T_pitch;
     w_cq          = 1 / T_cq;
     Asq           = Amq + ksq;
     Amp           = -1 / T_roll;
     w_cp          = 1 / T_cp;
     Amr           = -1 / T_yaw;
     w_cr          = 1 / T_cr;
     pr_ad_dot     = vb.Dense(new[] { p_ad_dot, r_ad_dot });
 }
Ejemplo n.º 12
0
        //event RecordAttitudeLoopEvent;

        public AttitudeLoop(Plane plane, Ship ship)
        {
            this.plane = plane;
            this.ship  = ship;

            U2             = vb.Dense(new[] { plane.DesiredParameter.Alpha, 0, 0 });
            U3FilterBuffer = mb.Dense(sampleNumber, 3, 0); // p q r
            X3             = vb.Dense(new[]
                                      { plane.Alpha, plane.Beta, plane.Miu });
            previousU3 = U3;
            filteredU2 = U2;
            //addlistener(plane, 'X3ChangedEvent', @updateState);
        }
Ejemplo n.º 13
0
    // Use this for initialization
    void Start()
    {
        /*SparseMatrix A = SparseMatrix.OfRowArrays(new double[7][]
         * {
         *  new double[] {0, -1, -1, -1, -1, -1, -1},
         *  new double[] {-1, 1, 0, 0, 0, 0, 0},
         *  new double[] {-1, 0, 1, 0, 0, 0, 0},
         *  new double[] {-1, 0, 0, 1, 0, 0, 0},
         *  new double[] {-1, 0, 0, 0, 1, 0, 0},
         *  new double[] {-1, 0, 0, 0, 0, 1, 0},
         *  new double[] {-1, 0, 1, 0, 0, 0, 1}
         * });*/

        Func <int, int, double> init = cValues;
        SparseMatrix            A    = SparseMatrix.Create(7, 7, init);

        double lambda = -993f / (6f * 0.01f);

        lambda *= 1f - 5f / 7f;
        double nDefault = 5f;
        VectorBuilder <double> Builder = Vector <double> .Build;
        Vector <double>        B       = Builder.DenseOfArray(new double[7]
        {
            0,
            lambda,
            lambda,
            lambda,
            lambda,
            lambda,
            lambda,
        });

        VectorBuilder <double> resultB = Vector <double> .Build;
        Vector <double>        result  = resultB.Dense(7);

        A.Solve(B, result);

        Debug.Log("Matrix A is : " + A.ToString());
        Debug.Log("Matrix B is : " + B.ToString());
        Debug.Log("Lambda is : " + lambda);
        Debug.Log("The result vector3 is : " + result.ToString());
    }
Ejemplo n.º 14
0
        private static DoubleCurve <T> Build(List <Contract <T> > contracts, Func <T, double> weighting,
                                             Func <T, double> multAdjustFunc, Func <T, double> addAdjustFunc, Func <T, T, double> timeFunc,
                                             double?frontFirstDerivative, double?backFirstDerivative)
        {
            if (contracts.Count < 2)
            {
                throw new ArgumentException("contracts must have at least two elements", nameof(contracts));
            }

            var curveStartPeriod = contracts[0].Start;

            int numGaps = 0;
            var timeToPolynomialBoundaries = new List <double>();

            // TODO optionally do/don't allow gaps in contracts
            for (int i = 0; i < contracts.Count - 1; i++)
            {
                var contractEnd       = contracts[i].End;
                var nextContractStart = contracts[i + 1].Start;

                if (contractEnd.CompareTo(nextContractStart) >= 0)
                {
                    throw new ArgumentException("contracts are overlapping");
                }
                timeToPolynomialBoundaries.Add(timeFunc(curveStartPeriod, nextContractStart));

                if (contractEnd.OffsetFrom(nextContractStart) < -1) // Gap in contracts
                {
                    numGaps++;
                    timeToPolynomialBoundaries.Add(timeFunc(curveStartPeriod, contractEnd.Next()));
                }
            }

            int numPolynomials         = contracts.Count + numGaps;
            int numCoefficientsToSolve = numPolynomials * 5;

            int numConstraints =
                (numPolynomials - 1) * 3   // Spline value, 1st derivative, and 2nd derivative constraints
                + numPolynomials - numGaps // Price constraints
                + (frontFirstDerivative.HasValue ? 1 : 0)
                + (backFirstDerivative.HasValue ? 1 : 0);

            MatrixBuilder <double> matrixBuilder = Matrix <double> .Build;
            VectorBuilder <double> vectorBuilder = Vector <double> .Build;

            var constraintMatrix = matrixBuilder.Dense(numConstraints, numCoefficientsToSolve);
            var vector           = vectorBuilder.Dense(numPolynomials * 5 + numConstraints);

            var twoHMatrix = matrixBuilder.Dense(numPolynomials * 5, numPolynomials * 5);

            int inputContractIndex = 0;

            bool gapFilled = false;

            int rowNum = 0;

            for (int i = 0; i < numPolynomials; i++)
            {
                int colOffset = i * 5;
                if (i < numPolynomials - 1)
                {
                    double timeToPolynomialBoundary     = timeToPolynomialBoundaries[i];
                    double timeToPolynomialBoundaryPow2 = Math.Pow(timeToPolynomialBoundary, 2);
                    double timeToPolynomialBoundaryPow3 = Math.Pow(timeToPolynomialBoundary, 3);
                    double timeToPolynomialBoundaryPow4 = Math.Pow(timeToPolynomialBoundary, 4);

                    // Polynomial equality at boundaries
                    constraintMatrix[rowNum, colOffset]     = 1.0;
                    constraintMatrix[rowNum, colOffset + 1] = timeToPolynomialBoundary;
                    constraintMatrix[rowNum, colOffset + 2] = timeToPolynomialBoundaryPow2;
                    constraintMatrix[rowNum, colOffset + 3] = timeToPolynomialBoundaryPow3;
                    constraintMatrix[rowNum, colOffset + 4] = timeToPolynomialBoundaryPow4;

                    constraintMatrix[rowNum, colOffset + 5] = -1.0;
                    constraintMatrix[rowNum, colOffset + 6] = -timeToPolynomialBoundary;
                    constraintMatrix[rowNum, colOffset + 7] = -timeToPolynomialBoundaryPow2;
                    constraintMatrix[rowNum, colOffset + 8] = -timeToPolynomialBoundaryPow3;
                    constraintMatrix[rowNum, colOffset + 9] = -timeToPolynomialBoundaryPow4;

                    // Polynomial first derivative equality at boundaries
                    constraintMatrix[rowNum + 1, colOffset]     = 0.0;
                    constraintMatrix[rowNum + 1, colOffset + 1] = 1.0;
                    constraintMatrix[rowNum + 1, colOffset + 2] = 2.0 * timeToPolynomialBoundary;
                    constraintMatrix[rowNum + 1, colOffset + 3] = 3.0 * timeToPolynomialBoundaryPow2;
                    constraintMatrix[rowNum + 1, colOffset + 4] = 4.0 * timeToPolynomialBoundaryPow3;

                    constraintMatrix[rowNum + 1, colOffset + 5] = 0.0;
                    constraintMatrix[rowNum + 1, colOffset + 6] = -1.0;
                    constraintMatrix[rowNum + 1, colOffset + 7] = -2.0 * timeToPolynomialBoundary;
                    constraintMatrix[rowNum + 1, colOffset + 8] = -3.0 * timeToPolynomialBoundaryPow2;
                    constraintMatrix[rowNum + 1, colOffset + 9] = -4.0 * timeToPolynomialBoundaryPow3;

                    // Polynomial second derivative equality at boundaries
                    constraintMatrix[rowNum + 2, colOffset]     = 0.0;
                    constraintMatrix[rowNum + 2, colOffset + 1] = 0.0;
                    constraintMatrix[rowNum + 2, colOffset + 2] = 2.0;
                    constraintMatrix[rowNum + 2, colOffset + 3] = 6.0 * timeToPolynomialBoundary;
                    constraintMatrix[rowNum + 2, colOffset + 4] = 12.0 * timeToPolynomialBoundaryPow2;

                    constraintMatrix[rowNum + 2, colOffset + 5] = 0.0;
                    constraintMatrix[rowNum + 2, colOffset + 6] = 0.0;
                    constraintMatrix[rowNum + 2, colOffset + 7] = -2.0;
                    constraintMatrix[rowNum + 2, colOffset + 8] = -6 * timeToPolynomialBoundary;
                    constraintMatrix[rowNum + 2, colOffset + 9] = -12.0 * timeToPolynomialBoundaryPow2;
                }

                // Contract price constraint
                if (i == 0 ||  // Can't be gap at the first position
                    contracts[inputContractIndex - 1].End.OffsetFrom(contracts[inputContractIndex].Start) ==
                    -1 ||      // No gap from previous
                    gapFilled) // Gap has already been dealt with
                {
                    Contract <T> contract              = contracts[inputContractIndex];
                    double       sumWeight             = 0.0;
                    double       sumWeightMult         = 0.0;
                    double       sumWeightMultTime     = 0.0;
                    double       sumWeightMultTimePow2 = 0.0;
                    double       sumWeightMultTimePow3 = 0.0;
                    double       sumWeightMultTimePow4 = 0.0;
                    double       sumWeightMultAdd      = 0.0;

                    foreach (T timePeriod in contract.Start.EnumerateTo(contract.End))
                    {
                        double timeToPeriod = timeFunc(curveStartPeriod, timePeriod);
                        double weight       = weighting(timePeriod);
                        double multAdjust   = multAdjustFunc(timePeriod);
                        double addAdjust    = addAdjustFunc(timePeriod);

                        sumWeight             += weight;
                        sumWeightMult         += weight * multAdjust;
                        sumWeightMultTime     += weight * multAdjust * timeToPeriod;
                        sumWeightMultTimePow2 += weight * multAdjust * Math.Pow(timeToPeriod, 2.0);
                        sumWeightMultTimePow3 += weight * multAdjust * Math.Pow(timeToPeriod, 3.0);
                        sumWeightMultTimePow4 += weight * multAdjust * Math.Pow(timeToPeriod, 4.0);
                        sumWeightMultAdd      += weight * multAdjust * addAdjust;
                    }

                    int priceConstraintRow = i == (numPolynomials - 1) ? rowNum : rowNum + 3;

                    constraintMatrix[priceConstraintRow, colOffset]     = sumWeightMult;         // Coefficient of a
                    constraintMatrix[priceConstraintRow, colOffset + 1] = sumWeightMultTime;     // Coefficient of b
                    constraintMatrix[priceConstraintRow, colOffset + 2] = sumWeightMultTimePow2; // Coefficient of c
                    constraintMatrix[priceConstraintRow, colOffset + 3] = sumWeightMultTimePow3; // Coefficient of d
                    constraintMatrix[priceConstraintRow, colOffset + 4] = sumWeightMultTimePow4; // Coefficient of e

                    vector[numPolynomials * 5 + priceConstraintRow] = sumWeight * contract.Price - sumWeightMultAdd;

                    twoHMatrix.SetSubMatrix(i * 5 + 2, i * 5 + 2,
                                            Create2HBottomRightSubMatrix(contract, curveStartPeriod, timeFunc));

                    inputContractIndex++;
                    rowNum   += 4;
                    gapFilled = false;
                }
                else
                {
                    // Gap in contracts
                    rowNum   += 3;
                    gapFilled = true;
                }
            }

            // TODO unit test first derivative constraints. How?
            rowNum -= 3;
            if (frontFirstDerivative.HasValue)
            {
                constraintMatrix[rowNum, 1]         = 1; // Coefficient of b
                vector[numPolynomials * 5 + rowNum] = frontFirstDerivative.Value;
                rowNum++;
            }

            if (backFirstDerivative.HasValue)
            {
                T      lastPeriod = contracts[contracts.Count - 1].End;
                double timeToEnd  = timeFunc(curveStartPeriod, lastPeriod.Offset(1));
                constraintMatrix[rowNum, numCoefficientsToSolve - 4] = 1;                          // Coefficient of b
                constraintMatrix[rowNum, numCoefficientsToSolve - 3] = 2 * timeToEnd;              // Coefficient of c
                constraintMatrix[rowNum, numCoefficientsToSolve - 2] = 3 * Math.Pow(timeToEnd, 2); // Coefficient of d
                constraintMatrix[rowNum, numCoefficientsToSolve - 1] = 4 * Math.Pow(timeToEnd, 3); // Coefficient of e
                vector[numPolynomials * 5 + rowNum] = backFirstDerivative.Value;
            }

            // Create system of equations to solve
            Matrix <double> tempMatrix1 = twoHMatrix.Append(constraintMatrix.Transpose());
            var             tempMatrix2 = constraintMatrix.Append(
                matrixBuilder.Dense(constraintMatrix.RowCount, constraintMatrix.RowCount));

            var matrix = tempMatrix1.Stack(tempMatrix2);

            Vector <double> solution = matrix.Solve(vector);

            // Read off results from polynomial
            T   curveEndPeriod      = contracts[contracts.Count - 1].End;
            int numOutputPeriods    = curveEndPeriod.OffsetFrom(curveStartPeriod) + 1;
            var outputCurvePeriods  = new T[numOutputPeriods];
            var outputCurvePrices   = new double[numOutputPeriods];
            int outputContractIndex = 0;

            gapFilled          = false;
            inputContractIndex = 0;

            for (int i = 0; i < numPolynomials; i++)
            {
                double EvaluateSpline(T timePeriod)
                {
                    double timeToPeriod = timeFunc(curveStartPeriod, timePeriod);

                    int    solutionOffset = i * 5;
                    double splineValue    = solution[solutionOffset] +
                                            solution[solutionOffset + 1] * timeToPeriod +
                                            solution[solutionOffset + 2] * Math.Pow(timeToPeriod, 2) +
                                            solution[solutionOffset + 3] * Math.Pow(timeToPeriod, 3) +
                                            solution[solutionOffset + 4] * Math.Pow(timeToPeriod, 4);

                    double multAdjust = multAdjustFunc(timePeriod);
                    double addAdjust  = addAdjustFunc(timePeriod);

                    return((splineValue + addAdjust) * multAdjust);
                }

                T start;
                T end;
                if (i == 0 ||                                                                                      // Can't be gap at the first position
                    contracts[inputContractIndex - 1].End.OffsetFrom(contracts[inputContractIndex].Start) == -1 || // No gap from previous
                    gapFilled)                                                                                     // Gap has already been dealt with
                {
                    Contract <T> contract = contracts[inputContractIndex];
                    start = contract.Start;
                    end   = contract.End;
                    inputContractIndex++;
                    gapFilled = false;
                }
                else
                {
                    start     = contracts[inputContractIndex - 1].End.Next();
                    end       = contracts[inputContractIndex].Start.Previous();
                    gapFilled = true;
                }

                foreach (var timePeriod in start.EnumerateTo(end))
                {
                    outputCurvePrices[outputContractIndex]  = EvaluateSpline(timePeriod);
                    outputCurvePeriods[outputContractIndex] = timePeriod;
                    outputContractIndex++;
                }
            }

            return(new DoubleCurve <T>(outputCurvePeriods, outputCurvePrices, weighting));
        }
Ejemplo n.º 15
0
        public static double[,] TrendAnalysis(double[,] z, int pow)
        {
            int row = z.GetLength(0);
            int col = z.GetLength(1);
            var n   = (pow + 1) * (pow + 2) / 2;
            //var m = row * col;

            var result = new double[row, col];

            var zv = new List <double>();
            var corev = new List <double[]>();
            int currXPow, currYPow, powIndex;

            for (int i = 0; i < row; i++)
            {
                for (int j = 0; j < col; j++)
                {
                    if (z[i, j] != 1.70141e38)
                    {
                        zv.Add(z[i, j]);
                        var co = new double[n];
                        powIndex = currXPow = currYPow = 0;
                        for (int k = 0; k < n; k++)
                        {
                            co[k] = Math.Pow(i + 1, currXPow) * Math.Pow(j + 1, currYPow);
                            if (currYPow == powIndex)
                            {
                                powIndex++;
                                currXPow = powIndex;
                                currYPow = 0;
                            }
                            else
                            {
                                currXPow--;
                                currYPow++;
                            }
                        }
                        corev.Add(co);
                    }
                }
            }

            int count     = corev.Count;
            var coreArray = new double[count, n];

            for (int i = 0; i < count; i++)
            {
                var co = corev[i];
                for (int j = 0; j < n; j++)
                {
                    coreArray[i, j] = co[j];
                }
            }

            var zx    = Vector.Dense(zv.ToArray());
            var core  = Matrix.DenseOfArray(coreArray);
            var coreT = core.Transpose();
            var temp  = coreT * core;
            var a     = temp.Inverse() * coreT * zx;
            var rv    = (Matrix.DenseOfArray(GetCoreMatrix(row * col, n, col)) * a).AsArray();

            int index = 0;

            for (int i = 0; i < row; i++)
            {
                for (int j = 0; j < col; j++)
                {
                    result[i, j] = rv[index];
                    index++;
                }
            }

            return(result);
        }
        public override void RunAnalysis()
        {
            int len = Barray.Length;
            // table phid-fz
            Vector <double> Fz_vector      = mybuilder.Dense(len, i => Harray[i] * lz);
            Vector <double> phid_Fz_vector = mybuilder.Dense(len, i => Barray[i] * Sz + Fz_vector[i] * Pslot);

            // table phid-fy
            Vector <double> Fy_vector      = mybuilder.Dense(len, i => Harray[i] * ly);
            Vector <double> phid_Fy_vector = mybuilder.Dense(len, i => Barray[i] * Sy);

            // reshape phid-fy, using interpolation to create new vector corresponding with phid_fy = phid_fz
            Vector <double> phid_vector = mybuilder.DenseOfVector(phid_Fz_vector);//same phid table for all

            LinearSpline    ls           = LinearSpline.Interpolate(phid_Fy_vector.ToArray(), Fy_vector.ToArray());
            Vector <double> eqvFy_vector = mybuilder.Dense(len, i => ls.Interpolate(phid_vector[i]));

            // table f_phid
            Vector <double> f_phid = mybuilder.Dense(len, i => phid_vector[i] / Pd + Fz_vector[i] + eqvFy_vector[i] - (phiR - phiHFe - phid_vector[i]) / (PM + PFe + Pb));

            var Results = new PMAnalyticalResults();

            Results.Analyser = this;
            this.Results     = Results;

            // calc phiD
            ls = LinearSpline.Interpolate(f_phid.ToArray(), phid_vector.ToArray());
            double phiD  = ls.Interpolate(0);
            double FM    = (phiR - phiHFe - phiD) / (PM + PFe + Pb);
            double phib  = FM * Pb;
            double phiFe = phiHFe + FM * PFe;
            double phiM  = phiD + phib + phiFe;

            ls = LinearSpline.Interpolate(phid_vector.ToArray(), Fz_vector.ToArray());
            double Fz = ls.Interpolate(phiD);

            ls = LinearSpline.Interpolate(phid_vector.ToArray(), eqvFy_vector.ToArray());
            double Fy = ls.Interpolate(phiD);

            double Bdelta = phiD / (L * wd * 1e-6);
            double BM     = phiM / (L * wm * 1e-6);
            double HM     = FM / (lm * 1e-3);
            double pc     = phiM / (PM * FM);

            double Bz = phiD / Sz;
            double By = phiD / Sy;

            double Vz   = Sz * lz;
            double Vy   = Sy * ly;
            double ro_  = 7800;//kg/m^3 - specific weight
            double kPMz = 1.3 * Bz * Bz * ro_ * Vz;
            double kPMy = 1.3 * By * By * ro_ * Vy;

            Results.kPMz = kPMz;
            Results.kPMy = kPMy;

            // assign results
            Results.phiD   = phiD;
            Results.Bdelta = Bdelta;
            Results.phiM   = phiM;
            Results.BM     = BM;
            Results.FM     = FM;
            Results.HM     = HM;
            Results.pc     = pc;
            Results.phib   = phib;
            Results.phiFe  = phiFe;
            Results.Fz     = Fz;
            Results.Fy     = Fy;
            Results.Bz     = Bz;
            Results.By     = By;
            Results.wd     = wd;
            Results.gammaM = gammaM;

            int    q  = Q / 3 / p / 2;
            double kp = Math.Sin(Math.PI / (2 * 3)) / (q * Math.Sin(Math.PI / (2 * 3 * q)));

            //Results.psiM = phiD * Nstrand * p * q * kp /*4 / Math.PI * Math.Sin(gammaM / 2 * Math.PI / 180)*/;

            // Ld, Lq

            //double In = 3;//Ampe whatever
            //double Fmm = q * kp * Nstrand * In;
            //double phidelta = (Fmm) / (1 / PMd + Fz / phiD + Fy / phiD) / 2;//2time,PMd=PM+Pdelta
            //double psi = phidelta * q * kp * Nstrand;
            //double LL = p * psi / In;

            //Results.LL = LL;
            //Results.Ld = 1.5 * LL;//M=-1/2L

            //phidelta = (Fmm) / (1 / PMq + Fz / phiD + Fy / phiD) / 2;//
            //psi = phidelta * q * kp * Nstrand;
            //LL = p * psi / In;
            //Results.Lq = 1.5 * LL;

            // Resistant
            Stator3Phase stator = Motor.Stator as Stator3Phase;

            Results.R = stator.resistancePhase;

            double delta2 = delta * 1.1;
            double ns     = 4 / Math.PI * kp * stator.NStrands * q;
            double dmin   = delta2;
            double alphaM = Motor.Rotor is VPMRotor ? (Motor.Rotor as VPMRotor).alphaM : 180;
            //double dmax = delta2 + lm / Math.Sin(alphaM);
            double dmax = delta2 + Constants.mu_0 * L * wd * 1e-3 * (1 / (PM + PFe + Pb) + Fy / phiD + Fz / phiD);//* wd / wm2 * ((VPMRotor)Motor.Rotor).mu_M;
            //double dmax = L * wd * 1e-3 * Constants.mu_0 / PMd;
            //double dmin2 = 1 / Math.PI * ((180 - gammaM) * dmin + gammaM * dmax) * Math.PI / 180 - 2 / Math.PI * Math.Sin(gammaM * Math.PI / 180) * (dmax - dmin);
            //double dmax2 = 1 / Math.PI * ((180 - gammaM) * dmin + gammaM * dmax) * Math.PI / 180 + 2 / Math.PI * Math.Sin(gammaM * Math.PI / 180) * (dmax - dmin);
            //double a1 = 0.5 * (1 / dmin2 + 1 / dmax2) * 1e3;
            //double a2 = 0.5 * (1 / dmin2 - 1 / dmax2) * 1e3;
            //double a1 = 0.5 * (dmin + dmax) / (dmin * dmax) * 1e3;
            //double a2 = 0.5 * (dmax - dmin) / (dmin * dmax) * 1e3;

            // test override

            double gm = gammaM * Math.PI / 180;
            double a1 = 1 / Math.PI * (gm / dmax + (Math.PI - gm) / dmin) * 1e3;
            double a2 = -2 / Math.PI * Math.Sin(gm) * (1 / dmax - 1 / dmin) * 1e3;
            double L1 = (ns / 2) * (ns / 2) * Math.PI * Motor.Rotor.RGap * Motor.GeneralParams.MotorLength * 1e-6 * a1 * 4 * Math.PI * 1e-7;
            double L2 = 0.5 * (ns / 2) * (ns / 2) * Math.PI * Motor.Rotor.RGap * Motor.GeneralParams.MotorLength * 1e-6 * a2 * 4 * Math.PI * 1e-7;

            Results.dmin = dmin;
            Results.dmax = dmax;
            Results.L1   = L1;
            Results.L2   = L2;
            Results.Ld   = 1.5 * (L1 - L2);
            Results.Lq   = 1.5 * (L1 + L2);

            double psim = 2 * Math.Sin(gammaM * Math.PI / 180 / 2) * ns * Bdelta * Motor.Rotor.RGap * Motor.GeneralParams.MotorLength * 1e-6;

            Results.psiM = psim;

            // current demagnetizing
            //Results.Ikm = 2 * phiR / PM / (kp * Nstrand * q); //old
            double hck = Motor.Rotor is VPMRotor ? (Motor.Rotor as VPMRotor).Hck : 0;

            Results.Ikm = Math.PI * (hck - HM) * lm * 1e-3 / (2 * q * Nstrand * kp);

            dataValid = (Results.Ld > 0 && Results.Lq > 0 && Results.psiM >= 0 && gm > 0);
        }
Ejemplo n.º 17
0
        /// <summary>
        /// Calculates a, b, c, d, e, and f using singular value decomposition.
        ///     lon = a*x + b*y + e;
        ///     lat = c*x + d*y + f;
        /// </summary>
        protected void createTransform()
        {
            Transform = null;
            if (DataList.Count < 3)
            {
                Utils.errMsg("Need at least three data points for calibration.");
                return;
            }

            // Define the matrices
            MatrixBuilder <double> M = Matrix <double> .Build;
            VectorBuilder <double> V = Vector <double> .Build;
            int             nPoints2 = 2 * DataList.Count;
            Matrix <double> aa       = M.Dense(nPoints2, 6);
            Vector <double> bb       = V.Dense(nPoints2);
            MapData         data     = null;
            int             row;

            for (int i = 0; i < DataList.Count; i++)
            {
                data       = DataList[i];
                row        = 2 * i;
                aa[row, 0] = data.X;
                aa[row, 1] = data.Y;
                aa[row, 4] = 1;
                bb[row]    = data.Lon;
                row++;
                aa[row, 2] = data.X;
                aa[row, 3] = data.Y;
                aa[row, 5] = 1;
                bb[row]    = data.Lat;
            }

            // Get the singular values
            try {
                Svd <double>    svd = aa.Svd(true);
                Matrix <double> u   = svd.U;
                Matrix <double> vt  = svd.VT;
                Matrix <double> w   = svd.W;
                Matrix <double> wi  = w.Clone();
                for (int i = 0; i < 6; i++)
                {
                    if (wi[i, i] != 0)
                    {
                        wi[i, i] = 1.0 / wi[i, i];
                    }
                }
                Matrix <double> aainv = vt.Transpose() * wi.Transpose() * u.Transpose();
                Vector <double> xx    = aainv * bb;
                double          a     = xx[0];
                double          b     = xx[1];
                double          c     = xx[2];
                double          d     = xx[3];
                double          e     = xx[4];
                double          f     = xx[5];
                Transform = new MapTransform(a, b, c, d, e, f);
            } catch (Exception ex) {
                Utils.excMsg("Failed to create calibration transform", ex);
                Transform = null;
            }
        }
Ejemplo n.º 18
0
        public void calc()
        {
            int len = Barray.Length;
            // table phid-fz
            Vector <double> Fz_vector      = mybuilder.Dense(len, i => Harray[i] * lz);
            Vector <double> phid_Fz_vector = mybuilder.Dense(len, i => Barray[i] * Sz + Fz_vector[i] * Pslot);

            // table phid-fy
            Vector <double> Fy_vector      = mybuilder.Dense(len, i => Harray[i] * ly);
            Vector <double> phid_Fy_vector = mybuilder.Dense(len, i => Barray[i] * Sy);

            // reshape phid-fy, using interpolation to create new vector corresponding with phid_fy = phid_fz
            Vector <double> phid_vector = mybuilder.DenseOfVector(phid_Fz_vector);//same phid table for all

            LinearSpline    ls           = LinearSpline.Interpolate(phid_Fy_vector.ToArray(), Fy_vector.ToArray());
            Vector <double> eqvFy_vector = mybuilder.Dense(len, i => ls.Interpolate(phid_vector[i]));

            // table f_phid
            Vector <double> f_phid = mybuilder.Dense(len, i => phid_vector[i] / Pd + Fz_vector[i] + eqvFy_vector[i] - (phiR - phiHFe - phid_vector[i]) / (PM + PFe + Pb));

            // calc phiD
            ls    = LinearSpline.Interpolate(f_phid.ToArray(), phid_vector.ToArray());
            phiD  = ls.Interpolate(0);
            FM    = (phiR - phiHFe - phiD) / (PM + PFe + Pb);
            phib  = FM * Pb;
            phiFe = phiHFe + FM * PFe;
            phiM  = phiD + phib + phiFe;

            ls = LinearSpline.Interpolate(phid_vector.ToArray(), Fz_vector.ToArray());
            Fz = ls.Interpolate(phiD);
            ls = LinearSpline.Interpolate(phid_vector.ToArray(), eqvFy_vector.ToArray());
            Fy = ls.Interpolate(phiD);
        }
        private static void Assignment1(int correctionLoop, double T = 0.01, int tmax = 10, bool verbose = true,
                                        string assignmentPath        = "C:/git/Computer-Aided-Analysis-and-Design/Homework_5/Files/Assignment1/", bool saveToFile = false)
        {
            Console.WriteLine("ASSIGNMENT 1");
            var start = VectorBuilder.Dense(new[] { 1.0, 1.0 });
            var A     = MatrixBuilder.Dense(2, 2, new[] { 0.0, -1.0, 1.0, 0.0 });
            var B     = MatrixBuilder.Dense(2, 2);
            Func <double, double> rt = _ => 0.0;

            var euler        = Euler(assignmentPath + "euler.txt", tmax, T, start, A, B, rt, verbose, saveToFile);
            var reverseEuler = ReverseEuler(assignmentPath + "reverseEuler.txt", tmax, T, start, A, B, rt, verbose, saveToFile);
            var trapezoid    = Trapezoid(assignmentPath + "trapezoid.txt", tmax, T, start, A, B, rt, verbose, saveToFile);
            var rungeKutta   = RungeKutta(assignmentPath + "rungeKutta.txt", tmax, T, start, A, B, rt, verbose, saveToFile);
            // var pece = Pece(assignmentPath + "pece.txt", tmax, T, start, A, B, rt, verbose, correctionLoop, saveToFile);
            // var pecece = Pecece(assignmentPath + "pecece.txt", tmax, T, start, A, B, rt, verbose, correctionLoop, saveToFile);
        }
        public double Normalize(int joint_index, double theta)
        {
            double u = Robot.Joint_upper_limit[joint_index];
            double l = Robot.Joint_lower_limit[joint_index];

            if (check_limits)
            {
                if (!(l < theta && theta < u))
                {
                    Vector <double> a = v_builder.DenseOfArray(new[] { -1.0, 1.0 });
                    a = a * 2 * Math.PI;
                    Vector <double> b     = a.Add(theta);
                    int             index = -1;
                    for (int i = 0; i < b.Count; i++)
                    {
                        if (l < b[i] && b[i] < u)
                        {
                            index = i;
                            break;
                        }
                    }
                    if (index == -1)
                    {
                        return(default(double));
                    }
                    else
                    {
                        theta = theta + a[index];
                    }
                }
            }
            if (use_last_joints)
            {
                double diff = Last_joints[joint_index] - theta;

                int    n_diff = (int)Math.Floor(diff / (Math.PI * 2.0));
                double r_diff = diff % (Math.PI * 2.0);
                if (r_diff > Math.PI)
                {
                    n_diff += 1;
                }
                if (Math.Abs(n_diff) > 0)
                {
                    if (!check_limits)
                    {
                        theta += 2 * Math.PI * n_diff;
                    }
                    else
                    {
                        double theta_vs = theta + 2 * Math.PI;

                        Vector <double> theta_v = v_builder.Dense(Math.Abs(n_diff));
                        for (int i = 0; i < theta_v.Count; i++)
                        {
                            if (n_diff > 0)
                            {
                                theta_v[i] = n_diff - i;
                            }
                            else if (n_diff < 0)
                            {
                                theta_v[i] = n_diff + i;
                            }
                        }
                        theta_v = theta_v * 2 * Math.PI;
                        theta_v = theta_v.Add(theta);
                        int index = -1;
                        for (int i = 0; i < theta_v.Count; i++)
                        {
                            if (l < theta_v[i] && theta_v[i] < u)
                            {
                                index = i;
                                break;
                            }
                        }
                        if (index != -1)
                        {
                            theta = theta_v[index];
                        }
                    }
                }
            }
            return(theta);
        }
Ejemplo n.º 21
0
        public Plane(Ship ship)
        {
            PlaneInertia = new()
            {
                Ixx   = 74608.24,// 转动惯量 kg/m2
                Iyy   = 227616.4,
                Izz   = 231114.32,
                Ixz   = 0,
                WingS = 62.006,       // 翼面积
                WingC = 4.218,        // 平均气动弦长
                WingL = 14.70,        // 翼展
                Mass  = 16382.85,     // 空重 kg
                TMax  = 122.6 * 1000, // 军用推力(两台)
                Rou   = 1.225,        // 空气密度
                G     = 9.8
            };
            DesiredParameter = new()
            {
                Alpha       = 10 * Pi / 180,   // 期望迎角 9.1
                Chi         = 0 * Pi / 180,    // 期望航向角
                Gamma       = -3.5 * Pi / 180, // 期望爬升角
                Vk          = 62,              // 期望速度 71
                EngineDelta = 0 * Pi / 180     // 发动机安装角
            };

            Flow            = 0.5 * PlaneInertia.Rou * Math.Pow(Vk, 2);
            T               = PlaneInertia.TMax * DeltaP;
            ThrustRange     = PlaneInertia.TMax * DeltaPRange;
            ThrustRateRange = PlaneInertia.TMax * DeltaPRateRange;
            CalculatePneumaticParameters();
            CalculateForceAndMoment();
            Initialize(ship);
            Theta           = Gamma + Alpha;
            DeltaTEFDesired = DeltaTEF;
            DesiredPosition = vb.Dense(3, Position[0]);
            DesiredPosition.SetSubVector(
                1, 2, HelperFunction.ideal_path(Position, ship.Position, ship.Theta, ship.Psi));
        }

        void AddListeners()
        {
        }

        void Initialize(Ship ship)
        {
            Vector <double> p_d_2p;

            if (l_path_0 > 1620)
            {
                double x_d_2p = -1620 * Cos(ship.Theta) * Cos(ship.Gamma) - (l_path_0 - 1620); // 期望点坐标 P系下表示
                double y_d_2p = 1620 * Sin(ship.Theta) * Cos(ship.Gamma);
                double z_d_2p = 1620 * Sin(ship.Gamma);

                p_d_2p = vb.Dense(new double[] { x_d_2p, y_d_2p, z_d_2p }); // 期望点坐标 P系下表示
            }
            else
            {
                double x_d_2p = -(l_path_0 - l_path) * Cos(ship.Theta) * Cos(ship.Gamma); // 期望点坐标 P系下表示
                double y_d_2p = (l_path_0 - l_path) * Sin(ship.Theta) * Cos(ship.Gamma);
                double z_d_2p = (l_path_0 - l_path) * Sin(ship.Gamma);

                p_d_2p = vb.Dense(new double[] { x_d_2p, y_d_2p, z_d_2p }); // 期望点坐标 P系下表示
            }
            Matrix <double> R_i2p = mb.DenseOfArray(new double[, ] {
                { Cos(ship.Psi), Sin(ship.Psi), 0 }, { -Sin(ship.Psi), Cos(ship.Psi), 0 }, { 0, 0, 1 }
            });
            Matrix <double> R_p2i  = R_i2p.Transpose();
            Vector <double> p_d_2i = R_p2i * p_d_2p + ship.Position; // 期望点坐标 I系下表示

            double kai_f   = 0;
            double gamma_f = 0;

            Matrix <double> R_i2f = mb.DenseOfArray(new double[, ] {
                { Cos(gamma_f) * Cos(kai_f), Cos(gamma_f) * Sin(kai_f), -Sin(gamma_f) },
                { -Sin(kai_f), Cos(kai_f), 0 },
                { Sin(gamma_f) * Cos(kai_f), Sin(gamma_f) * Sin(kai_f), Cos(gamma_f) }
            });
            Matrix <double> R_f2i  = R_i2f.Transpose();
            Vector <double> p_b_2f = vb.Dense(new double[] { 0, 5, 10 }); // 飞机在F系中初始位置

            x_b_2f = p_b_2f[0];
            y_b_2f = p_b_2f[1];
            z_b_2f = p_b_2f[2];

            Position = R_f2i * p_b_2f + p_d_2i; // 飞机在I系中初始位置
            Gamma    = 0;                       // 初始爬升角
            Chi      = -ship.Theta;             // 初始航向角
            Vector <double> omega_d_2f  = R_i2f * ship.omega_d_2i;
            double          omega_dx_2f = omega_d_2f[0];
            double          omega_dy_2f = omega_d_2f[1];
            double          omega_dz_2f = omega_d_2f[2];

            Vector <double> omega_f_2f = vb.Dense(new double[] { omega_dx_2f, omega_dy_2f, omega_dz_2f });

            omega_fx_2f = omega_f_2f[0];
            omega_fy_2f = omega_f_2f[1];
            omega_fz_2f = omega_f_2f[2];

            kai_b2f   = Chi - kai_f;
            gamma_b2f = Gamma - gamma_f;
        }

        public void Record()
        {
            RecordPlaneStateEvent?.Invoke(this, null);
        }

        void CalculatePneumaticParameters()
        {
            double WingC = PlaneInertia.WingC;
            double WingS = PlaneInertia.WingS;
            double WingL = PlaneInertia.WingL;

            double CM_delta_tef      = 0.001 * 180 / Pi;
            double CM_delta_lef      = 0;
            double Cnormal_delta_tef = 0.009 * 180 / Pi;
            double Cnormal_delta_lef = Cnormal_delta_tef * 0.5; // 推测数据,缺少实测数据
            double Caxis_delta_tef   = 0;
            double Caxis_delta_lef   = 0;

            // 升力系数相关参数 弧度制
            double CY_alpha3 = -20.779;
            double CY_alpha2 = 1.1682;
            double CY_alpha1 = 3.8091;
            double CY_0      = 0.12;

            CY_alpha = CY_alpha3 * Math.Pow(Alpha, 2) + CY_alpha2 * Alpha + CY_alpha1;
            double CY_delta_e   = 0.6;
            double CY_delta_lef = 0;

            CY_delta_tef = 0.02 * 57.3;
            CY           = CY_0
                           + CY_alpha * Alpha
                           + CY_delta_e * DeltaE
                           + CY_delta_lef * DeltaLEF  // leading - egde flap && tailing - edge flap
                           + CY_delta_tef * DeltaTEF; // lift coefficient
                                                      // 阻力系数相关参数 弧度制
            double CD_alpha2 = 0.3009;
            double CD_alpha1 = -0.0622;
            double CD_alpha0 = -1.4994;
            double CD_0      = -0.0019 + 0.0109;

            CD_alpha = CD_alpha2 * Alpha + CD_alpha1;
            double CD_delta_e   = 0.04;
            double CD_delta_lef = 0;
            double CD_delta_tef = 0.002 * 57.3;
            double CD_Ma;

            if (Vk / 340 <= 0.81)
            {
                CD_Ma = 0;
            }
            else
            {
                CD_Ma = 0.1007 * Math.Pow(Vk / 340, 3) - 0.4653 * Math.Pow(Vk / 340, 2) + 0.6903 * (Vk / 340) - 0.3074;
            }
            double CD_beta1 = 0.4946;
            double CD_beta  = CD_beta1 * Beta;

            CD = CD_0
                 + CD_beta * Beta                // 侧滑阻力
                 + CD_alpha * Alpha              // 迎角引起的阻力
                 + CD_delta_e * Math.Abs(DeltaE) // 升降舵偏转引起的阻力。
                 + CD_delta_lef * DeltaLEF       // leading - egde flap引起的阻力认为是0 && tailing - edge flap
                 + CD_delta_tef * DeltaTEF       // 襟翼引起的阻力
                 + 0.1 * Math.Pow(CY, 2)         // 升致阻力
                 + CD_Ma;                        // 马赫导致的阻力 // drag coefficient

            // 侧力系数 弧度制
            CC_beta = -1;
            CC      = CC_beta * Beta;

            // 滚转力矩相关参数 弧度制
            CL_beta    = -0.1;
            CL_delta_a = 0.12;
            CL_delta_r = 0.01;
            double CL_p = -0.4;
            double CL_r = 0.15;

            CL = CL_beta * Beta          // beta引起的滚转力矩
                 + CL_delta_a * DeltaA
                 + CL_delta_r * DeltaR
                 + WingL / 2 / Vk * CL_p * P
                 + WingL / 2 / Vk * CL_r * R; // rolling moment coefficient

            // 俯仰力矩相关参数 弧度制
            CM_delta_e = 0.35 * (Vk / 340) - 1.1;
            double CM_q = -23;

            CM_alpha = -0.1;
            double CM_alpha_dot = -9;

            CM = CM_alpha * Alpha                  // 由alpha引起的俯仰力矩
                 + CM_delta_e * DeltaE             // 升降舵产生的俯仰力矩
                 + WingC / 2 / Vk * CM_q * Q;      // 俯仰角速度产生的俯仰力矩


            // 偏航力矩相关参数 弧度制
            CN_beta    = 0.12;
            CN_delta_r = -0.15;
            double CN_r = -0.15;

            CN_delta_a = 0;
            CN         = CN_beta * Beta
                         + CN_delta_r * DeltaR
                         + WingL / 2 / Vk * CN_r * R; // yawing moment coefficient
        }

        void CalculateForceAndMoment()
        {
            Y = Flow * PlaneInertia.WingS * CY;
            D = Flow * PlaneInertia.WingS * CD;
            C = Flow * PlaneInertia.WingS * CC;
            L = Flow * PlaneInertia.WingS * PlaneInertia.WingL * CL;
            M = Flow * PlaneInertia.WingS * PlaneInertia.WingC * CM;
            N = Flow * PlaneInertia.WingS * PlaneInertia.WingL * CN;
        }