Matlab: Kalman Filter -- How to mitigate the Warning: Matrix is singular or badly scaled

Question

When performing the innovation update for Kalman filter, I am getting Warnings

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 2.169130e-017.

Maybe due to this, the result is not accurate. How can I solve this problem? I tried introducing a loop

[R,p] = chol(Ppred);
if p> 0;
                count = 300;
              return;
          end

where count is just a variable to stall the code until a good matrix is found. but this does not help.

UPDATE:

Linear system representation of Moving Average, MA(2) model

 x_n+1 = Bw_n
 y_n = Cx_n + v_n
% w = N(0,Q); v = N(0,R)
%true coefficients, h = [1 0.5 -0.9];

CODE SNIPPETS

These are the functions for the 3 modules of Kalman Filter

C = [ 1 0 0 ];

B = [1 0.5  -0.9 ;
     0  1     0.5;
     0  0     1]; 

noise_var = rand(1,1); % measurement noise

 order = 2;
 xpred = rand(order,1); 
 P = 10* eye(d,d);

 A = P;
 P = P + B*sqrt(noise_var)*B';
P = dlyap(A,B*B');
for i = 1:N
 [xpred, Ppred] = predict(xpred,B,Ppred, Q); 

  [nu, S] = innovation(xpred, Ppred, y(i), C, noise_var);

  [xnew, Ppred, yhat, KalmanGain] = innovation_update(xpred,Ppred,nu,S,C);
    if(isnan(Ppred))
         count = 300;
         return;
    end

end

function [xpred, Ppred] = predict(input_t,B,P, Q)
xpred =  B*input_t;
Ppred = P + Q;
end

function [nu, S] = innovation(xpred, Ppred, y, C, R)
nu = y - C*xpred; %% innovation

S = R + C*Ppred*C'; %% innovation covariance

end

function [xnew, Pnew, yhat, K] = innovation_update(xpred, Ppred, nu, S, C)
K1 = Ppred*C';
K = K1'*inv(S);
xnew = xpred + K'*nu; %% new state
Pnew = Ppred - Ppred*K'*C; %% new covariance
 yhat = C*xnew;

end

I would recommend providing code that reproduces your issue. — excaza
@excaza: Please see my update, where I have put up the code. Thank you for help. — SKM

Ben Jackson Ben Jackson · Accepted Answer · 2015-09-12T21:28:09

Numerical issues are a common problem with Kalman filters. You didn't share enough of your code to be sure (especially Q), but it is common for roundoff errors to cause P to become non-positive-definite (especially with the P update form you used).

If you google "Kalman filter numerical stability" you can find a lot of references on the subject. Simple things to try in this case would be increasing Q (aka "fictitious process noise") to avoid an ill-conditioned P, using the Joseph form of the covariance update, or forcing P to be symmetric by setting P = 0.5 * ( P + P' ).

More complex options include switching to a square root form (e.g. UDU'), or using a floating point representation with more precision (e.g. double instead of float, which is mainly hard because you're probably already at double).

Matlab: Kalman Filter -- How to mitigate the Warning: Matrix is singular or badly scaled

1 Answers