kalman_filter.m

基于kalman滤波器的信息融合 用MATLAB应用举例实现

function [x, V, VV, K, J, An] = kalman_filter(y, A, C, Q, W, S, R, init_x, init_V, varargin)
% Kalman filter.
%  [x, V, VV, K, J, An] = kalman_filter(y, A, C, Q, W, S, R,...
%  
% INPUTS:
% y(:,t)   - the observation at time t
% A - the system matrix
% C - the observation matrix 
% Q - the system covariance
% W - the noise effect matrix
% S - 
% 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)]

L = length(R);
[os T] = size(y(:,:,1));
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{i}
   case 'model', model = args{i+1};
   case 'u', u = args{i+1};
   case 'B', B = args{i+1};
   otherwise, error(['unrecognized argument ' args{i}])
  end
end

for j = 1:L
    x(:,:,j) = zeros(ss, T);
    V(:,:,:,j) = zeros(ss, ss, T);
    VV(:,:,:,j) = zeros(ss, ss, T);
end

loglik = 0;
for t=1:T
    m = model(t);
    if t==1
        for j = 1:L
        prevx(:,:,j) = init_x;
        prevV(:,:,:,j) = init_V;
    end
    initial = 1;
    else
        for j = 1:L
            prevx(:,:,j) = x(:,t-1,j);
            prevV(:,:,:,j) = V(:,:,t-1,j);
        end
        initial = 0;
    end

    for j = 1:L
        [x(:,t,j), V(:,:,t,j), K(:,:,t,j), VV(:,:,t,j), J(:,:,t,j), An(:,:,t,j)] = ...
        kalman_update(A(:,:,m), C(:,:,j,m), Q(:,:,m), W, S(:,:,j,m), R(:,:,j,m), y(:,t,j), prevx(:,:,j), prevV(:,:,:,j), 'initial', initial);
    end
end