LU分解で逆行列を求めたい（連立方程式も解きたい）

大学で数値線形代数の勉強をすると、LU分解について学ぶと思います。このLU分解、行列の計算に関して色々活用できそうなのでC++で実装してみました。

##連立方程式解いてみた。
連立方程式$\mathrm{A}\mathbf{x}=\mathbf{b}$を解くには、行列AをLU分解し置換行列Q,対角成分が全て1の下三角行列(単位下三角行列)L,上三角行列Uによる積で$\mathrm{A}=\mathrm{QLU}$と表すと、先の方程式が$\mathrm{L}\mathbf{y}=\mathrm{Q}\mathbf{b},\mathrm{U}\mathbf{x}=\mathbf{y}$の二式を解くことと等価になり、計算がし易くなります。

これを実際にコードで書いてみます。今回は、置換行列Qを生成する代わりに$\mathbf{b}$の行入れ替えをします。

#include<iostream>
#include<iomanip>
#include<algorithm>
#include<vector>
#include<math.h>
using namespace std;

#define D 3 //行列の次数を設定(今は3次元)

void Lu();
void eq_solve();

double a[D][D],b[D]; //行列A,ベクトルb
double l[D][D],u[D][D]; //上三角行列L,単位下三角行列U
double x[D],y[D]; //xは連立方程式の解

int main(){
    /*
        Ax=bに対応するa[D][D],b[D]の要素を決める
    */
    Lu();
    eq_solve();   
    for(int i=0;i<D;i++) cout << x[i] << endl; //解を出力
}

//LU分解の関数
void Lu(){
    // ピボット選択
    for(int i=0;i<D;i++){ for(int j=0;j<D;j++) u[i][j]=a[i][j];}
    for(int k=0;k<D-1;k++){
        double hold_val=0;
        int hold_index=0;
        for(int i=k;i<D;i++){
            if(hold_val<abs(u[i][k])){
                hold_val=abs(u[i][k]);
                hold_index=i;
            }
        }
        
        if(hold_index!=k){
            for(int i=0;i<D;i++) swap(u[hold_index][i],u[k][i]);
            for(int i=0;i<D;i++) swap(l[hold_index][i],l[k][i]);
            swap(b[hold_index],b[k]);
        }

        //上三角行列Lと単位下三角行列Uの構成
        for(int i=0;i<k;i++) l[i][k]=0;
        l[k][k]=1.0;
        for(int i=k+1;i<D;i++){
            double cat=u[i][k]/u[k][k];
            l[i][k]=cat;
            for(int j=0;j<D;j++) u[i][j]-=u[k][j]*cat;
        }
    }  
}

//連立方程式を解く
void eq_solve(){
    //Ly=b
    y[0]=b[0];
    for(int i=1;i<D;i++){
        y[i]=b[i];
        for(int j=0;j<i;j++) y[i]-=l[i][j]*y[j];
    }
    //Ux=y
    x[D-1]=y[D-1]/u[D-1][D-1];
    for(int i=D-2;i>=0;i--){
        x[i]=y[i];
        for(int j=i+1;j<D;j++) x[i]-=u[i][j]*x[j];
        x[i]/=u[i][i];
    }
}

##LU分解で逆行列も求めてみた。
行列AをLU分解して
$\mathrm{A}=\mathrm{QLU}$としたとき、
$\mathrm{A}^{-1}=(\mathrm{QLU})^{-1}=\mathrm{U}^{-1}\mathrm{L}^{-1}\mathrm{Q},\ (\mathrm{Q}=\mathrm{Q}^{-1})$となります。このとき、Lは逆行列も下三角行列、Uは逆行列も単位上三角行列であることから、それぞれ逆行列を求めやすいです。

このことを考慮して逆行列を求めるプログラムを実装してみます。

#include<iostream>
#include<iomanip>
#include<algorithm>
#include<vector>
#include<math.h>
using namespace std;

#define D 3 //行列の次数を設定(今は３次元)

void Lu();
void inverse_L();
void inverse_U();

double a[D][D]; //逆行列を求めたい行列A
double l[D][D],u[D][D],q[D][D]; //LU分解で用いる行列
double l_[D][D],u_[D][D]; //下三角行列L,単位上三角行列Uの逆行列

int main(){
    /*
       a[D][D]の要素を決める
    */

    Lu();
    inverse_L();
    inverse_U();

    //Aの逆行列を計算
    vector<vector<double> > p(D,vector<double>(D,0));
    vector<vector<double> > a_(D,vector<double>(D,0)); //Aの逆行列を格納
    for(int i=0;i<D;i++){
        for(int j=0;j<D;j++){
            for(int k=0;k<D;k++){
                p[i][j]+=u_[i][k]*l_[k][j];
            }
        }
    }
    for(int i=0;i<D;i++){
        for(int j=0;j<D;j++){
            for(int k=0;k<D;k++){
                a_[i][j]+=p[i][k]*q[k][j];
            }
        }
    }
    //Aの逆行列を出力
    for(int i=0;i<D;i++){
        for(int j=0;j<D;j++){
            cout << a_[i][j] << ' ';
        }
        cout << endl;
    }

}

//LU分解する関数
void Lu(){
    // ピボット選択
    for(int i=0;i<D;i++){ for(int j=0;j<D;j++) u[i][j]=a[i][j];}
    for(int i=0;i<D;i++) q[i][i]=1.0;
    for(int k=0;k<D-1;k++){
        double hold_val=0;
        int hold_index=0;
        for(int i=k;i<D;i++){
            if(hold_val<abs(u[i][k])){
                hold_val=abs(u[i][k]);
                hold_index=i;
            }
        }
        
        if(hold_index!=k){
            for(int i=0;i<D;i++) swap(u[hold_index][i],u[k][i]);
            for(int i=0;i<D;i++) swap(l[hold_index][i],l[k][i]);
            for(int i=0;i<D;i++) swap(q[hold_index][i],q[k][i]);            
        }

        //上三角行列Lと単位下三角行列Uの構成
        for(int i=0;i<k;i++) l[i][k]=0;
        l[k][k]=1.0;
        
        for(int i=k+1;i<D;i++){
            double cat=u[i][k]/u[k][k];
            l[i][k]=cat;
            for(int j=0;j<D;j++) u[i][j]-=u[k][j]*cat;
        }
    }  
    l[D-1][D-1]=1.0;
}

//下三角行列Lの逆行列を求める
void inverse_L(){
    for(int k=0;k<D;k++){
        vector<double> x(D,0);
        x[k]=1.0;
        for(int i=k+1;i<D;i++){
            double tmp=0;
            for(int j=0;j<i;j++){
                tmp+=l[i][j]*x[j];
            }
            x[i]=-tmp;
        }  
        for(int i=0;i<D;i++) l_[i][k]=x[i];   
    }
}

//単位上三角行列Uの逆行列を求める
void inverse_U(){
    for(int k=D-1;k>=0;k--){
        vector<double> x(D,0);
        x[k]=1.0/u[k][k];
        for(int i=k-1;i>=0;i--){
            double tmp=0;
            for(int j=D-1;j>i;j--){
                tmp+=u[i][j]*x[j];
            }
            x[i]=-tmp/u[i][i];
        }  
        for(int i=0;i<D;i++) u_[i][k]=x[i];
    }
}

何かの参考にしていただければありがたいです。修正点などありましたらご意見ください。