/*--------------------------------------------------------------------------------
ハウスホルダー変換を用いたQR分解と最小二乗法
QR decomposition using Householder transformation and least square method
QR_decomp.c
--------------------------------------------------------------------------------*/
#include "least_sq.h"
int least_sq_with_QR_decomp( double y[ROW_NUM], double X[ROW_NUM][COL_NUM], double A[COL_NUM] )
{
int i, j;
double yy[ROW_NUM];
double R_diag[ROW_NUM]; // Rの対角部を格納, diagonal part of R is stored.
double QR[ROW_NUM][COL_NUM]; // QR分解の結果を格納, result of QR decomposition is stored.
// yとXの中身を変えないために、それらをそれぞれyyとQRにコピーして、
// QR分解とパラメータの推定ではyyとQRを使う。
// To keep the contents of y and X unchanged, copy them to yy and QR respectively
// and use yy and QR in the QR decomposition and parameter estimation.
for ( i = 0; i < ROW_NUM; i++ ) {
yy[i] = y[i];
for ( j = 0; j < COL_NUM; j++ ) {
QR[i][j] = X[i][j];
}
}
// QR分解の結果は、QRとR_diagに格納
if ( householder_QR_decomp( QR, R_diag ) != 0 ) {
printf("# householder_QR_decomp cannot be done.\n");
return -1;
}
householder_QR_solve( yy, QR, R_diag, A );
return 0;
}
// 行列Xをハウスホルダー変換でQR分解
// QR分解の結果は、QRとR_diagに格納
// QR decomposition of matrix X using the Householder transformation
int householder_QR_decomp( double QR[ROW_NUM][COL_NUM], double R_diag[ROW_NUM] )
{
int i, j, k;
double r, s, p;
// u^(k)の計算, calculation of u^(k)
for(k = 0; k < COL_NUM; k++){
// k列ノルム, k column norm
s = 0.0;
for( i = k; i < ROW_NUM; i++){
s += QR[i][k] * QR[i][k];
}
s = sqrt( s );
if( s < EPSILON ){ // ランク判定, rank-determination
return k; // 異常終了, abnormal termination
}
R_diag[k] = ( QR[k][k] > 0 )? -s:s; // R[k]決定, determination of R[k]
r = 1.0/(double)sqrt( R_diag[k] * ( R_diag[k] - QR[k][k] ) );
QR[k][k] = (QR[k][k] - R_diag[k]) * r;
for(i = k+1; i < ROW_NUM; i++){ // u^(k)決定, determination of u^(k)
QR[i][k] *= r;
}
for(j = k+1; j < COL_NUM; j++){ // H(u^(k))W^(k)
p = 0.0; // 内積, inner product
for(i = k; i < ROW_NUM; i++){
p += QR[i][j] * QR[i][k];
}
for( i = k; i < ROW_NUM; i++ ){
QR[i][j] -= p * QR[i][k];
}
}
}
return 0;
}
// XのQR分解の結果を用いて最小2乗問題 y = X A を解く
// solve the least-squares problem y = X A using the results of the QR decomposition of X
void householder_QR_solve( double yy[ROW_NUM], double QR[ROW_NUM][COL_NUM], double R_diag[ROW_NUM], double A[COL_NUM] )
{
int i, j, k;
double p;
for( k = 0; k < COL_NUM; k++ ){
p = 0.0;
for(i = k; i < ROW_NUM; i++){
p += yy[i] * QR[i][k];
}
for(i = k; i < ROW_NUM; i++){
yy[i] -= p * QR[i][k];
}
}
for(i = COL_NUM-1; i >= 0; i--){ // 後退代入, backward substitution
for(j = i+1; j < COL_NUM; j++) {
yy[i] -= QR[i][j] * yy[j];
}
yy[i] = yy[i]/(double)R_diag[i];
A[i] = yy[i];
}
}