查看: 3873|回复: 0

[问题求助] 卡尔曼算法的matlab程序

[复制链接]

字体大小: 正常放大

厚积薄发

1341 主题	737 听众	2万积分

数学中国总编辑

TA的每日心情

	衰 2016-11-18 10:46

签到天数: 206 天

[LV.7]常住居民III

超级版主

群组: 2011年第一期数学建模

群组: 第一期sas基础实训课堂

群组: 第二届数模基础实训

群组: 2012第二期MCM/ICM优秀

群组: MCM优秀论文解析专题

1^#

发表于 2011-11-28 10:48 |显示全部楼层

|招呼Ta 关注Ta |邮箱已经成功绑定

matlab下面的kalman滤波程序
clear  N=200; w(1)=0; 　　
w=randn(1,N)
x(1)=0; 　　
a=1; 　　
for k=2:N; 　　
x(k)=a*x(k-1)+w(k-1); 　　
end 　　
V=randn(1,N); 　　
q1=std(V); 　　
Rvv=q1.^2; 　　
q2=std(x); 　　
Rxx=q2.^2; 　　
q3=std(w); 　　
Rww=q3.^2; 　　
c=0.2; 　　
Y=c*x+V; 　　
p(1)=0; 　　
s(1)=0;
for t=2:N; 　　
p1(t)=a.^2*p(t-1)+Rww; 　　
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); 　　
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1)); 　　
p(t)=p1(t)-c*b(t)*p1(t); 　　
end 　　
t=1:N; 　　
plot(t,s,'r',t,Y,'g',t,x,'b'); 　　
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 　　
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