Kalman Filter of a straight line doesn't converge

Question

I'm trying to write an algorithm that estimates and tracks a straigh line: y[k]=b1*x[k]+b2[k]. In the real physical system I work with, I can only measure y[k], and to control it the input is x[k] (I enter x[k] and expect to get a specific y[k]).

The problem is that the relation of y[k] and x[k] isn't constant: the slope b1 is constant for every iteration k, but the constant b2[k] isn't. Another thing I assumed was that: deltab2[k]=b2[k]-b2[k-1], and it's constant for every iteration.

I tried to use Kalman Filter, with state vector = (x[k], b2[k], Delatb2[k]), and measurement = y[k]. It didnt work - the kalman gain turned practicaly zero and the error covariance matrix didn't converge. I understood that the convergence issue relates to the observability of the system. However i have a bit trouble making my model observable. How can I make my algorithm work?

% 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

(I'll assume you know the b1)

In your approach, it all really boils down to how well you know deltab2. If you don't have very strong guess for it, the problem becomes quite hard. If you have strong guess for deltab2, you can give that information as a prior (initial state) to your algorithm.

Assuming you have strong first guess for deltab2, you could try something like this:

% 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

Obviously if you have better guesses for the process noise and/or observation noise, you can use those. In the above, we use the dynamics that we know: b2 changes by deltab2 and deltab2 is constant (with strong initial guess). Everything else is unknown.

In your approach you had set some other assumptions inside dynamics of A. I dont know where/how you came up with those, but if the only known things from the system are that deltab2 is constant and b2 changes according to deltab2, you should not add anything to A besides that.

Finally, I ran some tests with the above code block. You'll get pretty good estimates for x[k] depending on how well you know the deltab2. If you have no idea about deltab2, you might get very good prediction still, but the estimated x[k] does not correspond to the real values very well.

Hope this helps! If I missed something (like some additional information), please comment and I can edit my answer accordingly!