数值算法/人工智能

开发平台：
Visual C++

kalman_filter.m：源码内容
							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    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         初始状态协方差
　　%varargin                                       模式
　　% 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:
　　% .g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]
[os T] = size(y);
ss = size(A,1); % size of state space
　　% set default params
model = ones(1,T);
u = [];
B = [];
ndx = [];
args = varargin;
nargs = length(args);
for i=1:2:nargs
switch args
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])
end
end
x = 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;
　　% 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

						