0

我正在尝试编写一种算法来估计和跟踪直线:y[k]=b1*x[k]+b2[k]。在我使用的真实物理系统中,我只能测量 y[k],为了控制它,输入是 x[k](我输入 x[k] 并期望得到一个特定的 y[k])。

问题是 y[k] 和 x[k] 的关系不是恒定的:每次迭代 k 的斜率 b1 都是恒定的,但常数 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
4

1 回答 1

0

(我假设你知道 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-19T07:29:47.877 回答