regrkalman.m

matlab回归分析工具箱

function [Yhat0,Yhat1,Xhat0,Xhat1] = RegrKalman(U,Y,A,B,C,D,Rxx,Ryy,Rxy,x0)

%   [Yhat0,Yhat1,Xhat0,Xhat1] = RegrKalman(U,Y,A,B,C,D,Rxx,Ryy,Rxy,x0)
%   [Yhat0,Yhat1,Xhat0,Xhat1] = RegrKalman(U,Y,A,B,C,D,Rxx,Ryy,Rxy)
%
% Discrete time Kalman filter.
%
% Input parameters:
%  - U: Input data block (k x n)
%  - Y: Output data block (k x m)
%  - A,B,C,D,Rxx,Ryy,Rxy: System matrices
%  - x0: Initial state (default x0=0)
% Return parameters:
%  - Yhat0: Corresponding zero-lag output estimate (filter)
%  - Yhat1: One-step output estimate (predictor)
%  - Xhat0: State sequence estimate (k|k)
%  - Xhat1: State sequence estimate (k+1|k)
%
% Heikki Hyotyniemi Sep.13, 2000


Yhat0 = NaN; Yhat1 = NaN; Xhat0 = NaN; Xhat1 = NaN;
[ku,n] = size(U);
[ky,m] = size(Y);
[nA1,nA2] = size(A);
[nB1,nB2] = size(B);
[nC1,nC2] = size(C);
[nD1,nD2] = size(D);
[nRxx1,nRxx2] = size(Rxx);
[nRyy1,nRyy2] = size(Ryy);
[nRxy1,nRxy2] = size(Rxy);
if ky~=ku, disp('Input and output blocks incompatible!'); return; 
else k = ky; end
if nA1~=nA2, disp('System matrix A not square!'); return; 
else d = nA1; end
if nB1~=d | nB2~=n, disp('System matrix B incompatible!'); return; end
if nC2~=d | nC1~=m, disp('System matrix C incompatible!'); return; end
if nD1~=m | nD2~=n, disp('System matrix D incompatible!'); return; end
if nRxx2~=nRxx1 | nRxx1~=d, disp('Covariance matrix Rxx incompatible!'); return; end
if nRyy2~=nRyy1 | nRyy1~=m, disp('Covariance matrix Ryy incompatible!'); return; end
if nRxy1~=d | nRxy2~=m, disp('Covariance matrix Rxy incompatible!'); return; end

U = [U;zeros(1,size(U,2))];

barP = zeros(size(A));
barK = zeros(n,m);
for i = 1:1000  % Assume this is enough? Problems if too small
    barK = (A*barP*C'+Rxy)*inv(C*barP*C'+Ryy);
    barP = A*barP*A' + Rxx - barK*(C*barP*C'+Ryy)*barK';
end
barKf = barP*C'*inv(C*barP*C'+Ryy);

Xhat0 = NaN*ones(k+1,d);
Xhat1 = NaN*ones(k+1,d);
Yhat0 = NaN*ones(k+1,m);
Yhat1 = NaN*ones(k+1,m);
if nargin<10
   x0 = zeros(d,1);
end
x1 = x0;
Xhat1(1,:) = x1';

for i = 1:k
    y = Y(i,:)';
    u = U(i,:)';
    x0 = x1 + barKf*(y-C*x1);
    Yhat0(i,:) = (C*x0 + D*u)';
    Xhat0(i,:) = x0';
    x1 = A*x1 + B*u + barK*(y-C*x1-D*u);
    Xhat1(i+1,:) = x1';
    Yhat1(i+1,:) = (C*x1 + U(i+1,:)')';
end