function [X,P,e,u,Xf] = smoothervar(A,B,K,Z,Omg,Sgm,y,e,x0,P0,tol,allobserved)
if strcmpi(tol,'auto')
tol = {};
else
tol = {tol};
end
nx = size(A,1);
[ny,nper] = size(y);
p = size(A,2)/nx;
symm = @(X) (X+X')/2;
T = [A;eye([(p-1)*nx,p*nx])];
X = nan([nx*p,nper]);
Xf = nan([nx*p,nper]);
P = zeros([nx*p,nx*p,nper]);
F = nan([ny,ny,nper]);
Fi = nan([ny,ny,nper]);
pe = nan([ny,nper]);
Fipe = nan([ny,nper]);
L = nan([nx*p,nx*p,nper]);
G = nan([nx*p,ny,nper]);
if isempty(e)
if isempty(B)
ne = ny;
else
ne = size(B,2);
end
e = zeros([ne,nper]);
end
if isempty(B)
Be = e;
BOmg = Omg;
BOmgBt = Omg;
else
Be = B*e;
BOmg = B*Omg;
BOmgBt = BOmg*B';
end
deviation = isempty(K);
j = false([ny,1]);
X(1:nx,1) = A*x0 + Be(:,1);
if ~deviation
X(1:nx,1) = X(1:nx,1) + K;
end
X(nx+1:end,1) = x0(1:end-nx);
P(1:nx,1:nx,1) = symm(A*P0*A' + BOmgBt);
yindex = ~isnan(y);
issgm = any(any(Sgm ~= 0));
PP = BOmgBt;
FF = Z*PP*Z';
if issgm
FF = FF + Sgm;
end
FFi = pinv(FF,tol{:});
FF = symm(FF);
FFi = symm(FFi);
for t = 1 : nper
j0 = j;
j = yindex(:,t);
pe(j,t) = y(j,t) - Z(j,:)*X(1:nx,t);
if allobserved && all(j0) && all(j)
fprintf('%g',t);
F(:,:,t) = FF;
Fi(:,:,t) = FFi;
else
F(j,j,t) = Z(j,:)*P(1:nx,1:nx,t)*Z(j,:)';
if issgm
F(j,j,t) = F(j,j,t) + Sgm(j,j);
end
Fi(j,j,t) = pinv(F(j,j,t),tol{:});
F(j,j,t) = symm(F(j,j,t));
Fi(j,j,t) = symm(Fi(j,j,t));
end
Fipe(j,t) = Fi(j,j,t)*pe(j,t);
G(:,j,t) = T*P(:,1:nx,t)*Z(j,:)'*Fi(j,j,t);
Xf(:,t) = X(:,t) + P(:,1:nx,t)*Z(j,:)'*Fipe(j,t);
L(:,:,t) = T;
L(:,1:nx,t) = L(:,1:nx,t) - G(:,j,t)*Z(j,:);
if t < nper
X(1:nx,t+1) = A*X(:,t) + Be(:,t+1);
if ~deviation
X(1:nx,t+1) = X(1:nx,t+1) + K;
end
X(nx+1:end,t+1) = X(1:end-nx,t);
if any(j)
X(:,t+1) = X(:,t+1) + G(:,j,t)*pe(j,t);
end
P(:,:,t+1) = T*P(:,:,t)*L(:,:,t)';
if allobserved && all(j)
P(1:nx,1:nx,t+1) = PP;
else
P(1:nx,1:nx,t+1) = P(1:nx,1:nx,t+1) + BOmgBt;
end
P(:,:,t+1) = symm(P(:,:,t+1));
end
end
keyboard
lastobs = find(any(yindex,1),1,'last');
r = zeros([p*nx,1]);
N = 0;
isu = issgm && size(Sgm,1) == size(y,1) && nargout > 3;
if isu
u = zeros([size(Sgm,1),nper]);
end
for t = lastobs : -1 : 1
j = ~isnan(y(:,t));
if isu
u(j,t) = Sgm(j,j)'*(Fipe(j,t) - G(:,j,t)'*r);
end
r = L(:,:,t)'*r;
r(1:nx) = r(1:nx) + Z(j,:)'*Fipe(j,t);
X(:,t) = X(:,t) + P(:,:,t)*r;
e(:,t) = e(:,t) + BOmg'*r(1:nx);
N = L(:,:,t)'*N*L(:,:,t);
N(1:nx,1:nx) = N(1:nx,1:nx) + Z(j,:)'*Fi(j,j,t)*Z(j,:);
P(:,:,t) = symm(P(:,:,t) - P(:,:,t)*N*P(:,:,t));
end
X = X(1:nx,:);
P = P(1:nx,1:nx,:);
end
Input argument "tol" is undefined.
Error in ==> smoothervar at 8
if strcmpi(tol,'auto')