用于一维向量的卡尔曼滤波器实现

Question

我很难搞清楚我认为真正愚蠢的事情。

我想刷新我的卡尔曼滤波器知识，但我无法使其工作。

在一个简单的案例中 真实状态x：

x = [   1000    750 563 422 316 237 178 133 100 75]';

和观察

z_array = [ 1056    943 469 235 433 230 116 136 -75 267]';

我得到了一些非常好的东西（估计代表FILTERED）：

相反，当我使用另一个信号时，更复杂。我不知道为什么，但它不起作用（估计代表FILTERED）。

问题是，我很确定如何在第二种情况下在代码中选择a和r。但是我怎么能改变它们呢？

这是我在MATLAB中编写的简短代码，我迷路了（我真的很惭愧，因为我应该知道怎么做!!）。

close all

z_array = noisyMeas.signals.values;
x = meas.signals.values;

a = 0.75;
r= 200;
p=1;

% x = [ 1000    750 563 422 316 237 178 133 100 75]';
% z_array = [   1056    943 469 235 433 230 116 136 -75 267]';

xhat_array = zeros(length(z_array),1);
xhat = z_array(1);
xhat_array(1) = xhat;

% Core of the Kalman Filter 
for k=2:length(x)
    z = z_array(k);

    % Predict
    xhat = a * xhat;
    p    = a * p * a;

    % Update
    g    = p  / (p  + r);
    xhat = xhat + g * (z - xhat);
    p    = (1 - g) * p;
    xhat_array(k) = xhat;
end

% plotting
figure
hold on
plot(xhat_array,'r')
plot(x,'b');
plot(z_array,'g')
grid on
legend('estimated','notNoisy','Noisy')

提前感谢您的帮助。