卡爾曼濾波器不收斂

我試圖寫估計並跟蹤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

卡爾曼濾波器不收斂

回答

相關問題