Question

     static void Main()
        {

            double[] MPCV = { 0, 0, 0, 0, 0 }; //MPCV = velocity for MPC 
            double[] MPCW = { 0, 0, 0, 0, 0 }; // MPCW = omega for MPC


            double G1 = 0.04, G2 = 0.04, T = 20E-06;


            int k, n = 5, count;

            double[] X = new double[6];
            double[] Y = new double[6];
            double[] A = new double[6];


            int tx = 700;
            double ta = (Math.PI / 180) * 60;
            int ty = 300;

            double[,] qd = new double[3, 1] { { tx }, { ty }, { ta } };
            double[,] Lamda5 = new double[1, 3]; //lamda(5) transpose
            double[,] qk = new double[6, 3]; //q(k) matrix
            double[,] r = new double[3, 1]; //q(5)-qd
            double[,] Z = new double[3, 3]; //df/dq


            X[0] = 500;
            Y[0] = 600;
            A[0] = (Math.PI / 180) * 90; //converting from degree to radian




            for (count = 0; count < 30; count++) //main loop for MPC controller
            {

                for (k = 0; k < n; k++) // loop to find q(k) = [x[k] y[k] A[k]]
                {
                    X[k + 1] = X[k] + (T * MPCV[k]) * Math.Cos(A[k]); // state space model for X,Y,Angle
                    Y[k + 1] = Y[k] + (T * MPCV[k]) * Math.Sin(A[k]);
                    A[k + 1] = A[k] + (T * MPCW[k]);

                    Console.WriteLine("\nX({0}) = {1} \nY({0}) = {2} \nX({0}) = {3} ", k + 1, X[k + 1], Y[k + 1], A[k + 1]);

                    for (int j = 0; j <= n; j++) //update q(k) as a matrix
                    {
                        qk[j, 0] = X[j];
                        qk[j, 1] = Y[j];
                        qk[j, 2] = A[j];

                        Console.WriteLine("\nthe matrix is q({0}) =[X={1} Y={2}  A={3}]", j, qk[j, 0], qk[j, 1], qk[j, 2]);
                    }
                }
                //end first for loop for q(k)

                double X5 = qk[n, 0];
                double Y5 = qk[n, 1];

                r[0, 0] = X5 - tx;
                r[1, 0] = Y5 - ty;
                r[2, 0] = 0;
                Console.WriteLine("\nthe matrix is q(N) =[X={1} Y={1}  A={2}]", r[0, 0], r[1, 0], r[2, 0]);

                Lamda5[0, 0] = G1 * r[0, 0];
                Lamda5[0, 1] = G1 * r[1, 0];
                Lamda5[0, 2] = G1 * r[2, 0];

                Console.WriteLine("\nthe matrix is LamdaN =[X={1} Y={1}  A={2}]", Lamda5[0, 0], Lamda5[0, 1], Lamda5[0, 2]);

                for (k = n; k <= 1; k--) // loop for lamda
                {

                    Z[0, 0] = 1;
                    Z[0, 1] = 0;
                    Z[0, 2] = -T * MPCV[k] * (Math.Sin(A[k]));

                    Z[1, 0] = 1;
                    Z[1, 1] = 0;
                    Z[1, 2] = -T * MPCV[k] * (Math.Cos(A[k]));

                    Z[2, 0] = 0;
                    Z[2, 1] = 0;
                    Z[2, 2] = 1;


                    Console.WriteLine("\nthe matrix is Z =[X={1} Y={1}  A={2}]", Z[0,0], Z[0,1], Z[0,2]);
                    Console.WriteLine("\nthe matrix is Z =[X={1} Y={1}  A={2}]", Z[1,0], Z[1,1], Z[1,0]);
                    Console.WriteLine("\nthe matrix is Z =[X={1} Y={1}  A={2}]", Z[2,0], Z[2,1], Z[2,0]);
                }
}

嗨，我已经尝试按照我的评论行所描述的lamda循环进行迭代。但是，它不会迭代，并且不会显示矩阵Z的输出。我在某处弄错了吗？谢谢。

Answer 1

for (k = n; k <= 1; k--)

非常不正常，因为继续条件是检查k是否 <1>。

如果n大于1（并且因为您将其设置为5），则永远不会执行该循环体。

您应该使用>=而不是<=。

Answer 2

我在某处犯了错误吗？

是的 - 您的条件要求k小于或等于1，但从开始，因此该块永远不会执行。

将循环条件更改为

for (k = n; k >= 1; k--)

Answer 3

您的For循环条件错误。

你只是犯了更大而不是更多的错误。

你想循环直到K大于或等于1.

但是你检查过循环直到K小于1。

所以你只需要纠正条件。

for (k = n; k >= 1; k--) // loop for lambda                
{
}

循环的减量不会迭代

3 个答案: