楕円体フィッティングによるセンサキャリブレーションについて

現在，加速度センサ・地磁気センサの計測値を楕円体へのフィッティングを行う事で，オフセットと感度補正を行おうとしています．回転を含まない楕円体の式を以下のように定義して，最小二乗法を使って解こうとしています．

(x - p)^2 / a^2 + (y - q)^2 / b^2 + (z - r)^2 / c^2 = 1

上記の式を，展開し，一般形（？）に変形しました．

Ay^2 + Bz^2 + Cx + Dy + Ez + F = 0

この左辺を二乗したものを評価関数としてA~~Fで偏微分し，以下のような式が続く連立方程式としてA~~
Fを求められるプログラムを書きました．
Σ(Ay^4 + By^2z^2 + Cxy^2 + Dy^3 + Ey^2z + Fy^2)= -Σx^2y^2

ソースが以下です．

C#
1    public class Fitting {
2        private Matrix matrix = new Matrix(6, 6);
3        private double[] right = new double[6];
4
5        /// <summary>
6        /// 与えられた計測値セットの数を取得する
7        /// </summary>
8        public int Samples { get; private set; } = 0;
9
10        /// <summary>
11        /// 与えられた計測値を元に方程式のパラメータを更新する
12        /// </summary>
13        /// <param name="args">3軸のセンサ計測値</param>
14        public void Update(double[] args) {
15            // 有効な引数かチェック
16            if (args.Length != 3) {
17                throw new ArgumentException();
18            }
19
20            // 計測値セット数の加算
21            this.Samples++;
22
23            // 方程式のパラメータを更新する
24            var common = new double[6];
25            common[0] = Math.Pow(args[1], 2);
26            common[1] = Math.Pow(args[2], 2);
27            common[2] = args[0];
28            common[3] = args[1];
29            common[4] = args[2];
30            common[5] = 1;
31
32            for (int i = 0; i < 6; i++){
33                for (int j = 0; j < 6; j++){
34                    this.matrix[i, j] += common[i] * common[j];
35                }
36
37                this.right[i] -= common[i] * Math.Pow(args[1], 2);
38            }
39        }
40
41        /// <summary>
42        /// これまでの入力から楕円体のパラメータを計算して返す
43        /// </summary>
44        public double[] ComputeCoefficient(){
45            // 引数が有効かをチェック
46            if(this.matrix.Rows != 6 || this.right.Length != 6){
47                throw new ArgumentException();
48            }
49
50            // 逆行列を計算し，逆行列がなければ例外を投げる
51            if(!this.matrix.Inverse(out Matrix inv)){
52                throw new ArithmeticException();
53            }
54
55            // 連立方程式の解を求める
56            var result = new double[6];
57            for (int i = 0; i < 6; i++){
58                for (int j = 0; j < 6; j++){
59                    result[i] += inv[i, j] * this.right[j];
60                }
61            }
62
63            var A = result[0];
64            var B = result[1];
65            var C = result[2];
66            var D = result[3];
67            var E = result[4];
68            var F = result[5];
69
70            var p = -C / 2;
71            var q = -D / (2 * A);
72            var r = -E / (2 * B);
73
74            var a = Math.Sqrt(p * p + A * q * q + B * r * r - F);
75            var b = Math.Sqrt(a * a / A);
76            var c = Math.Sqrt(a * a / B);
77
78            return new double[] { p, q, r, a, b, c };
79        }
80}