Why does covariance P matrix become non positive definite in Unscented Kalman Filter of MATLAB?

I'm doing Unscented Kalman Filter in MATLAB code and I have followed this tutorial how to create a UKF filter.

First I initilize the vector and covariance P matrix first. In MATLAB code, I just set them into random numbers.

Then I compute the sigma points

MATLAB code:

[xhati] = ukf_compute_sigma_points(xhat, P, a, k, row);

Then I'm using the observation function with the current sigma point . In this case, I say that

MATLAB code:

yhati = xhati;

Then I compute the estimated vector.

MATLAB code:

yhat = ukf_multiply_weights(yhati, WM, M);

I compute the covariance matrix .

MATLAB code:

Py = ukf_estimate_covariance(yhati, yhat, Wc, R, row);

Cross covariance matrix .

MATLAB code:

Pxy = ukf_estimate_cross_covariance(xhati, xhat, yhati, yhat, a, k, M);

I compute kalman gain matrix K by using Cholesky decomposition for every column of .

MATLAB code:

K = ukf_create_kalman_K(Py, Pxy, M);

I do state update and covaraince P update as well.

MATLAB code:

[xhat, P] = ukf_state_update(K, Py, P, xhat, y, yhat, M);

Now I'm computing the sigma points again because now I'm going to predict the future states. Notice that I DON'T take . Instead I assume that L from is the square root of where c is a scalar. The authors of Unscented Kalman Filter did that assumption.

The sigma points capture the same mean and covariance irrespective of the choice of matrix square root which is used. 
Numerically efficient and stable methods such as the Cholesky decomposition[18] can be used.

MATLAB code:

xhati = ukf_compute_sigma_points(xhat, P, a, k, M);

Mow I'm suing the transition function with the new sigma points to get the future .

In this case the transition function is

MATLAB code:

xhati = ukf_transition(xhati, u, M);

And I compute the estimated states.

MATLAB code:

xhat = ukf_multiply_weights(xhati, WM, M);

And last I update the covariance matrix P, which will NOT become positive definite. That's the issue with my code and I don't know why.

**Question:**

When running this MATLAB code. why does covariance P get this covariance matrix.

P = [-0.028857 -0.583326
     -0.583326  2.546039]

At the function

[xi] = ukf_compute_sigma_points(x, P, a, k, M)

We can clearly see that P is not positive definite. Why?

My theory.

If you look at the code

[xhat, P] = ukf_state_update(K, Py, P, xhat, y, yhat, M)

You can clearly see this statement

P = P - K*Py*K';

I did try to change the transpose to

P = P - K'*Py*K;

and now the code is working. But is this correct way? What do you think?

I have also a textbook about UFK where this line is used with P instead of

P = P - K*P*K';

I also made a C-code version here.

Here is my complete MATLAB code:

