Problems of error covariance matrix positiveness of UKF

조회 수: 17 (최근 30일)
oussama sadki
oussama sadki 2023년 5월 5일
답변: Sabin 2023년 5월 18일
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);

답변 (1개)

Sabin
Sabin 2023년 5월 18일
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.

Community Treasure Hunt

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

Start Hunting!

Translated by