创建博客 登录  
 加关注
   显示下一条  |  关闭
温馨提示!由于新浪微博认证机制调整,您的新浪微博帐号绑定已过期,请重新绑定!立即重新绑定新浪微博》  |  关闭

paul.mcdean的博客

 
 
 

日志

 
 

kalman filter matlab code  

2007-05-07 00:00:55|  分类: 无线通信 |  标签: |举报 |字号 订阅

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

  评论这张
 
阅读(439)| 评论(0)
推荐 转载

历史上的今天

最近读者

热度

评论

<#--最新日志,群博日志--> <#--推荐日志--> <#--引用记录--> <#--博主推荐--> <#--随机阅读--> <#--首页推荐--> <#--历史上的今天--> <#--被推荐日志--> <#--上一篇,下一篇--> <#-- 热度 --> <#-- 网易新闻广告 --> <#--右边模块结构--> <#--评论模块结构--> <#--引用模块结构--> <#--博主发起的投票-->
 
 
 
 
 
 
 
 
 
 
 
 
 
 

页脚

网易公司版权所有 ©1997-2014