Moving frameを取り出す

• なんだかんだと時間がかかったが、観測点が等間隔ではないときに観測点(付近)のMoving Frameを取り出し、その変化がフルネ=セレ的行列で説明できるようにする関数ができた
• さて、実データでは、観測値に乱雑項が入るのだが、乱雑項はMoving Frameの高次ベクトルになればなるほど大きく影響を与え、「すごく滑らか」に観測されていていも、高次(とは言っても２次から)ではばらっばらです

my.Moving.Frame.4 <- function(X,k=length(X[1,])){
	n <- length(X[,1]) # No. points
	d <- length(X[1,]) # Dimension
	# inter-point distance
	diff.X <- list()
	diff.X[[1]] <- apply(X,2,diff)
	
	for(i in 2:k){
		diff.X[[i]] <- apply(diff.X[[i-1]],2,diff)
	}
	# ベクトル差分^iの長さ
	tmp.fx <- function(q){
		sqrt(apply(q^2,1,sum))
	}
	diff.L <- lapply(diff.X,tmp.fx)
	L <- diff.L[[1]]
	cumsum.L <- list()
	cumsum.L[[2]] <- L
	
	# ベクトル差分の垂直成分の長さ
	diff.L.rect <- list()
	diff.L.rect[[1]] <- diff.L[[1]]
	# その曲線長補正値：それが曲率(のようなもの)
	diff.L.rect.st <- list()
	diff.L.rect.st[[1]] <- diff.L.rect[[1]]/diff.L[[1]]
	mf <- list() # Moving Frame
	mf[[1]] <- diff.X[[1]]/diff.L[[1]]
	if(k>1){
		for(i in 2:(k-1)){
			cumsum.L[[i]] <- cumsum(cumsum.L[[i-1]])
			n.pt <- length(diff.X[[i]][,1])
			mf[[i]] <- diff.X[[i]]
			ips <- matrix(0,n.pt,i-1)
			for(j in 1:(i-1)){
				ips[,j] <- apply(diff.X[[i]] * mf[[j]][1:n.pt,],1,sum)
				mf[[i]] <- mf[[i]] - ips[,j] * mf[[j]][1:n.pt,]
			}
			#ip <- apply(diff.X[[i-1]] * mf[[i-1]][1:n.pt,],1,sum)
			#mf[[i]] <- diff.X[[i-1]] - ip * mf[[i-1]][1:n.pt,]
			diff.L.rect[[i]] <- sqrt(apply(mf[[i]]^2,1,sum))
			diff.L.rect.st[[i]] <- diff.L.rect[[i]]/cumsum.L[[i]][1:n.pt]
			mf[[i]] <- mf[[i]]/diff.L.rect[[i]]
		}
	}
	n.pt <- length(diff.X[[k]][,1])
	mf[[k]] <- matrix(0,n.pt,d)
	for(i in 1:n.pt){
		tmp.mat <- matrix(0,d-1,d-1)
		tmp.vec <- rep(0,d-1)
		for(j in 1:(k-1)){
			tmp <- mf[[j]][i,]
			tmp.mat[j,] <- tmp[1:(d-1)]
			tmp.vec[j] <- tmp[d]
		}
		mf[[k]][i,] <- c(-solve(tmp.mat) %*% tmp.vec,1)
	}
	mf[[k]] <- mf[[k]]/sqrt(apply(mf[[k]]^2,1,sum))
	
	Cs <- matrix(0,d-1,length(diff.L[[1]]))
	for(i in 1:(d-1)){
		n.pt <- length(diff.L[[i+1]])
		Cs[i,1:n.pt] <- diff.L[[i+1]][1:n.pt]/L[1:n.pt]
	}
	mf.array <- diff.array <- array(0,c(n,k,k))
	mf.array.2 <- array(0,c(n,k,k))
	for(i in 1:k){
		tmp.n <- length(mf[[i]][,1])
		delta.n <- n-tmp.n
		init.n <- ceiling(delta.n/2)
		if(init.n==0)init.n <- 1
		r.id <- init.n:(init.n+tmp.n-1)
		mf.array[r.id,i,] <- mf[[i]]
		mf.array.2[1:tmp.n,i,] <- mf[[i]]
		tmp.n <- length(diff.X[[i]][,1])
		delta.n <- n-tmp.n
		init.n <- ceiling(delta.n/2)
		if(init.n==0)init.n <- 1
		r.id <- init.n:(init.n+tmp.n-1)
		diff.array[r.id,i,] <- diff.X[[i]]
	}
	# mf.array.2を移行させる行列
	R.list <- R.list.st <- list()
	R.array <- array(0,c((n-1),k,k))
	for(i in 1:(n-1)){
		R.list[[i]] <- matrix(0,k,k)
		if(det(mf.array.2[i,,]) * det(mf.array.2[i+1,,]) !=0){
			R.list[[i]] <- mf.array.2[i+1,,] %*% t(mf.array.2[i,,])
			R.array[i,,] <- mf.array.2[i+1,,] %*% t(mf.array.2[i,,])
		}
	}
	return(list(L=L,mf=mf,mf.array=mf.array,mf.array.2=mf.array.2,diff.X=diff.X,diff.array=diff.array,diff.L=diff.L,diff.L.rect=diff.L.rect,diff.L.rect.st=diff.L.rect.st,Cs=Cs,R.list=R.list,R.array=R.array))
}
library(deSolve)
# 微分方程式
# この関数my.ODEに 引数 t は使われていないが
# runge-kuttaの本体関数 rk() との関係で、この引数は必要(らしい)
# 時刻を表す引数
# xは対象となっている変数の値のベクトル
# parmsは微分方程式の係数
# 差分を計算して返す
# この連立微分方程式はロトカ=ヴォルテラ
# parms[1],parms[4]は負、parms[2],parms[3]は正
my.ODE <- function(t, x, parms)  {

	dX <- (parms[1] + parms[2]*x[2])*x[1]
	dY <- (parms[3] + parms[4]*x[1])*x[2]

	res <- c(dX, dY)
	list(res)

}


## The parameters 
# 連立微分方程式の４係数を決める
# ２タイプあって、回転方向が逆
library(MCMCpack)
n <- 2
A1 <- rdirichlet(1,rep(1,4))*5
A1[c(1,4)] <- -A1[c(1,4)]
#A2 <- rdirichlet(1,rep(1,4))*5
#A2[c(1,4)] <- -A2[c(1,4)]
A2 <- -A1 + rnorm(4)*0.01
## vector of timesteps
# 計算する時刻
times <- seq(0, 2, length = 51)
times <- sort(sample(times,20))
## 初期値
# 固定点の周囲から始まるとうまく周期性を出すので
# 固定点に若干のずれを入れる

N <- 100
X <- list()
mfouts <- list()
dets <- matrix(0,length(times),N)
types <- c()
for(i in 1:N){


if(runif(1)<0.5){
	xstart <- c(-A1[3]/A1[4], -A1[1]/A1[2]) + rnorm(2)*0.1

out1  <- rk(xstart, times, my.ODE, A1, hini = 0.1, method = "rk4")
types <- c(types,1)
}else{
	xstart <- c(-A2[3]/A2[4], -A2[1]/A2[2]) + rnorm(2)*0.1

out1  <- rk(xstart, times, my.ODE, A2, hini = 0.1, method = "rk4")
types <- c(types,2)
}
X[[i]] <- out1[,2:(n+1)]
mf1 <- my.Moving.Frame.4(X[[i]])
mfouts[[i]] <- mf1
dets1 <- c()
for(ii in 1:length(mf1$diff.array[,1,1])){
	dets1 <- c(dets1,det(mf1$diff.array[ii,,]))
}

dets[,i] <- dets1
}
a <- which(types==1)[1]
b <- which(types==2)[1]
plot(X[[1]],type="l",xlim=range(c(X[[a]][,1],X[[b]][,1])),ylim=range(c(X[[a]][,2],X[[b]][,2])),col=types[1])
for(i in 2:N){
	lines(X[[i]],col=types[i])
}
matplot(dets,type="l",col=types)

plot(X[[1]],pch=20,cex=0.01,asp=TRUE)
rrr <- range(X[[1]])
rrr2 <- rrr[2]-rrr[1]
for(i in 1:length(mfouts[[1]]$mf)){
	for(j in 1:length(mfouts[[1]]$mf[[i]][,1])){
			segments(X[[1]][j,1],X[[1]][j,2],X[[1]][j,1]+mfouts[[1]]$mf[[i]][j,1]*rrr2*0.05,X[[1]][j,2]+mfouts[[1]]$mf[[i]][j,2]*rrr.2*0.05,col=i)
	}
}


t <- seq(from=0,to=1,length=200)
x <- exp(0.3*t)*cos(20*t)
y <- exp(t)*sin(20*t)
z <- exp(t)
w <- exp(2*t)
h <- t^2
X <- cbind(x,y,z,w,h)
X <- X + rnorm(length(X),0,0.0001)

s.t <- sort(sample(1:length(t),100))
X <- X[s.t,]
mf1 <- my.Moving.Frame.4(X)
library(rgl)
X.st <- rbind(X,rep(min(X),3))
X.st <- rbind(X.st,rep(max(X),3))
plot3d(X.st)
for(i in 1:length(mf1$mf)){
	for(j in 1:length(mf1$mf[[i]][,1])){
		segments3d(matrix(c(X[j,1],X[j,2],X[j,3],X[j,1]+mf1$mf[[i]][j,1],X[j,2]+mf1$mf[[i]][j,2],X[j,3]+mf1$mf[[i]][j,3]),byrow=TRUE,nrow=2),col=i)
		#segments3d(X[j,1],X[j,2],X[j,3],X[j,1]+0.1,X[j,2]+0.1,X[j,3]+0.1)
	}
}
matplot(s.t,X,type="l")

for(i in 1:100){
	v1 <- mf1$mf[[1]][i,]
	v2 <- mf1$mf[[2]][i,]
	v3 <- mf1$mf[[3]][i,]
	V <- cbind(v1,v2,v3)
	print(V %*% t(V))
}

plot(mf1$diff.L.rect[[2]])
plot(mf1$diff.L.rect.st[[2]])
plot(mf1$diff.L.rect[[3]])
plot(mf1$diff.L.rect.st[[3]],type="l")

apply(mf1$mf.array.2,1,det)
matplot(cbind(mf1$R.array[,1,],mf1$R.array[,2,],mf1$R.array[,3,]),type="l")

for(i in 1:length(mf1$R.array[,1,1])){
	tmp <- eigen(mf1$R.array[i,,])
	print(tmp[[1]])
}

for(i in 1:length(mf1$R.array[,1,1])){
	tmp <- eigen(mf1$R.array[i,,])
	print(tmp)
	tmp.eigen.v <- tmp[[1]]
	tmp.eigen.mat <- tmp[[2]]
	M <- Mod(tmp.eigen.v)
	R <- Arg(tmp.eigen.v)
	tmp.M <- M^(1/mf1$L[i])
	tmp.R <- R/mf1$L[i]
	new.eigen.v <- tmp.M * cos(tmp.R) + 1i * tmp.M * sin(tmp.R)
	new.R <- tmp.eigen.mat %*% diag(new.eigen.v) %*% t(tmp.eigen.mat)
	print(new.R)
}
par(ask=TRUE)
for(k in 1:length(X[,1])){
	#print(round((mf1$R.array[k+1,,]-mf1$R.array[k,,]) %*% t(mf1$R.array[k,,]),8))
	tmp <- (mf1$R.array[k+1,,]-mf1$R.array[k,,]) %*% t(mf1$R.array[k,,])
	crds <- which(abs(tmp)>-1,arr.ind=TRUE)
	plot(crds,col=sign(tmp)+ 2,cex=abs(tmp)/max(abs(tmp)) * 10,pch=20)
}