matrix elements must be finite
6 ビュー (過去 30 日間)
古いコメントを表示
So my partner and I are working on a code to solve the lorentz equations and track the parameters and we ended up with a matrix elements must be finite error and we cant figure out what needs to be changed since we went over everything several times over. Any help or hints would be greatly appreciated if possible. The error is on line 52
clear all; close all;
global dT dt nn % Sampling time step as global variable
dq = 3; dx = dq + 3; dy = 1;
% Dimensions: (dq for param. vector, dx augmented state, dy observation)
fct = 'vossFNfct'; % this is the model function F(x) used in filtering
obsfct = 'vossFNobsfct'; % this is the observation function G(x)
N = 10000; % number of data samples
dT = 0.001; % sampling time step (global variable)
dt = dT; nn = fix(dT/dt); % the integration time step can be smaller than dT
% Preallocate arrays
x0 = zeros(3,N); % Preallocate x0, the underlying true trajectory
xhat = zeros(dx,N); % Preallocate estimated x
Pxx = zeros(dx,dx,N); % Preallocate Covariance in x
errors = zeros(dx,N); % Preallocate errors
Ks = zeros(dx,dy,N); % Preallocate Kalman gains
% Initial Conditions
x0(:,1) = [1; 1; 1]; % initial value for x0
% External input current, estimated as parameter p later on:
z = (1:N)/250*2*pi; z = -.4-1.01*abs(sin(z/2));
sigma = ones(1,N)*10;
r_sym = ones(1,N)*46;
b = ones(1,N)*(8/3);
% RuKu integrator of 4th order:
for n = 1:N-1
xx = x0(:,n);
for i = 1:nn
k1 = dt*vossLorentzint(xx,sigma(n),r_sym(n),b(n));
k2 = dt*vossLorentzint(xx+k1/2,sigma(n),r_sym(n),b(n));
k3 = dt*vossLorentzint(xx+k2/2,sigma(n),r_sym(n),b(n));
k4 = dt*vossLorentzint(xx+k3,sigma(n),r_sym(n),b(n));
xx = xx+k1/6+k2/3+k3/3+k4/6;
end
x0(:,n+1) = xx;
end
x = [b; sigma; r_sym; x0]; % augmented state vector (notation a bit different to paper)
xhat(:,1) = x(:,1); % first guess of x_1 set to observation
% Covariances
Q = .015; % process noise covariance matrix
R = .2^2 * var(vossFNobsfct(x)) * eye(dy,dy);
% observation noise covariance matrix
randn('state',0);
y = feval(obsfct,x) + sqrtm(R) * randn(dy,N); % noisy data %%%%%ERROR IS HERE
Pxx(:,:,1) = blkdiag(Q,Q,Q,Q,R,R);% Initial Condition for Pxx
% Main loop for recursive estimation
for k = 2:N
[xhat(:,k),Pxx(:,:,k),Ks(:,:,k)] = ...
vossut(xhat(:,k-1),Pxx(:,:,k-1),y(:,k),fct,obsfct,dq,dx,dy,R);
Pxx(1,1,k) = Q;
Pxx(2,2,k) = Q*0.1;
Pxx(3,3,k) = Q*0.1;
Pxx(4,4,k) = Q*0.1;
errors(:,k) = sqrt(diag(Pxx(:,:,k)));
end % k
% Results
chisq=...
mean((x(1,:)-xhat(1,:)).^2+(x(2,:)-xhat(2,:)).^2+(x(3,:)-xhat(3,:)).^2)
est = xhat(1:dq,N)'; % last estimate
error = errors(1:dq,N)'; % last error
meanest = mean(xhat(1:dq,:)');
meanerror = mean(errors(1:dq,:)')
% Plot Results
set(0,'DefaultAxesFontSize',24)
figure(1)
subplot(2,1,1)
plot(y,'bd','MarkerEdgeColor','blue', 'MarkerFaceColor','blue',...
'MarkerSize',3);
hold on;
plot(x(dq+1,:),'k','LineWidth',2);
xlabel('t');
ylabel('x_1, y');
hold off;
axis tight
title('(a)')
subplot(2,1,2)
plot(x(dq+2,:),'k','LineWidth',2);
hold on
plot(xhat(dq+2,:),'r','LineWidth',2);
plot(x(1,:),'k','LineWidth',2);
for i = 1:dq; plot(xhat(i,:),'m','LineWidth',2); end
for i = 1:dq; plot(xhat(i,:)+errors(i,:),'m'); end
for i = 1:dq; plot(xhat(i,:)-errors(i,:),'m'); end
xlabel('t');
ylabel('z, estimated z, x_2, estimated x_2');
hold off
axis tight
title('(b)')
% This is trying to make the plot shown in the textbook
figure(2)
plot(xhat(dq+1,:),y)
function [xhat,Pxx,K] = vossut(xhat,Pxx,y,fct,obsfct,dq,dx,dy,R)
N = 2*dx; %Number of Sigma Points
Pxx = (Pxx + Pxx')/2; %Symmetrize Pxx - good numerical safety
xsigma = chol(dx * Pxx)'; % Cholesky decomposition - note that Pxx=chol'*chol
Xa = xhat * ones(1,N) + [xsigma, -xsigma]; %Generate Sigma Points
X = feval(fct,dq,Xa); %Calculate all of the X's at once
xtilde = sum(X')'/N; %Mean of X's
X1 = X - xtilde * ones(1,size(X,2)); % subtract mean from X columns
Pxx = X1 * X1' / N;
Pxx = (Pxx + Pxx') / 2; %Pxx covariance calculation
Y = feval(obsfct,X);
ytilde = sum(Y')' / N;
Y1 = Y - ytilde * ones(1,size(Y,2)); % subtract mean from Y columns
Pyy = Y1 * Y1' / N + R; %Pyy covariance calculation
Pxy = X1 * Y1' / N; %cross-covariance calculation
K = Pxy * inv(Pyy);
xhat = xtilde + K * (y-ytilde);
Pxx = Pxx - K * Pxy'; Pxx = (Pxx+Pxx') / 2;
end
function r = vossFNobsfct(x)
r = x(4,:);
end
%This function calculates the Lorentz equations
function r = vossLorentzint(x,sigma,r_sym,b)
r = [sigma*(-x(1)+x(2)); x(1)*x(3)+r_sym*x(1); x(1)*x(2)-b*x(3)];
%x = [sigma; r_sym; b; x0];
end
function r = vossFNfct(dq,x)
global dT dt nn
p = x(1:dq,:);
b = p(1,:);
sigma = p(2,:);
r_sym = p(3,:);
xnl = x(dq+1:size(x(:,1)),:);
for n = 1:nn
k1 = dt*fc(xnl,sigma,r_sym,b);
k2 = dt*fc(xnl+k1/2,sigma,r_sym,b);
k3 = dt*fc(xnl+k2/2,sigma,r_sym,b);
k4 = dt*fc(xnl+k3,sigma,r_sym,b);
xnl = xnl+k1/6+k2/3+k3/3+k4/6;
end
r = [x(1:dq,:); xnl];
end
function r = fc(x,sigma,r_sym,b)
r = [sigma.*(-x(1,:)+x(2,:)); x(1,:).*x(3,:)+r_sym.*x(1,:); x(1,:).*x(2,:)-b.*x(3,:)];
end
0 件のコメント
採用された回答
その他の回答 (0 件)
参考
カテゴリ
Help Center および File Exchange で Systems of Nonlinear Equations についてさらに検索
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!