0

直線を推定して追跡するアルゴリズムを作成しようとしています: y[k]=b1*x[k]+b2[k]. 私が扱っている実際の物理システムでは、y[k] しか測定できず、それを制御するための入力は x[k] です (x[k] を入力し、特定の y[k] を取得することを期待しています)。

問題は、y[k] と x[k] の関係が一定でないことです。勾配 b1 は反復 k ごとに一定ですが、定数 b2[k] はそうではありません。私が想定したもう1つのことは、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
4

1 に答える 1