Error in array dimensions!
显示 更早的评论
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;
回答（1 个）
类别
在 帮助中心 和 File Exchange 中查找有关 Control System Toolbox 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!
Translated by 
