数学建模社区-数学中国

标题: 卡尔曼算法的matlab程序 [打印本页]

作者: 工科男 时间: 2011-11-26 16:16
标题: 卡尔曼算法的matlab程序
求卡尔曼算法的matlab预测程序

作者: 厚积薄发 时间: 2011-11-28 10:48
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

作者: 剑游九天 时间: 2011-12-19 23:38

厚积薄发发表于 2011-11-28 10:48
: H# W/ J, l' g; z4 z; a$ gmatlab下面的kalman滤波程序" n) @1 X. u, [/ N9 F1 t
clear N=200; w(1)=0; 　　
# J1 V$ x1 a# Xw=randn(1,N)

强大呀，下了

作者: 工科男 时间: 2012-2-5 10:24
太强大了

作者: 244190977 时间: 2013-10-7 19:59
太令人。。。。

作者: yulun9988 时间: 2014-1-13 21:45
谢谢。。。。。。。。。。。。。。。。。。。。。。。。。。

作者: yulun9988 时间: 2014-1-13 21:45

欢迎光临数学建模社区-数学中国 (http://www.madio.net/)