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

Question

我正在使用MathNet进行矩阵和矢量的二维三角测量。这我我的代码：

 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(); 
    }