function ukf_test()
      % Initial states
      y = [0; 0];
    	u = [1; 2];
    	xhat = [0; 0];
    	P = [5  0; 0 2];
    	Q = [1, 0; 0, 2];
    	R = [1.5, 0; 0, 2];
    	a = 1;
    	k = 2;
    	b = 3;
      M = 2;
      for i = 1:2
        [xhat, y, P] = ukf(xhat, y, u, P, Q, R, a, k, b, M);
      end
    end
    
    function [xhat, y, P] = ukf(xhat, y, u, P, Q, R, a, k, b, M)
      column = 2 * M + 1;
      row = M;
      
      % Step 0 - Create the weights
      [WM, Wc] = ukf_create_weights(a, b, k, row);
      
      % UPDATE: Step 1 - Compute sigma points
      [xhati] = ukf_compute_sigma_points(xhat, P, a, k, row);
      
      % UPDATE: Step 2 - Use the nonlinear measurement function to compute the predicted measurements for each of the sigma points.
      yhati = xhati; % Here we assume that the observation function y = h(x, u) = x
      
      % UPDATE: Step 3 - Combine the predicted measurements to obtain the predicted measurement
      yhat = ukf_multiply_weights(yhati, WM, M);
      
      % UPDATE: Step 4 - Estimate the covariance of the predicted measurement
      Py = ukf_estimate_covariance(yhati, yhat, Wc, R, row);
      
      % UPDATE: Step 5 - Estimate the cross-covariance between xhat and yhat. Here i begins at 1 because xhati(0) - xhat(0) = 0
      Pxy = ukf_estimate_cross_covariance(xhati, xhat, yhati, yhat, a, k, M);
      
      % UPDATE: Step 6 - Find kalman K matrix
    	K = ukf_create_kalman_K(Py, Pxy, M);
      
      % UPDATE: Step 7 - Obtain the estimated state and state estimation error covariance at time step
      [xhat, P] = ukf_state_update(K, Py, P, xhat, y, yhat, M);
      
      % PREDICT: Step 0 - Predict the state and state estimation error covariance at the next time step
    	xhati = ukf_compute_sigma_points(xhat, P, a, k, M);
    
    	% PREDICT: Step 1 - Use the nonlinear state transition function to compute the predicted states for each of the sigma points.
    	xhati = ukf_transition(xhati, u, M);
    
    	% PREDICT: Step 2 - Combine the predicted states to obtain the predicted states at time
    	xhat = ukf_multiply_weights(xhati, WM, M);
    
    	% PREDICT: Step 3 - Compute the covariance of the predicted state
    	P = ukf_estimate_covariance(xhati, xhat, Wc, Q, M);
      
    end
    
    function [WM, Wc] = ukf_create_weights(a, b, k, M)
      column = 2 * M + 1;
      WM = zeros(1, column);
      Wc = zeros(1, column);
      for i = 1:column
        if(i == 1)
          Wc(i) = (2 - a * a + b) - M / (a * a * (M + k));
          WM(i) = 1 - M / (a * a * (M + k));
        else
          Wc(i) = 1 / (2 * a * a * (M + k));
          WM(i) = 1 / (2 * a * a * (M + k));
        end
      end
    end
    
    function [xi] = ukf_compute_sigma_points(x, P, a, k, M)
      column = 2 * M + 1;
    	compensate_column = 2 * M - 1;
    	row = M;
    	c = a * a * (M + k);
      xi = zeros(row, column);
      
    	% According to the paper "A New Extension of the Kalman Filter to Nonlinear Systems"
    	% by Simon J. Julier and Jeffrey K. Uhlmann, they used L = chol(c*P) as "square root",
    	% instead of computing the square root of c*P. According to them, cholesky decomposition
    	% was a numerically efficient and a stable method.
      L = chol(c*P, 'lower');
    
      for j = 1:column
        if(j == 1)
          xi(:, j) = x;
        elseif(and(j >= 2, j <= row + 1))
          xi(:, j) = x + L(:, j - 1);
        else 
          xi(:, j) = x - L(:, j - compensate_column);
        end
      end
      
    end
    
    function x = ukf_multiply_weights(xi, W, M)
      column = 2 * M + 1;
    	row = M;
      x = zeros(row, 1);
      for i = 1:column
        x = x + W(i)*xi(:, i);
      end
    end
    
    function P = ukf_estimate_covariance(xi, x, W, O, M)
      column = 2 * M + 1;
    	row = M;
      P = zeros(row, row);
      for i = 1:column
        P = P + W(i)*(xi(:, i) - x)*(xi(:, i) - x)';
      end
      P = P + O;
    end
    
    function P = ukf_estimate_cross_covariance(xi, x, yi, y, a, k, M)
    	column = 2 * M + 1;
    	row = M;
      c = 1 / (2 * a * a * (M + k));
      P = zeros(row, row);
      for i = 2:column % Begins at 2 because xi(:, 1) - x = 0
        P = P + (xi(:, i) - x)*(yi(:, i) - y)';
      end
      P = c*P;
    end
    
    function K = ukf_create_kalman_K(Py, Pxy, M)
      row = M;
      K = zeros(row, row);
      for i = 1:row
        % Solve Ax = b with Cholesky
        L = chol(Py, 'lower');
        y = linsolve(L, Pxy(:, i));
        K(:, i) = linsolve(L', y);
      end
      % This will work to K = linsolve(Py, Pxy);
    end
    
    function [xhat, P] = ukf_state_update(K, Py, P, xhat, y, yhat, M)
    	row = M;
      xhat = xhat + K*(y - yhat);
      P = P - K*Py*K';
    end
    
    function xhati = ukf_transition(x, u, M)
      column = 2 * M + 1;
      row = M;
      xhati = zeros(row, column);
      for i = 1:column
        xhati(:, i) = transistion_function(x(:, i), u);
      end
    end
    
    function dx = transistion_function(x, u)
      dx = zeros(2, 1);
      dx(1) = -2*x(1)*x(2) + 4*x(2) + 4*u(1);
      dx(2) = -x(1) - 3*x(2) + 7*u(2);
    end

Answer 1

[Harun Topbas]

Hi Daniel,

I encountered this problem too, i can advice you to use square root unscented kalman algorithm, here is the ref;

Van der Merwe, Rudolph, and Eric 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), 6:3461–64. Salt Lake City, UT, USA: IEEE, 2001. https://doi.org/10.1109/ICASSP.2001.940586.

