Problems of error covariance matrix positiveness of UKF

Hello,

I have encountered a problem when simulating the UKF, I reviewed the algorithm multiple times, tuned the covariance matrices but can't figure out what prevents it from working. you find below the system's equations. The issue is that the error covariance matrix tends to be non positive definite.

I will attach a link to the matlab code of the UKF.

clear all
close all
clc

%% System
tk = 1/1000;
g = 9.8;
m = 1;
t = 0:tk:1;
N = length(t);
x0 = [0.5;0.3;0.1;0.7];
n = length(x0);
sig_r = 1e-3;
sig_r_dot = 2e-2;
sig_tht = 1.5e-1;
sig_tht_dot = 1e-1;
sig_x = 2*[sig_r;sig_r_dot;sig_tht/3;sig_tht_dot];
Q = diag(sig_x);
sig_r_y = 1;
sig_tht_y = 1e-1;
sig_y = [sig_r_y;sig_tht_y];
R = 1*diag(sig_y);
u1 = sin(t);
u2(t<1.5) = 1;
u2(t>=1.5) = 0;
muq = zeros(4,1);
mur = zeros(2,1);

for hh=1:4
w(:,hh) = normrnd(muq(hh,:),Q(hh,hh),N,1);
end


for nn=1:2
v(:,nn) = normrnd(mur(nn,:),R(nn,nn),N,1);
end


xh0 = [0.5;0.5;0.1;0.7];
P0 = diag([100,0.01,100,0.01]);
gamma = sqrt(n);


% Initialize Cells
xh = cell(1,N);
P = cell(1,N);
Pm = cell(1,N);
Pchol = cell(1,N);
Pcholm = cell(1,N);
xhm = cell(1,N);
xhi = cell(1,N);
yhi = cell(1,N);
yh = cell(1,N);
Py = cell(1,N);
Pxy = cell(1,N);
K = cell(1,N);
E = eye(4);
x(1,:) = [0.5 0.3 0.1 0.7];
y(:,1) = [x(1,1);x(1,3)];
Wn = 1/(2*n);
for z=2:N
    x(z,1) = x(z-1,1)+(x(z-1,2)+E(1,:)*w(z-1,:)')*tk;
    x(z,2) = x(z-1,2)+tk*(x(z-1,1)*(x(z-1,4))^2-g*sin(x(z-1,3))+u1(z-1)/m+E(2,:)*w(z-1,:)');
    x(z,3) = x(z-1,3)+tk*(x(z-1,4)+E(3,:)*w(z-1,:)');
    x(z,4) = x(z-1,4)+tk*(-2*x(z-1,1)*x(z-1,2)*x(z-1,4)-g*x(z-1,1)*cos(x(z-1,3))+u2(z-1)/m+E(4,:)*w(z-1,:)')/(x(z-1,1)^2);
    y(:,z) = [x(z,1)+v(z,1);x(z,3)+v(z,2)];
end
%% Recursive UKF
% Initialization
P{1} = P0;
xh{1} = xh0;
for k=2:N
Pm{k} = 0;
xhm{k} = 0;
    % Choose sigma points:
    Pchol{k-1} = chol(P{k-1},'lower');
    for i =1:2*n
        if i<=n
            xt = (gamma*Pchol{k-1}(i,:))';
        else
            xt = (-gamma*Pchol{k-1}(i-n,:))';
        end
        xhi{k-1}(:,i) = xh{k-1}+xt;
        
    % Propagation through the nonlinear model
        xhi{k}(:,i) = [xhi{k-1}(1,i) + tk*xhi{k-1}(2,i);...
                       xhi{k-1}(2,i) + tk*(xhi{k-1}(1,i)*xhi{k-1}(4,i)^2-g*sin(xhi{k-1}(3,i))+u1(k)/m);...
                       xhi{k-1}(3,i) + tk*xhi{k-1}(4,i);...
                       xhi{k-1}(4,i) + tk*(-2*xhi{k-1}(1,i)*xhi{k-1}(2,i)*xhi{k-1}(4,i)-g*xhi{k-1}(1,i)*cos(xhi{k-1}(3,i))+u2(k)/m)/(xhi{k-1}(1,i)^2)];
    
    % Combination of the state vectors from sigma points to obtain the a-priori estimate of the state estimate at time k

        xhm{k} = xhm{k}+Wn*xhi{k}(:,i);
        Pm{k} = Pm{k}+Wn*(xhi{k}(:,i)-xhm{k})*(xhi{k}(:,i)-xhm{k})';
    end
     Pm{k} = Pm{k}+Q;
    % a-prior error covariance estimation
   
    yh{k} = 0;
    % Accuracy improvement (Optional)
    Pcholm{k} = chol(Pm{k},'lower');
    for i=1:2*n
        if i<=n
            xtm = (gamma*Pcholm{k}(i,:))';
        else
            xtm = (-gamma*Pcholm{k}(i-n,:))';
        end
        
        xhi{k}(:,i) = xhm{k}+xtm;
        
    % Propagation through the nonlinear measurement
        yhi{k}(:,i) = [xhi{k}(1,i)^2;xhi{k}(3,i)^2];
        
        yh{k} = yh{k}+Wn*yhi{k}(:,i);
    end
    
    % Estimation of the covariance of the predicted measurement and Estimate the cross covariance between xhm and yh at time k at time k
    Py{k} = 0;
    Pxy{k} = 0;
    for i=1:2*n
        Py{k} = Py{k}+Wn*(yhi{k}(:,i)-yh{k})*(yhi{k}(:,i)-yh{k})';
        Pxy{k} = Wn*(xhi{k}(:,i)-xhm{k})*(yhi{k}(:,i)-yh{k})';
    end
    Py{k} = Py{k}+R;
    % Kalman equations
    K{k} = Pxy{k}/(Py{k});
    xh{k} = xhm{k}+K{k}*(y(:,k)-yh{k});
    P{k} = Pm{k}-K{k}*Py{k}*K{k}';
end

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Error using chol
Matrix must be positive definite.

for i=1:N

% UKF Estimates
xh1(i) = xh{i}(1,1);
xh2(i) = xh{i}(2,1);
xh3(i) = xh{i}(3,1);
xh4(i) = xh{i}(4,1);

end

subplot(4,1,1)
plot(t,x(:,1),t,xh1);
subplot(4,1,2)
plot(t,x(:,2),t,xh2);
subplot(4,1,3)
plot(t,x(:,3),t,xh3);
subplot(4,1,4)
plot(t,x(:,4),t,xh4);

Answer 1

[Sabin]

The main issue with UKF is the matrix square root and can run into singulatrity issues. I suggest to check the folowing article:

Van der Merwe, R., and E. A. Wan, The Square-Root Unscented Kalman Filter for State and Parameter-Estimation. 2001 IEEE International Conference on Acoustics, Speech, and Signal Processing. Proceedings (Cat. No.01CH37221), vol. 6, IEEE, 2001, pp. 3461–64.