UTransformation

UT <- function(f, x, dims, P, k, method="chol"){
	# f    : 非線形あ関数
        # x    : サンプルするデータ点（平均点）
	# dims : xの次元
	# P    : xの共分散行列
        # k    : パラメータ(m+k=3になるように取るのがよいらしい, 0以上であればコレスキー分解の存在保証がされるそうで・・・)
        # method : 平方根行列を取る方法 コレスキー分解は"chol", 特異値分解は"svd"
	
	m  <- dims                            # 次数を保存
        N  <- (2*m)+1                         # 計算するシグマポイントの数
	w0 <- k/(m+k)                         # w0 を計算
	w  <- c(w0, rep((1)/(2*(m+k)),2*m ))  # wi を計算
        W  <- diag(w)
	x0 <- x        # x0 にxを保存
    
    if(method=="svd"){
        tmp    <- svd(P)                  # 特異値分解によるPの平方根行列を保存
        d      <- diag(sqrt(tmp$d))
        L      <- tmp$u %*% d
    }else if(method=="chol"){
    	L      <- chol(P)                 # コレスキー分解によるPの平方根行列を保存
    }else{
    	stop("methodが不正な値")
    	}
	
	sigma_points       <- matrix(0, nrow=N, ncol=m)   # シグマポイントを保存する行列を作成
	sigma_points[1, ]  <- x0   # シグマポイントの2列目以降にxpmsの値を保存	
	
	pm  <- function(a,b){ # プラスとマイナスを計算する関数
		r1 <- a+b
		r2 <- a-b
		return(list(a.plus.b=r1, a.minus.b=r2))
	}
	
	for(i in 1:m){
		tmp1 <- pm(x0, sqrt(m+k)*L[,i])	
		sigma_points[i+1, ]     <- as.numeric(tmp1[[1]])
		sigma_points[(i+m+1), ] <- as.numeric(tmp1[[2]])
	}
    
    # u変換によるyの近似
    tmp1 <- apply(sigma_points, 1, f)
    
    if(is.matrix(tmp1)){
    	Y   <- tmp1
        ym  <- apply(Y %*% W, 1, sum)
        ym.minus <- function(a, b=ym){
    	                    return(a-b)
                                     }
        Yd  <- t(apply(Y, 2, ym.minus))
    }else{ 
    	Y   <-  tmp1
    	ym  <- sum(Y %*% W)    
        Yd  <- Y - rep(ym, length(Y))
       	}
    
    Pyy <- t(Yd) %*% W %*% (Yd)

    if(sum(dim(Pyy))==2){
      Pyy <- as.numeric(Pyy)
    } 
    # 計算したシグマポイントとシグマウェイトをリストに格納して返す
    rslt.list <- list(ym=ym, Pyy=Pyy, Sigma_points=sigma_points, F_sgima=t(tmp1) ,Weight=W)
    return(rslt.list)
}