Mercurial > hg > camir-aes2014
diff toolboxes/FullBNT-1.0.7/Kalman/kalman_filter.m @ 0:e9a9cd732c1e tip
first hg version after svn
author | wolffd |
---|---|
date | Tue, 10 Feb 2015 15:05:51 +0000 |
parents | |
children |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/toolboxes/FullBNT-1.0.7/Kalman/kalman_filter.m Tue Feb 10 15:05:51 2015 +0000 @@ -0,0 +1,101 @@ +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)] + +[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{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}]) + 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{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 + + + + +