Error in array dimensions!
1 Ansicht (letzte 30 Tage)
Ältere Kommentare anzeigen
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;
0 Kommentare
Siehe auch
Kategorien
Mehr zu Statistics and Machine Learning Toolbox finden Sie in Help Center und File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!