Error in array dimensions!
7 views (last 30 days)
Show older comments
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 Comments
See Also
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!