カルマンUD分解フィルター（例）

#kalman U-D decomposition filter

library(dplyr)

n=5

H=sample(c(1:10),n,replace=T)

x=rnorm(n,mean=4,sd=3);y=2*x+9

#initial conditions

x=rep(1,n);P=diag(1,n)

R=5

times=10000

for(j in 1:times){

f=P%*%(H)   

a=H%*%f+R  

K=f/c(a)

x=x+K*(y-H*x)

P_past=P

P=P-K%*%t(f)

print(sum(abs(K)))

#print(P-P_past)



}

plot(c(1:n),y,type="o",col=2,ylim=c(min(c(H*x)+mean(y)-mean(H*x),y),max(c(H*x)+mean(y)-mean(H*x),y)),xlab="samples",ylab="values",main=paste0(cor(c(H*x)+mean(y)-mean(H*x),y)))

par(new=T)

plot(c(1:n),c(H*x)+mean(y)-mean(H*x),type="o",col=3,ylim=c(min(c(H*x)+mean(y)-mean(H*x),y),max(c(H*x)+mean(y)-mean(H*x),y)),xlab="samples",ylab="values",main=paste0(cor(c(H*x)+mean(y)-mean(H*x),y)))

#=>K

P%*%H/c(H%*%P%*%H+R)

f_mat=matrix(sample(1:10,n^2,replace = T),ncol=n)

G_mat=matrix(sample(1:10,n^2,replace = T),ncol=n)

x_vec=f_mat%*%x

P_bar=f_mat%*%P%*%t(f_mat)