How to track 3 parameters with this code

Question

Vezzaz 2022-3-12

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1669774-how-to-track-3-parameters-with-this-code

评论： Vezzaz 2022-3-12

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

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Torsten 2022-3-12

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1669774-how-to-track-3-parameters-with-this-code#answer_915774

编辑：Torsten 2022-3-12

在 MATLAB Online 中打开

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 个评论
显示 -1更早的评论隐藏 -1更早的评论

Vezzaz 2022-3-12

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.

请先登录，再进行评论。