Error in array dimensions!
1 vue (au cours des 30 derniers jours)
Afficher commentaires plus anciens
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 commentaires
Voir également
Catégories
En savoir plus sur Statistics and Machine Learning Toolbox dans Help Center et File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!