Problems of error covariance matrix positiveness of UKF

Question

oussama sadki 2023-5-5

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1958639-problems-of-error-covariance-matrix-positiveness-of-ukf

回答： 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);

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Sabin 2023-5-18

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1958639-problems-of-error-covariance-matrix-positiveness-of-ukf#answer_1238779

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.

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

Problems of error covariance matrix positiveness of UKF

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

回答（1 个）

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

另请参阅

类别

标签

Community Treasure Hunt

Problems of error covariance matrix positiveness of UKF

0 个评论 显示 -2更早的评论隐藏 -2更早的评论

回答（1 个）

0 个评论 显示 -2更早的评论隐藏 -2更早的评论

另请参阅

类别

标签

Community Treasure Hunt

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

0 个评论
显示 -2更早的评论隐藏 -2更早的评论