Borrar filtros
Borrar filtros

How to track 3 parameters with this code

1 visualización (últimos 30 días)
Vezzaz
Vezzaz el 12 de Mzo. de 2022
Comentada: Vezzaz el 12 de Mzo. de 2022
I am trying to modify this code my professor gave but I am getting an input argument error on line 129. Can someone tell me why? I marked it down below to make it easier to find. I needed to modify the code to track the parameters a,b,and c since it already tracked z so stuff might be wrong or out of place since I have been trying to tweak it. But I thought I was doing everything properly
clear all; close all;
global dT dt nn % Sampling time step as global variable
dq = 4; dx = dq + 2; 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 = 800; % number of data samples
dT = 0.2; % sampling time step (global variable)
dt = dT; nn = fix(dT/dt); % the integration time step can be smaller than dT
% Preallocate arrays
x0 = zeros(2,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) = [0; 0]; % 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));
a= ones(1,800)*0.7;
b= ones(1,800)*0.8;
c= ones(1,800)*3;
% RuKu integrator of 4th order:
for n = 1:N-1
xx = x0(:,n);
for i = 1:nn
k1 = dt*vossFNint(xx,z(n),a(n),b(n),c(n));
k2 = dt*vossFNint(xx+k1/2,z(n),a(n),b(n),c(n));
k3 = dt*vossFNint(xx+k2/2,z(n),a(n),b(n),c(n));
k4 = dt*vossFNint(xx+k3,z(n)),a(n),b(n),c(n);
xx = xx+k1/6+k2/3+k3/3+k4/6;
end
x0(:,n+1) = xx;
end
x = [z; x0; a; b; c]; % 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
Pxx(:,:,1) = blkdiag(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;
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)')
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(2,:);
end
function r = vossFNint(x,z,a,b,c)
r = [c*(x(2) +x(1) - x(1)^3/3 + z); -(x(1) - a + b* x(2)) / c]; %%%%line 129 error is here
end
function r = vossFNfct(dq,x)
global dT dt nn
xnl = x(dq+1:size(x(:,1)),:);
for n = 1:nn
k1 = dt*fc(xnl,dq);
k2 = dt*fc(xnl+k1/2,dq);
k3 = dt*fc(xnl+k2/2,dq);
k4 = dt*fc(xnl+k3,dq);
xnl = xnl+k1/6+k2/3+k3/3+k4/6;
end
r = [x(1:dq,:); xnl];
end
function r = fc(x,dq,a,b,c)
r = [c*(x(2,:)+x(1,:)-x(1,:).^3/3+dq); -(x(1,:)-a+b*x(2,:))/c];
end

Respuesta aceptada

Torsten
Torsten el 12 de Mzo. de 2022
Editada: Torsten el 12 de Mzo. de 2022
k4 = dt*vossFNint(xx+k3,z(n),a(n),b(n),c(n);
instead of
k4 = dt*vossFNint(xx+k3,z(n)),a(n),b(n),c(n);
Further I don't understand this loop
for i = 1:nn
k1 = dt*vossFNint(xx,z(n),a(n),b(n),c(n));
k2 = dt*vossFNint(xx+k1/2,z(n),a(n),b(n),c(n));
k3 = dt*vossFNint(xx+k2/2,z(n),a(n),b(n),c(n));
k4 = dt*vossFNint(xx+k3,z(n),a(n),b(n),c(n);
xx = xx+k1/6+k2/3+k3/3+k4/6;
end
since the loop index i is never adressed.
  1 comentario
Vezzaz
Vezzaz el 12 de Mzo. de 2022
Yeah that was part of the code that was given to me and my understanding is not great of it. But thank you for seeing that error that I over looked. I might be able to work through the rest.

Iniciar sesión para comentar.

Más respuestas (0)

Categorías

Más información sobre Conditional Mean Models en Help Center y File Exchange.

Etiquetas

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by