用于R中偏差估计的卡尔曼滤波器

Question

我正在尝试在简单模型上应用KF来估计信号的偏差。但该方法似乎不起作用。偏差误差估计为零并且不会改变。关于我缺少什么和/或KF是否适合这个问题的任何线索？

以下是模型演示：

x_(k+1) = x_k + T*V_k    --> Position
V_(k+1) = V_k + T*(a_k - b_k)    --> Velocity 
a_(k+1) = a_k    --> Acceleration
b_(k+1) = b_k   --> Bias

这是我的代码：

# Get state space matrices
T = 0.005;
numberOfStatesVariables = 4;
A = diag(numberOfStatesVariables);
A[1,2] = T;
A[2,3] = T;
A[2,4] = -T;
C = matrix(0, 1, numberOfStatesVariables);
C[1, 3] = 1;
Q = 1*diag(numberOfStatesVariables);
R = diag(1);
P = diag(numberOfStatesVariables);
# Specify observations
numberOfSamples = 5000;
t = 1:numberOfSamples / 2 / numberOfSamples;
w1 = sample(0:10, numberOfSamples, replace = T) / 10 - 0.5 + sin(t/pi*180);
yt = rbind(w1+0.5); # 0.5 is the bias error.

estimatedT = matrix(0, numberOfStatesVariables, numberOfSamples);

for (i in 2:numberOfSamples)
{
  estimatedT[, i] = A %*% estimatedT[, i-1];
  P = A %*% P %*% t(A) + Q;
  S = C %*% P %*% t(C) + R;
  K = P %*% t(C) %*% solve(S);
  estimatedT[, i] = estimatedT[, i] + K %*% (yt[,i] - C %*% estimatedT[, i]);
  P = (diag(numberOfStatesVariables) - K %*% C) %*% P;
}

plot.ts(estimatedT[4,])