在 C# 中使用线性最小二乘法进行二维三边测量

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

& pC 是信标和 rA， rB 和 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();
    }

Answer 2

您没有正确计算 B 向量。

它应该是：

  //dBA = 0.5 * (rA^2 - rB^2 - length_vA^2 + length_vB^2)
  b[0] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rB, 2) - Math.Pow(getDistance(pA,{0,0}), 2) + Math.Pow(getDistance(pB,{0,0}), 2));
  b[1] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rC, 2) - Math.Pow(getDistance(pA,{0,0}), 2) + Math.Pow(getDistance(pC,{0,0}), 2));