RANSAC в MatLab

Есть задача. Необходимо добавить собственный код алгоритма RANSAC. Я попробывал, но програма выдает не правильный результат. Может кто подскажет, где я ошибся.

Входящие параметры – облако точек p из набора данных и следующие настройки:

confidence – достоверность результата в процентах;

inlier_margin – максимальное расстояние от точки до плоскости, при которой эта точка считается «inlier» (в метрах);

parameters.sigma1 – минимальное расстояние всех отобранных точек одна до одной (в метрах);

Выходные параметры функции: a, b, c, d – параметры плоскости определены из уравнения:

? ∙ ? + ? ∙ ? + ? ∙ ? + ? = 0

inliers – логический (бинарный) массив 1×N, обозначающий точки облака как "inliers". Содержит 1 для каждого inlier и 0 для outliers для соответствующей позиции в массиве p.

Например, inliers = [0, 1, 0, 0, 1, 0, 1, 1, ...] означает, что точки из p с номерами колонок 2, 5, 7, 8,... являются «inliers» и входят в рассчитанную плоскость

k – необходимое количество итераций для достижения достоверности результата Дополнительно описания исходных параметров смотреть в комментариях к файлу. Облако точек p – это массив (6×N, где N – количество точек в облаке), содержащее следующие данные:

1. x – расстояние, метры
2. y – расстояние, метры
3. z – расстояние, метры
4. r – яркость пикселя (0-1)
5. g – яркость пикселя (0-1)
6. b – яркость пикселя (0-1)

Есть код - https://pastebin.com/deE4ZvAK