卡尔曼算法的matlab程序

工科男

求卡尔曼算法的matlab预测程序

yulun9988

yulun9988

谢谢。。。。。。。。。。。。。。。。。。。。。。。。。。

244190977

太令人。。。。

工科男

太强大了

剑游九天

厚积薄发 发表于 2011-11-28 10:48
3 G% [6 J% f$ z5 C$ Bmatlab下面的kalman滤波程序
clear  N=200; w(1)=0; 　　
w=randn(1,N)

6 ^: s% k" W, [+ w; o/ k强大呀，下了

厚积薄发

matlab下面的kalman滤波程序
clear  N=200; w(1)=0; 　　
w=randn(1,N)
3 E2 f2 M% I( Y: hx(1)=0; 　　
4 V. H; r5 C7 h4 r' G3 m2 Pa=1; 　　
for k=2:N; 　　
+ U- y" I2 ~. K1 V7 cx(k)=a*x(k-1)+w(k-1); 　　
end 　　
V=randn(1,N); 　　
q1=std(V); 　　
2 E4 ~' Y( O/ \6 `# d( B/ ]Rvv=q1.^2; 　　
9 a% z; \  G' c0 O3 o# mq2=std(x); 　　
, J, {3 _! z" b, A+ R* G. V3 RRxx=q2.^2; 　　
q3=std(w); 　　
3 S5 u( Q: b% a9 H( E! h0 sRww=q3.^2; 　　
c=0.2; 　　
Y=c*x+V; 　　
p(1)=0; 　　
+ ?# D9 A) h2 ~( M) F( Zs(1)=0;
for t=2:N; 　　
) |  u: r7 a. n; u' Z* |1 B2 `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)); 　　
6 P9 k8 G$ ?9 U. Q; D" u" _  F7 Bp(t)=p1(t)-c*b(t)*p1(t); 　　
4 n: A' G( P4 j+ u4 u6 b: Uend 　　
t=1:N; 　　
9 }0 D' [9 i% v0 {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) 　　
# m9 i  L" W& n  K1 N2 O3 F% Kalman filter. 　　
3 b. `. z& I9 r3 v/ P# u2 g% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...) 　　
# B" {' A* }+ ~5 x/ q: L% 　　
% INPUTS: 　　
; A$ }" h4 K- L9 M$ H( q: ?% y(:,t) - the observation at time t 　　
# S9 f  S' Z) u( {* m# x7 f) X% A - the system matrix 　　
0 t8 `" H7 v( P% C - the observation matrix 　　
% Q - the system covariance 　　
: ~- p4 }& I3 e& u& _% R - the observation covariance 　　
: \  ?$ d. w- t, n  x4 g% init_x - the initial state (column) vector 　　
% init_V - the initial state covariance 　　
, V( J, i4 |( N7 y- w% F* g( K3 b% 　　
% OPTIONAL INPUTS (string/value pairs [default in brackets]) 　　
" Y' N) ~+ U) }8 P& J% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ] 　　
$ f) r7 z( x' X$ w% In this case, all the above matrices take an additional final dimension, 　　
% i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m). 　　
  ]/ F8 Q( f2 M# J  N9 }% However, init_x and init_V are independent of model(1). 　　
% 'u' - u(:,t) the control signal at time t [ [] ] 　　
4 B2 O" N- q5 K: V8 j% 'B' - B(:,:,m) the input regression matrix for model m 　　
- M. X& w, C) C% 　　
: `. _, ^) Z8 V; ?6 s) P% OUTPUTS (where X is the hidden state being estimated) 　　
1 _0 m9 ]" u6 x+ H7 q; z% x(:,t) = E[X(:,t) | y(:,1:t)] 　　
% V(:,:,t) = Cov[X(:,t) | y(:,1:t)] 　　
, D3 U' U/ G: x. u4 k3 V% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2 　　
( z: V0 w5 `0 B; t* q: l% loglik = sum{t=1}^T log P(y(:,t)) 　　
% 　　
" D1 L  K# z% b4 C( ]) f6 U% If an input signal is specified, we also condition on it: 　　
. M* j6 m* K; B! O' \8 r/ w) g- i; @% 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); 　　
+ e4 y; M/ C3 P- P" y% w3 bss = size(A,1); % size of state space 　　
% set default params 　　
model = ones(1,T); 　　
u = []; 　　
B = []; 　　
' o( t/ J: I" l( Jndx = []; 　　
args = varargin; 　　
nargs = length(args); 　　
) k. e7 w( `* J' B' ?. Ifor i=1:2:nargs 　　
switch args 　　
1 A% f' s  }: X# Qcase 'model', model = args{i+1}; 　　
case 'u', u = args{i+1}; 　　
case 'B', B = args{i+1}; 　　
5 o- }. B. R3 h' g% r9 acase 'ndx', ndx = args{i+1}; 　　
otherwise, error(['unrecognized argument ' args]) 　　
+ l5 H- I4 t$ \, ?$ [( _1 Nend 　　
end 　　
x = zeros(ss, T); 　　
$ s( u! O* x( m+ ]V = zeros(ss, ss, T); 　　
, P/ n8 N/ T( N# P9 Q& K) |. |VV = zeros(ss, ss, T); 　　
loglik = 0; 　　
for t=1:T 　　m = model(t); 　　
if t==1 　　%prevx = init_x(:,m); 　　
; v6 w- d) l: z6 y* S%prevV = init_V(:,:,m); 　　
* x& J  {/ t! c! t* Z3 q7 jprevx = init_x; 　　
( r* R' I( k, HprevV = init_V; 　　
initial = 1; 　　
else 　　prevx = x(:,t-1); 　　
" X, ]- d' k7 z+ T3 Z+ `# T) w+ ]5 FprevV = V(:,:,t-1); 　　
- t; V# e9 E0 T7 Qinitial = 0; 　　
$ C- U9 j9 }( G" E* w' vend 　　
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 　　
' _. E+ v$ O7 V+ G# di = ndx; 　　
% copy over all elements; only some will get updated 　　x(:,t) = prevx; 　　
prevP = inv(prevV); 　　
prevPsmall = prevP(i,i); 　　
prevVsmall = inv(prevPsmall); 　　
, X- R; L" j! `8 L$ D2 j[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 　　
+ X! M2 O/ p; y+ Z; D4 vloglik = loglik + LL; 　　
( w' b( N& ^6 t$ Nend