simplestarの技術ブログ

目的を書いて、思想と試行、結果と考察、そして具体的な手段を記録します。

Math:Unityでの連立方程式の解き方

本ブログの数学カテゴリを作りまして、第一回目は連立方程式の解き方を示したいと思います。

みなさんご存じの通り、中学校で学ぶ連立方程式のことですが、これがなかなか簡単ではありません。
例えば、次の連立方程式がテストで出された時、変数 x, y, z, w, u, v を時間内にみなさんは求められましたでしょうか?

f:id:simplestar_tech:20170402193356j:plain

すみません、中学校ではここまで難易度の高い連立方程式は出題されませんでしたね。
ちなみに、上の式の答えは次の通り

x = 7.0
y = 5.0
z =-5.0
w = 3.0
u = 6.0
v = 4.0

さて、理系の大学を出て、そっち方面のお仕事に就くと、実際にこうした数値を計算する場面に遭遇します。
これをマイクロ秒オーダーで解くのはなかなかテクニックが必要でして、今日はその計算テクニックを動作確認済みのプログラムとして紹介します。

        /// <summary>
        /// 連立方程式を解く
        /// </summary>
        /// <param name="J">係数行列</param>
        /// <param name="b">定数項ベクトル</param>
        /// <param name="x">解ベクトル</param>
        /// <returns>true:成功,false:失敗</returns>
        public static bool SolveEq(float[][] J, float[] b, float[] x)
        {
            int jCount = J.Length;
            int n = x.Length;

            // 係数行列を正定値対称行列化
            float[][] JtJ;
            SimpleMath.CreateMatrix(n, n, out JtJ);
            for (int row = 0; row < n; row++)
            {
                for (int col = 0; col < n; col++)
                {
                    JtJ[row][col] = 0;
                    for (int i = 0; i < jCount; i++)
                    {
                        JtJ[row][col] += J[i][row] * J[i][col];
                    }
                }
            }

            // 定数項を対称行列化にともない修正
            float[] Jtb = new float[n];
            for (int row = 0; row < n; row++)
            {
                Jtb[row] = 0;
                for (int col = 0; col < jCount; col++)
                {
                    Jtb[row] += J[col][row] * b[col];
                }
            }

            // 修正 Cholesky 分解
            float[][] L;
            SimpleMath.CreateMatrix(n, n, out L);
            float[] d = new float[n];
            SimpleMath.ModifiedCholeskyDecomposition(JtJ, L, d);

            // 以下、連立方程式を解く

            // 前進代入
            float[] y = new float[n];
            for (int row = 0; row < n; row++)
            {
                y[row] = Jtb[row];
                for (int col = 0; col < row; col++)
                {
                    y[row] -= L[row][col] * y[col];
                }
                y[row] /= L[row][row];
            }
            // 後退代入
            for (int row = n - 1; row >= 0; row--)
            {
                x[row] = y[row];
                for (int col = n - 1; col > row; col--)
                {
                    x[row] -= (L[col][row] * d[row]) * x[col];
                }
            }
            return true;
        }

        /// <summary>
        /// 修正コレスキー分解(modified Cholesky decomposition)
        /// </summary>
        /// <remarks>
        /// 対称行列A(n×n)を下三角行列(L:Lower triangular matrix)と対角行列の積(LDL^T)に分解する
        /// l_ii * d_i = 1とした場合
        /// L: i > jの要素が非ゼロで対角成分は1
        /// </remarks>
        /// <param name="A">A n×nの対称行列</param>
        /// <param name="L">対角成分が1の下三角行列</param>
        /// <param name="d">対角行列(対角成分のみ)</param>
        /// <returns>true:成功,false:失敗</returns>
        public static bool ModifiedCholeskyDecomposition(float[][] A, float[][] L, float[] d)
        {
            int n = d.Length;
            if (n <= 0)
                return false;

            L[0][0] = A[0][0];
            d[0] = 1.0f / L[0][0];

            for (int row = 1; row < n; ++row)
            {
                for (int col = 0; col < n; ++col)
                {
                    L[row][col] = 0;
                    if (col <= row)
                    {
                        float A_SumLdL = A[row][col];
                        for (int nonZeroIdx = 0; nonZeroIdx < col; ++nonZeroIdx)
                        {
                            A_SumLdL -= L[row][nonZeroIdx] * d[nonZeroIdx] * L[col][nonZeroIdx];
                        }
                        L[row][col] = A_SumLdL;
                    }
                }
                d[row] = 1.0f / L[row][row];
            }

            return true;
        }

係数行列をfloatの2次元配列として渡して、定数項ベクトルもfloat配列で渡すと、解ベクトルがfloat配列で返ってきます。
正しく動作するか確認するプログラムは次の通りです。

    public Transform panelA;
    public Transform panelx;
    public Transform panelb;

    float[][] _A;
    float[] _b;
    float[] _x;

    public void CreateProblem()
    {
        int n = 6;
        SimpleMath.CreateMatrix(n, n, out _A);
        int row = 0;
        int col = 0;
        foreach (InputField child in panelA.GetComponentsInChildren<InputField>())
        {
            float random = Random.Range(-9.0f, 9.0f);
            child.text = random.ToString("0.00");
            _A[row][col++] = float.Parse(child.text);
            if (n == col)
            {
                col = 0;
                row++;
                if (n == row)
                {
                    break;
                }
            }
        }

        _x = new float[n];
        
        int i = 0;
        foreach (InputField child in panelx.GetComponentsInChildren<InputField>())
        {
            float random = Random.Range(-9, 9);
            child.text = random.ToString("0.00");
            _x[i++] = float.Parse(child.text);
            if (n == i)
            {
                break;
            }
        }

        float[][] b;
        float[][] x;
        SimpleMath.CreateMatrix(n, 1, out b);
        SimpleMath.CreateMatrix(n, 1, out x);

        for (int k = 0; k < n; k++)
        {
            x[k][0] = _x[k];
        }

        if (SimpleMath.MatrixMultiply(_A, x, b))
        {
            _b = new float[n];
            for (int k = 0; k < n; k++)
            {
                _b[k] = b[k][0];
            }
            i = 0;
            foreach (InputField child in panelb.GetComponentsInChildren<InputField>())
            {
                child.text = _b[i++].ToString("0.00");
                if (n == i)
                {
                    break;
                }
            }
        }
    }

    public void SolveProblem()
    {
        if(SimpleMath.SolveEq(_A, _b, _x))
        {
            for (int i = 0; i < _x.Length; i++)
            {
                Debug.Log("x[" + i + "] = " + _x[i].ToString("0.000"));
            }
        }
    }

この連立方程式を解く関数はちょっとその辺の LU 分解タイプとは異なり、式の数が変数の数よりも多くても、もっともらしい解を返します。
疑似逆行列とか最小二乗法で調べてもらえると、原理の説明にたどり着けるでしょう。
そこで出てくる正定値対称行列のLU分解はコレスキー分解と呼ばれていて、そのコレスキー分解もまた高速化された修正版が存在します。
今回はその修正コレスキー分解を用いて、連立方程式を解きました。

変数の数が 6 より大きい場合も利用できますので、もし同じ問題でお困りの方がいましたら、使ってみてください。

以下、参考にしたサイトです。

こちらの記事で、コレスキー分解について学びました。
コレスキー分解 - PukiWiki for PBCG Lab