Also matlab ukf software uses this technique as you can see link. hope it will help.

[Harun Topbas]

https://github.com/JJHu1993/sr-ukf

[Daniel]

Thank you! But how can I do square root for matricies in C? What algorithm should I use there?

Isn't cholesky good enought?

[Harun Topbas]

you need to use cholesky rank 1 update or downdate. begining at the filter, you should expect square root of error covarince matrix as an input from user and dont need to use sqrt() or chol() anymore. in paper i share in previous comment Algorithm 3.1 i guess will right method to ukf implementation as matlab does. I saw your C codes in github. QR factorization and cholesky is very good. You can use them. Remember cholupdate(A,x,+-) ~= chol(A'*A +- x*x')

[Harun Topbas]

By the way, cholesky factorization is quiet efficient method and requires less compution cost than diagonalization methods. If you are asking just getting square root of a matrix

[Daniel]

Hi again.

Can you try to run this MATLAB code. It's very basic MATLAB code and it's follow the same UKF filter from Wikipedia, not MATLAB Central. I find that MATLAB's own UKF filter have issues, because the P covariance matrix is NOT positive definitve and symmetrical. But in this code, P becomes positive and symmetrical.

https://github.com/DanielMartensson/STM32-Libraries/blob/master/VL6180X/Distribution/Unscented_Kalman_Filter.m

Then at the transition function, you include your state space model. In this case, it's just a random variable as a transition function.

function x = ukf_transition(s, L)
  N = 2 * L + 1;
  x = zeros(L, 1);
  for i = 1:N
    x(i) = std(s(:, i))*randn + mean(s(:, i)); % std*random_variable + average = Gaussian distribution
  end
end

[Daniel]

I don't think Cholesky can be used to find square root of a matrix.

See here:

䤕>> A
A =

   9.424055  -0.320562   4.367752  -0.922137   3.482455
  -0.320562   3.351337  -1.878821  -1.474543  -1.805529
   4.367752  -1.878821   4.217955  -0.655662   2.148280
  -0.922137  -1.474543  -0.655662   2.960406  -0.080390
   3.482455  -1.805529   2.148280  -0.080390   4.983472

>> sqrtm(A)
ans =

   2.849004   0.104019   0.920514  -0.137971   0.655768
   0.104019   1.585628  -0.593944  -0.507058  -0.465215
   0.920514  -0.593944   1.686292  -0.247093   0.336458
  -0.137971  -0.507058  -0.247093   1.619166  -0.038832
   0.655768  -0.465215   0.336458  -0.038832   2.054824

>> L = chol(A, 'lower')
L =

   3.0699        0        0        0        0
  -0.1044   1.8277        0        0        0
   1.4228  -0.9467   1.1390        0        0
  -0.3004  -0.8239  -0.8852   1.1865        0
   1.1344  -0.9231  -0.2981  -0.6440   1.5300

>> L*L
ans =

   9.4241        0        0        0        0
  -0.5114   3.3404        0        0        0
   6.0872  -2.8086   1.2974        0        0
  -2.4520  -1.6455  -2.0586   1.4077        0
   5.0838  -2.2865  -0.2256  -1.7494   2.3409

>>

[Harun Topbas]

Hi again, firstly, cholesky decomposition -> A = L'*L -> L = chol(A) , so if you want to get original A matrix you need to multiply like this if you use chol(A,'lower'), A = L*L'

[Harun Topbas]

in square root ukf you dont need to directly use chol(matrix) or square root a matrix, roughly saying, in SR ukf, you are already updating square root of input covariance matrix if you use algoritm 3.1 in the paper:

Van der Merwe, Rudolph, and Eric 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), 6:3461–64. Salt Lake City, UT, USA: IEEE, 2001. https://doi.org/10.1109/ICASSP.2001.940586.

Actually, your negative definite covarince matrix problem does not arise from square root tecnique, it is standart ukf methodology problem. No guarrantee that updated covariance matrix is always symmetric or positive, so i strongly suggest to check over the paper. Also simply you can use standart ukf using a tecnique for finding nearest positive definite matrix of covariance matrix end of the each update step

[Harun Topbas]

