直线的卡尔曼滤波器不收敛

Posted 2023-02-21

【中文标题】直线的卡尔曼滤波器不收敛

【英文标题】：Kalman Filter of a straight line doesn't converge

【发布时间】：2016-09-30 13:52:32

【问题描述】：

我正在尝试编写一种算法来估计和跟踪直线： 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

【问题讨论】：

您的流程模型与您的描述不符。您的代码正在根据您的描述估计 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]。

【参考方案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 变化，除此之外，您不应向 A 添加任何内容。

最后，我用上面的代码块进行了一些测试。根据您对 deltab2 的了解程度，您会得到很好的 x[k] 估计值。如果您不知道 deltab2，您可能仍然会得到非常好的预测，但估计的 x[k] 与实际值并不能很好地对应。

希望这会有所帮助！如果我遗漏了什么（比如一些附加信息），请发表评论，我可以相应地编辑我的答案！