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