Я надеваю петлю фильтра Калмана, почему отображаются несоответствующие массивы? - PullRequest
Новая
       53

Я надеваю петлю фильтра Калмана, почему отображаются несоответствующие массивы?

0 голосов
/ 31 марта 2019

Я пытаюсь сделать цикл фильтра Калмана, и он показывает, что несоответствующие массивы. 

rm(list=ls())
load("C:/Users/AR1.RData")
y=as.matrix(y)
T=nrow(y)
a=0.05; b=0.05; c=0.05; d=0.01
PHI=matrix(c((1-b),a,-c,(1+d)),ncol=2,byrow=T) 
Id=matrix(0,nrow(PHI),ncol(PHI))
diag(Id)=1
A=Id
sig.w=1
Q=(sig.w^2)*Id;

sig.v=1 
R=(sig.v^2)*Id;
xf=matrix(0,2,T);
x=matrix(0,2,T);   
eps=matrix(0,2,T);  
M=array(0,dim=c(T,2,2)) 
P=array(0,dim=c(T,2,2)) 

for (k in 1:(T-1))
+ {
+     xold=matrix(x[,k]) 
+     Pold=P[k,,] 
+     
+     xfnew=PHI%*%xold  
+     Mnew=PHI%*%Pold%*%t(PHI)+Q   
+     
+     xf[,k+1]=matrix(xfnew) 
+     M[k+1,,]=Mnew; 
+     
+     
+     yobs=matrix(y[k+1,])
+     Pnew=solve(Mnew)+t(A)%*%solve(R)%*%A; 
+     Pnew=solve(Pnew); 
+     K=Pnew%*%t(A)%*%solve(R); 
+     xnew=xfnew+K%*%(yobs-A%*%xfnew); 
+     
+     x[,k+1]=xnew; 
+     P[k+1,,]=Pnew;
+     
+     eps[,k+1]=yobs-A%*%xfnew 
+     
+ }
...