Why does P covariance become non definite when doing Cholesky decomposition for unscented Kalman filters?
15 vues (au cours des 30 derniers jours)
Afficher commentaires plus anciens
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');
0 commentaires
Réponses (1)
Garmit Pant
le 30 Août 2023
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:
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/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!
0 commentaires
Voir également
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!