Error in array dimensions!

7 views (last 30 days)
GTA
GTA on 30 Oct 2018
Commented: Matt J on 31 Oct 2018
I'm implementing the Unscented Kalman Filter, and I can not figure out how to solve the kalman gain matrix error. Can anyone help?
This is the main code ...
clear
clc
load('dados.mat')
y_total = Dados;
% y(:,1)=10^6*y_total(:,1);
y(:,2)=10^6*y_total(:,2);
y(:,3)=10^6*y_total(:,3);
betah = 0.009;
muh = 3.6530e-005;
alphah = 0.033;
deltah = 0.09;
mui = 0.38;
mun = 0.002;
betai = 0.015;
lambdai = 0.36;
lambdah = 0.0006;
H=0.000000000000000000000000000000000000000000001;
x(:,1) = [208701300 326*10e6 217*10e6 44000000 22000000 lambdah]';
f=@(x)[-betah*H*x(5)*x(1)-muh*H*x(1)+x(6)*H*x(1)+x(1)+x(6)*H*(x(3)+x(2));
betah*H*x(5)*x(1)+x(2)-muh*H*x(2)-alphah*H*x(2)-deltah*H*x(2);
deltah*H*x(2)+x(3)-muh*H*x(3);
x(4)-betai*H*x(2)*x(4)+lambdai*H*x(4)-mui*H*x(4)-mun*H*x(4);
x(5)+betai*H*x(2)*x(4)+lambdai*H*x(5)-mui*H*x(5)-mun*H*x(5);
x(6)];
h=@(x)[-betah*H*x(5)*x(1)-muh*H*x(1)+x(2)+x(3);
(-deltah*H-muh*H-alphah*H)*x(2)+betah*H*x(1)*x(5);
deltah*H*x(2)-muh*H*x(3)];
Q = [10 0 0 0 0 0;
0 10 0 0 0 0;
0 0 10 0 0 0;
0 0 0 15 0 0;
0 0 0 0 33 0;
0 0 0 0 0 18];
R = [.1 0 0;
0 .3 0 ;
0 0 .1];
P(:,:,1) = 1e7*eye(6);
n = 6; %entradas
m = 3; %saídas
steps=38;
fXis = zeros(n, 2*n+1);
hXis = zeros(m, 2*n+1);
x_pred(:,1) = x;
P_pred(:,:,1) = P;
Pxz(:,:,2*n+1) = zeros(n, m);
% K(:,:,2*n+1) = zeros(n, m);
for s = 2:steps
%
[Xis, W] = SigmaPoints(x_pred(:,s-1),P_pred(:,:,s-1),0);
for k = 1:2*n+1
fXis(:, k) = f(Xis(:,k));
end
[xp, Pp] = UT(fXis, W, Q);
for k = 1:2*n+1
hXis(:, k) = h(Xis(:,k));
end
[zp, Pz] = UT(hXis, W, R);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% K(:,:,2*n+1) = zeros(n, m);
for k = 1:2*n+1
Pxz(:,:,k) = Pxz(:,:,k)+ W(k)*((fXis(:,k) - xp)*(hXis(:,k) - zp)');
end
K(:,:,s) = Pxz(:,:,s)/Pz;
x_pred(:,s)= xp + K(:,:,s)*(y_total(s,:).' - zp);
P_pred(:,:,s)= Pp - K(:,:,s)*Pz*K(:,:,s)';
end
The error that appears
Index exceeds matrix dimensions.
Error in UKF_Lambdah_2 (line 81)
K(:,:,s) = Pxz(:,:,s)/Pz;

Answers (1)

Matt J
Matt J on 30 Oct 2018
Edited: Matt J on 30 Oct 2018
Pxz(:,:,s) makes no sense because it has only 13 layers, yet s runs up to 38.
  2 Comments
GTA
GTA on 30 Oct 2018
Because in reality I would like to run for every k = 1: 38, 2n + 1 sigma points. So not to repeat the k, since it was already using within the second for I changed by s. But since I'm not a programmer, I think I did a lot wrong. How could I execute this idea correctly?
Matt J
Matt J on 31 Oct 2018
Only you know what is correct, because only you know what the code is supposed to do.

Sign in to comment.

Categories

Find more on Statistics and Machine Learning Toolbox in Help Center and File Exchange

Community Treasure Hunt

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

Start Hunting!