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

 Accepted Answer

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.

20 Comments

Thank you! But how can I do square root for matricies in C? What algorithm should I use there?
Isn't cholesky good enought?
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')
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
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.
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
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
>>
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'
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
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.
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.
Thank you.
I did not know that SR-UKF can be used for traning a neural network. Very usefull!
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!
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.
topbash@itu.edu.tr
Thanks for your attention. good luck.
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
See my example. I have tried LOTS of algorithms. SINDy algorithm is better than all other algorithms in practice.
Thank you very much Daniel.
Works in your github directories are amazing.
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?
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
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') ?
  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.

Sign in to comment.

More Answers (0)

Products

Release

R2021a

Asked:

on 13 Jun 2021

Edited:

on 5 Oct 2026

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!