卡尔曼滤波器不收敛

我试图写估计并跟踪straigh线的算法： Y [k]的= B1 * X [k]的+ B2 [K]。在我使用的真实物理系统中，我只能测量y [k]，并控制输入是x [k]（我输入x [k]并期望得到特定的y [k]）。卡尔曼滤波器不收敛

的问题是，Y [k]和X [k]的关系不是恒定的：斜率b1为每迭代k恒定的，而是恒定B2 [K]是没有的。我假设的另一件事是：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

来源

2016-09-30 Shaked

您的过程模型与您的描述不符。你的代码是从你的描述中估计'x [k]'，但是在你的描述中它听起来像'x [k]'。如果'x [k]'真的处于你的状态，那么'H'不会评估'mx + b'。评估'x [2] * x [1] + x [3]'不会是一个线性矩阵运算。 –

我会说清楚 - x [k]没有给出。我想估计正确的x [k]，所以它会得到我想要的y [k]。当v和b2是我的状态向量的两个元素时，H正在评估beta [k] = b1 * v [k] + b2 [k]。 – Shaked

（我假设你知道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]并不能很好地对应真实值。

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

来源

2016-10-19 07:29:47 tele

卡尔曼滤波器不收敛

回答

相关问题