function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin)% Kalman filter.% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...)%% INPUTS:% y(:,t)   - the observation at time t% A - the system matrix% C - the observation matrix % Q - the system covariance % R - the observation covariance% init_x - the initial state (column) vector % init_V - the initial state covariance %% OPTIONAL INPUTS (string/value pairs [default in brackets])% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ]%     In this case, all the above matrices take an additional final dimension,%     i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m).%     However, init_x and init_V are independent of model(1).% 'u'     - u(:,t) the control signal at time t [ [] ]% 'B'     - B(:,:,m) the input regression matrix for model m%% OUTPUTS (where X is the hidden state being estimated)% x(:,t) = E[X(:,t) | y(:,1:t)]% V(:,:,t) = Cov[X(:,t) | y(:,1:t)]% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2% loglik = sum{t=1}^T log P(y(:,t))%% If an input signal is specified, we also condition on it:% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]% If a model sequence is specified, we also condition on it:% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]clearAk=exp(-0.02);                          %各系數由前面確定；Ck=1;                                 Qk=1-exp(-0.04);                      Rk=1;                                 p(1)=1;                                  %各初值；p1(1)=Ak*p(1)*Ak'+Qk;                    %由p1代表p'；x(1)=0;                                  %設信號初值為0；H(1)=p1(1)*Ck'*inv(Ck*p1(1)*Ck'+Rk);[os T] = size(y);ss = size(A,1); % size of state space% set default paramsmodel = ones(1,T);u = [];B = [];ndx = [];args = varargin;nargs = length(args);for i=1:2:nargs  switch args{i}   case 'model', model = args{i+1};   case 'u', u = args{i+1};   case 'B', B = args{i+1};   case 'ndx', ndx = args{i+1};   otherwise, error(['unrecognized argument ' args{i}])  endendx = zeros(ss, T);V = zeros(ss, ss, T);VV = zeros(ss, ss, T);loglik = 0;for t=1:T  m = model(t);  if t==1    %prevx = init_x(:,m);    %prevV = init_V(:,:,m);    prevx = init_x;    prevV = init_V;    initial = 1;  else    prevx = x(:,t-1);    prevV = V(:,:,t-1);    initial = 0;  end  if isempty(u)    [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...	kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial);  else    if isempty(ndx)      [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...	  kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ... 			'initial', initial, 'u', u(:,t), 'B', B(:,:,m));    else      i = ndx{t};      % copy over all elements; only some will get updated      x(:,t) = prevx;      prevP = inv(prevV);      prevPsmall = prevP(i,i);      prevVsmall = inv(prevPsmall);      [x(i,t), smallV, LL, VV(i,i,t)] = ...	  kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ...			'initial', initial, 'u', u(:,t), 'B', B(i,:,m));      smallP = inv(smallV);      prevP(i,i) = smallP;      V(:,:,t) = inv(prevP);    end      end  loglik = loglik + LL;end