Question

我正在使用MathNet为矩阵和向量进行2D三边测量。这是我的代码：

        public static double[] trilaterate2DLinear(double[] pA, double[] pB, double[] pC, double rA, double rB, double rC) {
        //Convert doubles to vectors for processing
        Vector<double> vA = Vector<double>.Build.Dense(pA);
        Vector<double> vB = Vector<double>.Build.Dense(pB);
        Vector<double> vC = Vector<double>.Build.Dense(pC);

        //Declare elements of b vector
        //bBA = 1/2 * (rA^2 - rB^2 + dBA^2)
        double[] b = {0, 0};
        b[0] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rB, 2) + Math.Pow(getDistance(pB, pA), 2));
        b[1] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rC, 2) + Math.Pow(getDistance(pC, pA), 2));
        //Convert b array to vector form
        Vector<double> vb = Vector<double>.Build.Dense(b);

        //Build A array
        //A =   {x2 -x1, y2 - y1}
        //      {x3 - x1, y3 - y1}
        double[,] A =  { { pB[0] - pA[0], pB[1] - pA[1] }, { pC[0] - pA[0], pC[1] - pA[1] } };
        //Convert A to Matrix form
        Matrix<double> mA = Matrix<double>.Build.DenseOfArray(A);
        //Declare Transpose of A matrix;
        Matrix<double> mAT = mA.Transpose();

        //Declare solution vector x to 0
        Vector<double> x = Vector<double>.Build.Dense(2);

        //Check if A*AT is non-singular (non 0 determinant)
        if (mA.Multiply(mAT).Determinant() == 0)
        {
            //x = ((AT * A)^-1)*AT*b
            x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
        }
        else 
        { 
            //TODO case for A*AT to be singular
            x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
        }

        //final position is x + vA
        //return as double so as not
        return (x.Add(vA)).ToArray();
    }

    //Gets the Euclidean distance between two points
    private static double getDistance(double[] p1, double[] p2)
    {
        //d^2 = (p1[0] - p2[0])^2 + (p1[1] - p2[1]);
        double distSquared = Math.Pow((p1[0] - p2[0]),2) + Math.Pow((p1[1] - p2[1]),2);
        return Math.Sqrt(distSquared);
    }

pA，pB＆amp; pC是Beacons和rA，rB＆amp;的坐标。 rC是从每个信标到用户的距离。有什么明显的我做错了吗？也许矩阵乘法的顺序需要改变，但我对线性最小二乘法不够熟悉，无法跟踪矩阵并告诉它。

Answer 1

解决。 if语句中的if语句和计算错误。

校正：

public static double[] trilaterate2DLinear(double[] pA, double[] pB, double[] pC, double rA, double rB, double rC) {
        //Convert doubles to vectors for processing
        Vector<double> vA = Vector<double>.Build.Dense(pA);
        Vector<double> vB = Vector<double>.Build.Dense(pB);
        Vector<double> vC = Vector<double>.Build.Dense(pC);

        //Declare elements of b vector
        //bBA = 1/2 * (rA^2 - rB^2 + dBA^2)
        double[] b = {0, 0};
        b[0] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rB, 2) + Math.Pow(getDistance(pB, pA), 2));
        b[1] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rC, 2) + Math.Pow(getDistance(pC, pA), 2));
        //Convert b array to vector form
        Vector<double> vb = Vector<double>.Build.Dense(b);

        //Build A array
        //A =   {x2 -x1, y2 - y1}
        //      {x3 - x1, y3 - y1}
        double[,] A =  { { pB[0] - pA[0], pB[1] - pA[1] }, { pC[0] - pA[0], pC[1] - pA[1] } };
        //Convert A to Matrix form
        Matrix<double> mA = Matrix<double>.Build.DenseOfArray(A);
        //Declare Transpose of A matrix;
        Matrix<double> mAT = mA.Transpose();

        //Declare solution vector x to 0
        Vector<double> x = Vector<double>.Build.Dense(2);

        //Check if A*AT is non-singular (non 0 determinant)
        double det = mA.Multiply(mAT).Determinant();
        if (mA.Multiply(mAT).Determinant() > 0.1)
        {
            //x = ((AT * A)^-1)*AT*b
           // x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);

            x = (mA.Transpose() * mA).Inverse() * (mA.Transpose() * vb);
        }
        else 
        { 
            //TODO case for A*AT to be singular
            x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
        }

        //final position is x + vA
        //return as double so as not
        return (x.Add(vA)).ToArray();
    }

在C＃中使用线性最小二乘法进行2D三边测量

1 个答案: