確率システム制御の基礎（確率システム制御の基礎　椹木 義一先生）

```{r}
#確率システム制御の基礎　p.5


A=t(matrix(c(-1,2,0,1),nrow=2,ncol=2));B=c(0,1)

P=eigen(A)$vectors

if(P%*%diag(eigen(A)$values,2)%*%solve(P)==A){
  
  print("result is right!")
  
}

#trans_prob

trans_prob=function(x){
  
z<- P%*%diag(exp(eigen(A)$values*x),nrow(A))%*%solve(P) 
  
return(z) 
 
}

#initial condition x:

x0=c(1,1);

#u

u=function(x){
  
z<-1/(x+1)

return(z)
  
}

#C

C=function(x){
  
return(matrix(c(sin(x),cos(x),-sin(x),sin(x)),nrow=2,ncol=2))  
  
}

#inpulse:H

H_t=function(x){
  
if(x>=0){
  
return(C(x)%*%trans_prob(x)%*%B)  
  
}else{
  
return(0)  
  
}
 
}

#G

G=function(x,s){
  
z<-C(x)%*%solve(s*diag(2)-A)%*%B
  
return(z)   
  
  
}

time=10

x_data=data.frame(t=seq(0,time,0.1),x1=0,x2=0,y1=0,y2=0,H1=0,H2=0,G1=0,G2=0)

for(k in 1:nrow(x_data)){

#states:x  
  
t=x_data$t[k];

x=trans_prob(t)%*%x0

func=function(x){
  y<-trans_prob(x)%*%B*u(x)
  
  return(y)
}

a=0;H=t;n=100000

Y=(H/3)*(1/(n-1))*(func(a)+func(a+H));

for(j in 1:(n-2)){
  coefficient=ifelse(j-floor(j/2)*2==0,4,2)
  Y=Y+((H*coefficient)/(3*(n-1)))*func(a+(j*H)/(n-1))

}

x=x+Y

x_data$x1[k]=x[1];x_data$x2[k]=x[2];

#output:y

x=C(t)%*%trans_prob(t)%*%x0

func=function(x){
  y<-C(x)%*%trans_prob(x)%*%B*u(x)
  
  return(y)
}

a=0;H=t;n=100000

Y=(H/3)*(1/(n-1))*(func(a)+func(a+H));

for(j in 1:(n-2)){
  coefficient=ifelse(j-floor(j/2)*2==0,4,2)
  Y=Y+((H*coefficient)/(3*(n-1)))*func(a+(j*H)/(n-1))

}

x=x+Y

x_data$y1[k]=x[1];x_data$y2[k]=x[2]

x_data$H1[k]=H_t(t)[1];x_data$H2[k]=H_t(t)[2]

x_data$G1[k]=G(t,2)[1];x_data$G2[k]=G(t,2)[2]

}

write.csv(x_data,"~/stochastic_control_data.csv")
```

```{r}

#x1,x2

plot(x_data$t,x_data$x1,type="o",col=2,xlim = c(min(x_data$t),max(x_data$t)),ylim=c(min(x_data[,colnames(x_data) %in% c("x1")]),max(x_data[,colnames(x_data) %in% c("x1")])),main="x1",xlab="t",ylab="values")

plot(x_data$t,x_data$x2,type="o",col=3,xlim = c(min(x_data$t),max(x_data$t)),ylim=c(min(x_data[,colnames(x_data) %in% c("x2")]),max(x_data[,colnames(x_data) %in% c("x2")])),main="x2",xlab="t",ylab="values")

#y1,y2

plot(x_data$t,x_data$y1,type="o",col=2,xlim = c(min(x_data$t),max(x_data$t)),ylim=c(min(x_data[,colnames(x_data) %in% c("y1")]),max(x_data[,colnames(x_data) %in% c("y1")])),main="y1",xlab="t",ylab="values")


plot(x_data$t,x_data$y2,type="o",col=3,xlim = c(min(x_data$t),max(x_data$t)),ylim=c(min(x_data[,colnames(x_data) %in% c("y2")]),max(x_data[,colnames(x_data) %in% c("y2")])),main="y2",xlab="t",ylab="values")

#H1,H2

plot(x_data$t,x_data$H1,type="o",col=2,xlim = c(min(x_data$t),max(x_data$t)),ylim=c(min(x_data[,colnames(x_data) %in% c("H1")]),max(x_data[,colnames(x_data) %in% c("H1")])),main="H1",xlab="t",ylab="values")


plot(x_data$t,x_data$H2,type="o",col=3,xlim = c(min(x_data$t),max(x_data$t)),ylim=c(min(x_data[,colnames(x_data) %in% c("H2")]),max(x_data[,colnames(x_data) %in% c("H2")])),main="H2",xlab="t",ylab="values")

```
```{r}

#Hamilton-Jacobi-Bellman equation


#確率システム制御の基礎　p.61

a=1;b=3

#dx/dt=-a*x(t)+b*u(t)+w(t),E[w(t)]=0,V[w(t)]=1

#sample_path:x(t)

u=function(s){
  
z<-1/(1+s)

return(z)
  
}

t=100;sep=1;x0=1

x_data=data.frame(t=seq(0,t,sep),x=0)

x_data$x[1]=x0;

for(j in 2:nrow(x_data)){
  
x_data$x[j]=(1-a)*x_data$x[j-1]+b*u(x_data$t[j-1])+rnorm(1) 

}

plot(x_data$t,x_data$x,type="o",col=2,xlab="t",ylab="x")

J=function(s){
  
m=100;expect=c();

for(k in 1:m){

initial_value=0;H=s;n=100000

data=data.frame(t=seq(0,s,H/(n-1)),x=0,u=0)

data$x[1]=x0;data$u[1]=u(0)

for(j in 2:nrow(data)){
  
data$x[j]=(1-a)*data$x[j-1]+b*u(data$t[j-1])+rnorm(1)

data$u[j]=u(data$t[j])
  
}

data=data %>% dplyr::mutate(x2=x^2,u2=u^2)

Y=(H/3)*(1/(n-1))*(data$x2[1]+data$u2[1]+data$x2[nrow(data)]+data$u2[nrow(data)]);

for(j in 2:(n-1)){
  coefficient=ifelse(j-floor(j/2)*2==0,4,2)
  Y=Y+((H*coefficient)/(3*(n-1)))*(data$x2[j]+data$u2[j])
  #print(Y)

}

expect=c(expect,Y)

print(k)

}

return(mean(expect))

}

#Riccati equation(-dp/dt=1-2*a*p+p*b^2)

F_matrix=t(matrix(c(-a,-b^2,-1,a),nrow=2,ncol=2))

gamma=eigen(F_matrix)$values

matrix1=diag(2);matrix2=F_matrix/abs(max(gamma))

K1=K2=array(0,dim=c(2,2))

for(i in 1:2){
for(j in 1:2){ 
  
vec=c(matrix1[i,j],matrix2[i,j]);vec=t(t(vec)) 
  
matrix=matrix(c(1,1,1,-1),nrow=2,ncol=2)  

sub_m1=matrix;sub_m2=matrix;

sub_m1[,1]=vec;sub_m2[,2]=vec;

K1[i,j]=det(sub_m1)/det(matrix);

K2[i,j]=det(sub_m2)/det(matrix)
  
}  
}

trans_func=function(r,s){
  
z<-K1*exp(-gamma[1]*(r-s))+K2*exp(-gamma[2]*(r-s))

return(z)
  
}

p=function(s){
  
z<-(K1*exp(-gamma[1]*(s))+K2*exp(-gamma[2]*(s)))[2,1]/(K1*exp(-gamma[1]*(s))+K2*exp(-gamma[2]*(s)))[1,1]  

return(z)  
  
}

#最適な強制力:u0=-b*p(t)*x(t)

t=1;sep=0.01;x0=1

x_data=data.frame(t=seq(0,t,sep),J0=0,p=0,x=0,u0=0)

x_data$x[1]=x0;

for(k in 2:nrow(x_data)){
  

  
Y=(x0^2)*p(t-0)

func=function(x){
  
  y<-p(t-x)
  
  return(y)
}

initial_a=0;H=x_data$t[k];n=100000

Y=Y+(H/3)*(1/(n-1))*(func(initial_a)+func(a+H));

for(j in 1:(n-2)){
  
  coefficient=ifelse(j-floor(j/2)*2==0,4,2)
  
  Y=Y+((H*coefficient)/(3*(n-1)))*func(initial_a+(j*H)/(n-1))

}

x_data$J0[k]=Y

x_data$p[k]=p(t-x_data$t[k])

x_data$x[k]=((1-a)-(b^2)*p(t-x_data$t[k-1]))*x_data$x[k-1]+rnorm(1)

x_data$u0[k]=-b*p(t-x_data$t[k])*x_data$x[k]

}

data=x_data

x_data=tail(x_data,nrow(x_data)-1)

```


```{r}

plot(x_data$t,x_data$p,type="o",col=2,xlab="t",ylab="p")

plot(x_data$t,x_data$J0,type="o",col=2,xlab="t",ylab="J0")

plot(x_data$t,x_data$u0,type="o",col=2,xlab="t",ylab="u0",xlim=c(0,t))

plot(x_data$t,x_data$x,type="o",col=2,xlab="t",ylab="x",xlim=c(0,t))

```