Question

我正在尝试编写估算和跟踪直线的算法： Y [k]的= B1 * X [k]的+ B2 [K]。在我使用的真实物理系统中，我只能测量y [k]，并且控制输入是x [k]（我输入x [k]并期望获得特定的y [k]）。

问题是y [k]和x [k]的关系不是常数：斜率b1对于每次迭代k都是常数，但常数b2 [k]不是“t”。我假设的另一件事是：deltab2 [k] = b2 [k] -b2 [k-1]，并且它每次迭代都是常数。

我尝试使用卡尔曼滤波器，状态向量=（x [k]，b2 [k]，Delatb2 [k]），测量值= y [k]。它没有工作 - 卡尔曼增益实际上变为零，误差协方差矩阵没有收敛。我知道收敛问题与系统的可观察性有关。但是我有点麻烦使我的模型可观察。如何使我的算法工作？

% note - y[k] is beta here, x[k] is v.

A=[1 0 -1/b1;0 1 1;0 0 1];
H=[b1 1 0];

% varb2 = b2[k] variance
% varb2' = b2[k-1] variance
% varbeta = measurement noise variance
% covbbt = b2[k], b2[k-1] covariance - assumed to b2 0

Qk=varb2*[1/b1^2 -1/b1 -1/b1;-1/b1 1 1; -1/b1 1 1]+covbbt*[0 0 1/b1; 0 0 -1; 1/b1 -1 -2]+varb2t*[0 0 0; 0 0 0; 0 0 1]+varbeta*[1 0 0; 0 0 0; 0 0 0];
Rk=varbeta;
P=Qk;
x=[5,handles.b(2),0].'; %Assuming the initial drift is 0

% b1 is assumed to be 200, b2[k=1] assumed to be -400

%% the algorithm
v=5;
while(get(handles.UseK,'Value'))
    %get covariances
    x_est=A*x
    P_est=A*P*A.'+Qk
    sample_vector = handles.s_in1.startForeground();
    I = mean(sample_vector(:,2));% average of the 200 samples
    Q = mean(sample_vector(:,1));% average of the 200 samples
    beta=unwrap(atan2(I,Q)); % measurment of beta 
    K=P*H.'*inv(H*P*H.'+Rk) %kalman gain
    x=x_est+K*(beta-H*x_est)
    P=P_est-K*H*P_est
    vo=v;
    v=x(1);
    outputSingleScan(handles.s_output1,v);
end

Answer 1

（我假设你知道b1）

在你的方法中，这一切都归结为你对deltab2的了解程度。如果你没有非常强烈的猜测，问题会变得非常困难。如果您对deltab2有很强的猜测，可以将该信息作为先前（初始状态）提供给您的算法。

假设你对deltab2有一个强烈的猜测，你可以尝试这样的事情：

% State is [x[k], b2[k], deltab2[k]]
state = [0 0 deltab2]
C = [100 0 0;0 100 0;0 0 0.001];

% We predict with the dynamics we know of
A = [1 0 0;0 1 1;0 0 1];

% The observation model assumes we know b1
H = [b1 1 0];

% Identity process noise
Q = [1 0 0;0 1 0;0 0 0.001];

% Some observation noise
R = 1;

% Assume observations are stacked in vector y
for i = 1:size(y,1)
   m = state(end,:)';
   P = C(:,:,end);

   % Predict
   M = A * M;
   P = A * P * A' + Q;


   % Update
   mu = H * M;
   nu = y(i) - mu;

   S = H * P * H' + R;
   K = P * H' / S;

   M = M + K * nu;
   P = P - K * S * K';

   state(end+1,:) = M;
   C(:,:,end+1) = P;
end

显然，如果你对过程噪音和/或观察噪音有更好的猜测，你可以使用它们。在上面，我们使用我们知道的动力学：b2 deltab2和deltab2的变化是恒定的（具有强烈的初始猜测）。其他一切都是未知的。

在你的方法中你已经在A的动力学中设置了一些其他假设。我不知道你在哪里或如何提出这些假设，但如果系统中唯一已知的东西是deltab2是常数而b2根据deltab2而变化，除此之外，你不应该向A添加任何东西。

最后，我使用上面的代码块运行了一些测试。根据你对deltab2的了解程度，你会得到相当好的x [k]估计值。如果你不知道deltab2，你可能会得到非常好的预测，但是估计的x [k]并不能很好地与实际值相对应。

希望这有帮助！如果我遗漏了某些内容（比如一些其他信息），请发表评论，我可以相应地编辑我的答案！

直线的卡尔曼滤波器不会收敛

1 个答案: