Clear Filters
Clear Filters

How to track 3 parameters with this code

1 view (last 30 days)
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

Accepted Answer

Torsten
Torsten on 12 Mar 2022
Edited: Torsten on 12 Mar 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 Comment
Vezzaz
Vezzaz on 12 Mar 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.

Sign in to comment.

More Answers (0)

Categories

Find more on Conditional Mean Models in Help Center and File Exchange

Tags

Community Treasure Hunt

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

Start Hunting!