Kalman直線のフィルタが収束しない

Question

私は直線を推定して追跡するアルゴリズムを作成しようとしています。 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 

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それ以外には何も追加しないでください。

最後に、上記のコードブロックでいくつかのテストを実行しました。あなたはdeltab2をどれだけうまく知っているかによって、x [k]の推定値はかなり良いでしょう。 deltab2についてわからないのであれば、非常に良い予測が得られるかもしれませんが、推定されたx [k]は実際の値に非常によく一致しません。

希望すると便利です。私が何か（追加の情報のように）見逃してしまった場合は、コメントしてください。