Why does P covariance become non definite when doing Cholesky decomposition for unscented Kalman filters?

I am doing unscented kalman for estimation of position and orientation of vehicles.The cholesky decomposition works fine for 8 iterations but then gives me error that P is not positive definite.

Q = eye(L);

R = eye(M);

P0 =eye(L); % Set initial error covariance

P=P0;for k=2:N

disp(['At step ',num2str(k)]);

sP=chol(P,'lower');

%%Step1: Generation of sigma points

chi_p=[x(:,k-1),x(:,k-1)*ones(1,L)+sqrt(L+lambda)*sP,x(:,k-1)*ones(1,L)-sqrt(L+lambda)*sP];

%%Step2: Prediction transformation

chi_m=zeros(L,2*L+1);

present_data=[imu(:,k);dvl(:,k)];

for i=1:2*L+1

chi_m(:,i) = process_model(chi_p(:,i),F);

end

x_m=chi_m*wm;

P_m=Q;

for i=1:2*L+1

P_m=P_m+wc(i)*(chi_m(:,i)-x_m)*(chi_m(:,i)-x_m)';

end

%%Step3:Observation transformation

psi_m=zeros(M,2*L+1);

for i=1:2*L+1

psi_m(:,i)=measurement_model(chi_m(:,i),present_data);

end

y_m=psi_m*wm;

Pyy=R;

Pxy=zeros(L,M);

for i=1:2*L+1

Pyy=Pyy+wc(i)*(psi_m(:,i) - y_m)*(psi_m(:,i) - y_m)';

Pxy = Pxy + wc(i)*(chi_m(:,i) - x_m)*(psi_m(:,i) - y_m)';

end

%%Step4:Measurment update

K=Pxy/Pyy; %Kalman gain

x(:,k) = x_m + K*(yt(:,k) - y_m); % Update state estimate

P = P_m - K*Pyy*K'; % Update covariance estimate

disp(P);

rotation_matrix=get_rotation_matrix(euler_angles(k,1),euler_angles(k,2),euler_angles(k,3),0);

J = [rotation_matrix, zeros(3); zeros(3), rotation_matrix];

F=J*F;

end

The output I get is as follows,

At step 2

1.0e+05 *

0.3979 0.2134 -0.2347 -0.2480 -0.0195 0.0209

0.2134 1.2765 -0.0005 -0.3554 -0.0420 0.0023

-0.2347 -0.0005 1.4799 -0.0419 0.0075 -0.0359

-0.2480 -0.3554 -0.0419 0.3434 0.0275 -0.0143

-0.0195 -0.0420 0.0075 0.0275 0.0026 -0.0010

0.0209 0.0023 -0.0359 -0.0143 -0.0010 0.0026

At step 3

1.0e+12 *

2.6451 0.5728 -1.5551 -0.0207 -0.0028 0.0062

0.5728 0.4972 0.0441 -0.0607 -0.0035 0.0237

-1.5551 0.0441 3.9825 -0.1677 0.0131 0.0447

-0.0207 -0.0607 -0.1677 0.0544 0.0021 -0.0145

-0.0028 -0.0035 0.0131 0.0021 0.0003 -0.0006

0.0062 0.0237 0.0447 -0.0145 -0.0006 0.0053

At step 4

1.0e+26 *

1.9746 -0.4713 -0.9324 -0.6681 0.0004 0.0105

-0.4713 0.4186 0.3313 0.1953 -0.0012 -0.0011

-0.9324 0.3313 1.0904 0.5177 -0.0034 0.0007

-0.6681 0.1953 0.5177 2.7647 0.0038 -0.0346

0.0004 -0.0012 -0.0034 0.0038 0.0000 -0.0001

0.0105 -0.0011 0.0007 -0.0346 -0.0001 0.0018

At step 5

1.0e+65 *

0.0000 -0.0000 -0.0000 0.0000 0.0000 0.0000

-0.0000 0.0000 -0.0000 0.0000 0.0000 -0.0000

-0.0000 -0.0000 0.0000 -0.0000 0.0000 -0.0000

0.0000 0.0000 -0.0000 2.5450 0.0000 0.0000

0.0000 0.0000 0.0000 0.0000 0.0000 -0.0000

0.0000 -0.0000 -0.0000 0.0000 -0.0000 0.0000

At step 6

1.0e+148 *

0.0000 0.0000 -0.0000 -0.0000 0.0000 -0.0000

0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0000

-0.0000 0.0000 0.0000 0.0000 0.0000 -0.0000

-0.0000 -0.0000 0.0000 3.8122 -0.0000 0.0000

0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0000

-0.0000 -0.0000 -0.0000 0.0000 -0.0000 0.0000

At step 7

1.0e+268 *

0.0000 -0.0000 -0.0000 NaN NaN NaN

-0.0000 0.0638 0.1927 NaN NaN NaN

-0.0000 0.1927 1.8373 NaN NaN NaN

NaN NaN NaN NaN NaN NaN

NaN NaN NaN NaN NaN NaN

NaN NaN NaN NaN NaN NaN

At step 8

Error using chol

Matrix must be positive definite.

Error in final_ukf (line 73)

sP=chol(P,'lower');

Answer 1

[Garmit Pant]

Hello NIshchala

I understand that you are trying to implement the Unscented Kalman Filter algorithm in MATLAB and are facing an error in the implementation. The error is occurring when you are trying to calculate the Cholesky factorization of the matrix ‘P’ for sigma points calculation.

sP=chol(P,'lower'); 

The function ‘chol’ only calculates the Cholesky Factorization when the input matrix is a symmetric positive definite matrix. In the case that the matrix is not symmetric, it considers the diagonal and the upper or lower triangle of the matrix according to what is specified in the function call. In your case, the function will consider the lower triangle of the matrix ‘P’ if the matrix is not symmetric. Even then, the resultant matrix has to be positive definite for the operation to work.

We can observe that in step 7 of the execution of your code, the matrix ‘P’ has some values as ‘NaN’. This results in the matrix not being positive definite and thus the function ‘chol’ throwing an error.

You can check the particular points in your code where the matrix ‘P’ is being updated and the matrices used to update it are being calculated, for eg: check the calculation of matrices like ‘P_m’, ‘K’, ‘Pxy’, ‘Pyy’. Incorrect calculation for these matrices will lead to numerical instability or incorrect estimation.

You can try adding breakpoints to you code to check the values of these matrices as your code runs.

You can find implementations of the Unscented Kalman Filters in MATLAB File Exchange. You can refer to this particular implementation to compare it with your implementation:

https://in.mathworks.com/matlabcentral/fileexchange/18217-learning-the-unscented-kalman-filter

The author of the above implementation has used the following function to generate sigma points:

function X=sigmas(x,P,c) 
%Sigma points around reference point 
%Inputs: 
    %       x: reference point 
    %       P: covariance 
    %       c: coefficient 
    %Output: 
    %       X: Sigma points 
    
    A = c*chol(P)'; 
    Y = x(:,ones(1,numel(x))); 
    X = [x Y+A Y-A];  
end 

Please refer to the entire code to check your implementation.

Here are some additional documentation links to help you out:

https://in.mathworks.com/help/matlab/ref/chol.html

https://in.mathworks.com/help/control/ug/nonlinear-state-estimation-using-unscented-kalman-filter.html - This example shows how to use the unscented Kalman filter and particle filter algorithms for nonlinear state estimation for the van der Pol oscillator.

I hope the explanation and suggested resolutions help you resolve your query!