Взвешенный метод наименьших квадратов - подгонка плоскости к набору трехмерных точек

Я подгоняю плоскость к набору трехмерных точек методом наименьших квадратов. У меня уже есть алгоритм для этого, но я хочу изменить его, чтобы использовать взвешенный метод наименьших квадратов . Это означает, что у меня есть вес для каждой точки (чем больше вес, тем ближе плоскость должна быть к точке).

Текущий алгоритм (без веса) выглядит так:

Вычислить сумму:

for(Point3D p3d : pointCloud) {
    pos = p3d.getPosition();
    fSumX += pos[0];
    fSumY += pos[1];
    fSumZ += pos[2];
    fSumXX += pos[0]*pos[0];
    fSumXY += pos[0]*pos[1];
    fSumXZ += pos[0]*pos[2];
    fSumYY += pos[1]*pos[1];
    fSumYZ += pos[1]*pos[2];
}

, чем составьте матрицы:

double[][] A = {
    {fSumXX, fSumXY, fSumX},
    {fSumXY, fSumYY, fSumY},
    {fSumX,  fSumY,  pointCloud.size()}
};

double[][] B =  {
    {fSumXZ},
    {fSumYZ},
    {fSumZ}
};

затем решите Ax = B и 3 компонента решения будут коэффициентами подобранной плоскости ...

Итак, не могли бы вы помочь мне, как изменить это, чтобы использовать веса? Спасибо!