https://github.com/DanielMartensson/STM32-Libraries/blob/master/VL6180X/Distribution/Unscented_Kalman_Filter.m

no guarantee that in this algorithm P is always symmetric and positive. Different cases can cause negative P matrices.

[Harun Topbas]

try this algorithm ?

https://github.com/JJHu1993/sr-ukf

[Daniel]

Ok. I understand.

The algorithm from the paper is much more better(safer) than the algorithm from Wikipedia and Matlab Central?

So I think I will use that algorithm with Octave code and also rewrite it to C code as well.

[Harun Topbas]

That is right. Matlab ukf software uses this algorithm (SR ukf in the paper) . You can see the reference in matlab ukf algrithm page. States that "For better numerical performance, the software uses the square-root factorization of the covariance matrices. For more information on this factorization, see [2] ." and [2] is this paper. Good luck.

[Daniel]

Thank you.

I did not know that SR-UKF can be used for traning a neural network. Very usefull!

[Daniel]

By the way.

If you have more good papers that are easy to read, please, tell me.

I'm looking for a good paper about particle filter, but I haven't found any.

That SR-UKF paper you gave me was excellent!

[Harun Topbas]

I can suggest Cyrill Stachniss ' s MSR and SLAM courses in youtube video serises. You can find particle filter algorithm topics. I will study particle filter soon. If i find a good paper, i will share with you. Could you share with me if you find a papar ? i would appriciate.

[e-mail]

Thanks for your attention. good luck.

[Daniel]

I have seen his video about particle filter. MSR Course - 07 Particle Filter (Stachniss) - YouTube

But I don't understand how he find the target distribution model.

It feels like he begins with a regular gaussian distribution model, then he take for example 10000 measurements and shaping the initial gaussian model to the target distribution model. But that takes 10000 samples.

Isn't that a little to much for finding the most frequently used measurement in a nonlinear distribution?

Yes!

I can recommend a good paper - SINDy. This algorithm can identify a nonlinear model without knowing anything about the nonlinear plant system. If you like system identification

Sparse Identification of Nonlinear Dynamics (SINDy): Sparse Machine Learning Models 5 Years Later! - YouTube

See my example. I have tried LOTS of algorithms. SINDy algorithm is better than all other algorithms in practice.

DanielMartensson/Mataveid: System identification toolbox for GNU Octave and MATLAB (github.com)

[Harun Topbas]

Thank you very much Daniel.

Works in your github directories are amazing.

[Daniel]

Thank you!

I have some questions. I'm planning to write the SR-UKF filter and estimation algorithm to MATLAB code and C code, but I need some answers first. It's quite basic.

1. Does the estimation algorihtm work for multivariable system of equations?
2. Can I use the estimation algorithm to train a deep neural network, and not only a neural network as in the paper?
3. Have you tried the algorithms? What's your experience with them? Did they work well? What did not work for you?

[Harun Topbas]

Hi Daniel,

Sorry i am late, i have no idea this algoritm is useful for multivariable systems or traininng deep neural network. I used for radar measurement which is tracking a target and measurement model is highly nonlinear.

This algorithm did great job for my problem. I coded the algorithm in C. I revised a little the m file in the link above;

1. Instead of cholupdate(A,x,'+-') -> chol(A'*A '+-' x*x'), I used your chol() and qr() function in github :).
2. In calculation kalman gain step, (line 69 in ukf.m) K = P12*inv(S2'*S2). But instead of inverse operation, you better use linear solver for computation efficiency.

then it worked for me. I can share my code with you if you want.

Good luck

[Daniel]

Hello.

I understand how it works now. The equation D_k_k|-1 = G(x_k, W_k_k-1) (35 - Algorithm 3.2) is called multiple times. Same as algorithm y_k_k|-1 = H[X_k_k|-1] (23 - Algorithm 3.1).

So that means they have been using 1 layer neural networks and not multiple neural networks. But perhaps it's possible to use multi palyer neural networks.

1. I have recently wrote cholupdate in C code. It must be a reason to use cholupdate(A, x) instead of chol(A'*A + x*x') ?https://github.com/DanielMartensson/CControl/blob/master/src/CControl/Sources/LinearAlgebra/cholupdate.c
2. Yes. I can use Cholesky, LUP-decomposition, SVD or QR-factorization for solving linear systems. But I think I will use Sequential Threshold Least Square because it solves with better than L1 norm. It results very sparse solutions. Very good.

Yes.

You can send one to me. I would love to implement SR-UKF in pure C-code.

[e-mail]