Mercurial > hg > camir-aes2014
comparison 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 |
comparison
equal
deleted
inserted
replaced
-1:000000000000 | 0:e9a9cd732c1e |
---|---|
1 function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin) | |
2 % Kalman filter. | |
3 % [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...) | |
4 % | |
5 % INPUTS: | |
6 % y(:,t) - the observation at time t | |
7 % A - the system matrix | |
8 % C - the observation matrix | |
9 % Q - the system covariance | |
10 % R - the observation covariance | |
11 % init_x - the initial state (column) vector | |
12 % init_V - the initial state covariance | |
13 % | |
14 % OPTIONAL INPUTS (string/value pairs [default in brackets]) | |
15 % 'model' - model(t)=m means use params from model m at time t [ones(1,T) ] | |
16 % In this case, all the above matrices take an additional final dimension, | |
17 % i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m). | |
18 % However, init_x and init_V are independent of model(1). | |
19 % 'u' - u(:,t) the control signal at time t [ [] ] | |
20 % 'B' - B(:,:,m) the input regression matrix for model m | |
21 % | |
22 % OUTPUTS (where X is the hidden state being estimated) | |
23 % x(:,t) = E[X(:,t) | y(:,1:t)] | |
24 % V(:,:,t) = Cov[X(:,t) | y(:,1:t)] | |
25 % VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2 | |
26 % loglik = sum{t=1}^T log P(y(:,t)) | |
27 % | |
28 % If an input signal is specified, we also condition on it: | |
29 % e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)] | |
30 % If a model sequence is specified, we also condition on it: | |
31 % e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)] | |
32 | |
33 [os T] = size(y); | |
34 ss = size(A,1); % size of state space | |
35 | |
36 % set default params | |
37 model = ones(1,T); | |
38 u = []; | |
39 B = []; | |
40 ndx = []; | |
41 | |
42 args = varargin; | |
43 nargs = length(args); | |
44 for i=1:2:nargs | |
45 switch args{i} | |
46 case 'model', model = args{i+1}; | |
47 case 'u', u = args{i+1}; | |
48 case 'B', B = args{i+1}; | |
49 case 'ndx', ndx = args{i+1}; | |
50 otherwise, error(['unrecognized argument ' args{i}]) | |
51 end | |
52 end | |
53 | |
54 x = zeros(ss, T); | |
55 V = zeros(ss, ss, T); | |
56 VV = zeros(ss, ss, T); | |
57 | |
58 loglik = 0; | |
59 for t=1:T | |
60 m = model(t); | |
61 if t==1 | |
62 %prevx = init_x(:,m); | |
63 %prevV = init_V(:,:,m); | |
64 prevx = init_x; | |
65 prevV = init_V; | |
66 initial = 1; | |
67 else | |
68 prevx = x(:,t-1); | |
69 prevV = V(:,:,t-1); | |
70 initial = 0; | |
71 end | |
72 if isempty(u) | |
73 [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ... | |
74 kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial); | |
75 else | |
76 if isempty(ndx) | |
77 [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ... | |
78 kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ... | |
79 'initial', initial, 'u', u(:,t), 'B', B(:,:,m)); | |
80 else | |
81 i = ndx{t}; | |
82 % copy over all elements; only some will get updated | |
83 x(:,t) = prevx; | |
84 prevP = inv(prevV); | |
85 prevPsmall = prevP(i,i); | |
86 prevVsmall = inv(prevPsmall); | |
87 [x(i,t), smallV, LL, VV(i,i,t)] = ... | |
88 kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ... | |
89 'initial', initial, 'u', u(:,t), 'B', B(i,:,m)); | |
90 smallP = inv(smallV); | |
91 prevP(i,i) = smallP; | |
92 V(:,:,t) = inv(prevP); | |
93 end | |
94 end | |
95 loglik = loglik + LL; | |
96 end | |
97 | |
98 | |
99 | |
100 | |
101 |