数学建模社区-数学中国

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

作者: 工科男    时间: 2011-11-26 16:16
标题: 卡尔曼算法的matlab程序
求卡尔曼算法的matlab预测程序
作者: 厚积薄发    时间: 2011-11-28 10:48
matlab下面的kalman滤波程序
2 t" x9 P5 n6 V/ a. I; Cclear  N=200; w(1)=0;   
9 G. }  V  W7 e( |! nw=randn(1,N)
: x4 x% V( \0 ox(1)=0;   
7 {& i2 B# {2 e2 a/ ?a=1;   
7 P# f2 U! R* c! c& sfor k=2:N;   5 e$ W  _4 o6 N# x- e& T- U8 K
x(k)=a*x(k-1)+w(k-1);   ; e; _, U6 v' F9 |  ?
end   
( C( x8 Q! u" U* _V=randn(1,N);   ; X* f, {+ o) @- `  g2 Z" B5 P% g
q1=std(V);   
3 e+ ~7 R3 V; V7 V" r* s, v- `Rvv=q1.^2;   , a7 ]) o3 `$ l& ~# l% w- s- u
q2=std(x);   1 O4 l6 R+ }6 x( \
Rxx=q2.^2;   
. H2 H+ ^+ Q- @  k  X) q# f6 @q3=std(w);   
6 Q( S/ `0 o" x/ L8 J5 CRww=q3.^2;   
; a/ Y: b8 A" R8 Jc=0.2;   
' c8 O- \1 e1 L9 g9 g/ |! VY=c*x+V;   
3 l. o! h# ]0 T+ I: L! ap(1)=0;   
* Z5 |& ]/ B( y% Z. @: Ks(1)=0;
$ i7 t7 I) f6 y- E6 G2 L' ]for t=2:N;   : Z3 Z- H3 y3 _9 S! [3 V
p1(t)=a.^2*p(t-1)+Rww;   
" Z7 B$ g7 s' rb(t)=c*p1(t)/(c.^2*p1(t)+Rvv);   
/ T$ ~+ n# i9 ~0 W) ls(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));   
% A+ R$ g0 l0 w% a* h! }; tp(t)=p1(t)-c*b(t)*p1(t);   
, j1 H* x9 v$ M/ E% p4 S( Zend   
- j/ c6 f/ j: P: R+ ^! i! f1 R! s* N: Rt=1:N;   ( g- J4 }9 C( }6 S2 b# i
plot(t,s,'r',t,Y,'g',t,x,'b');   
$ U7 Y$ z7 z8 [: dfunction [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin)   , K# X" }4 b& P0 `
% Kalman filter.   7 R9 v" I( d9 O# a6 F
% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...)   
% _5 u* f5 X. Q* ?4 w1 l%   
2 N2 B6 B* ~8 S0 X8 V% INPUTS:   
3 N' z" F$ U& h% _8 C: Z% y(:,t) - the observation at time t   2 p' x* I* a* Q1 Y3 v
% A - the system matrix   $ S. V, I" Q0 G' s
% C - the observation matrix   
7 m) W. v6 F2 E% Q - the system covariance   $ Z  C  s' @8 h5 o2 J9 m6 o$ x
% R - the observation covariance   ; i$ D" Q8 Z! U4 m
% init_x - the initial state (column) vector   
( A$ }% U! @- x$ C0 ^/ w% init_V - the initial state covariance   
. D9 f0 W$ M; D% c6 V8 j+ L0 [%     X% A: c% ~4 f* R8 h, V) A/ b
% OPTIONAL INPUTS (string/value pairs [default in brackets])   6 |4 a  f( z" p: c
% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ]   
& ]$ V8 B+ |" k8 b2 q% C% In this case, all the above matrices take an additional final dimension,   9 ^  _8 `" w5 L; C. y  g
% i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m).   1 ?! ^* W! A7 A4 X$ P
% However, init_x and init_V are independent of model(1).   , ^% [$ ?0 q$ z' ~* d
% 'u' - u(:,t) the control signal at time t [ [] ]   " t  A  Z( S6 \" M* ~3 A" v
% 'B' - B(:,:,m) the input regression matrix for model m   ; k' Z7 M+ P6 k
%   
2 J& }- P7 N9 Z; N0 V) q# e9 r% OUTPUTS (where X is the hidden state being estimated)   # N3 ^+ h9 V+ b6 }) s( l
% x(:,t) = E[X(:,t) | y(:,1:t)]   ( k7 |0 Z' J% z; t
% V(:,:,t) = Cov[X(:,t) | y(:,1:t)]   9 n# b! N) l  ~* g
% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2   
4 p: _5 I; [( `% loglik = sum{t=1}^T log P(y(:,t))   
; Q- z$ h% r$ q& c% ^& ~%   6 N5 S# F0 k. d. [" I8 x+ s
% If an input signal is specified, we also condition on it:   % s4 Z9 P8 f9 b: M5 c/ x5 a
% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]   - r4 c% o+ c3 _% q; U% E7 o
% If a model sequence is specified, we also condition on it:   0 J- a- z& _8 V
% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]   
- {  K& i+ W4 u  p0 A3 d6 D7 V[os T] = size(y);   
$ q. ?/ z5 @9 f* u" Jss = size(A,1); % size of state space   
# S% D) O8 E  M+ }  `% set default params   
- F' z5 P; B. tmodel = ones(1,T);   1 X/ z. M+ d. S4 K
u = [];   0 l5 a5 v9 }! ~! U
B = [];   . |& @, Z' q9 @  |4 X  f/ i* {0 m5 |
ndx = [];   # p; B. P) O9 E' s
args = varargin;   + g7 w$ L9 T; R/ j! X4 C
nargs = length(args);   
- O$ _& h9 _7 @5 m0 Hfor i=1:2:nargs   ! I& t) Z! g" C- G8 o
switch args   
  |  g; t4 i3 R+ F) P$ Lcase 'model', model = args{i+1};   $ X6 M0 u+ t, ]' u6 s: F
case 'u', u = args{i+1};   
; L9 A* h5 a& Xcase 'B', B = args{i+1};   2 K! }! W. n! W; y
case 'ndx', ndx = args{i+1};   5 E- f3 ?) w) K4 R8 n0 q* \
otherwise, error(['unrecognized argument ' args])   0 v! w9 F% g5 j' {9 G5 R
end   ) E5 V9 ~% V$ z, m+ p* k# w
end   $ o- [' ~4 ?; ?1 v" t# a3 H+ G
x = zeros(ss, T);   
; C$ a# |+ N% t9 Y- UV = zeros(ss, ss, T);   % v$ O  ~& b" m. P& R. a- n
VV = zeros(ss, ss, T);   , m2 I/ i5 {3 G/ A8 e/ J/ z/ m
loglik = 0;   0 Y1 Z% E" E7 ~$ i$ d4 T
for t=1:T   m = model(t);   ; D- z9 J) k' F; }2 Y6 g
if t==1   %prevx = init_x(:,m);   + B* X7 }* V. f1 @  `8 D
%prevV = init_V(:,:,m);   4 r. ]* H# o1 y' y9 s
prevx = init_x;   
& q3 ?& c0 J* n4 {prevV = init_V;   
8 o6 e. V( w- |, v" _initial = 1;   ; {9 O  w7 q1 @/ j- [! i
else   prevx = x(:,t-1);   
" o- M0 ?) G$ O9 ]" }, k+ v+ EprevV = V(:,:,t-1);   
& ?( K5 P( K* Z% Rinitial = 0;   
$ \2 L% O/ v1 N0 Q- kend   
9 a1 f* d1 t  x3 g% {6 g" Lif isempty(u)   
% W" C! b" Z, T[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...   
& s, {9 v! T0 ~. g/ B$ y$ @/ ]kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial);   else   2 {% @2 n9 w3 f$ S/ @
  if isempty(ndx)   [x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...   7 _  C3 r, M6 V$ x" G; Y( K9 e
     kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ...   'initial', initial,     'u', u(:,t), 'B', B(:,:,m));   ; b8 _0 x: G. `! {- `/ y, A2 Y6 W/ U
else   
- }; e+ Q1 T' B& Y! \i = ndx;   
3 D& Z( a) B2 B6 z* w& b$ a% copy over all elements; only some will get updated   x(:,t) = prevx;   & ^8 W7 V+ h6 b* q: A( F1 J) ?" v
prevP = inv(prevV);   
+ r0 D; y5 l; B1 L" a4 ~5 KprevPsmall = prevP(i,i);   
% c  ]# ~+ j5 w1 d0 A9 {; FprevVsmall = inv(prevPsmall);     a  k# @* T; M7 @2 a; R
[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));   9 e' n3 ?& O& K" p. @
smallP = inv(smallV);   
, l& J2 A, V' q. \' S3 KprevP(i,i) = smallP;   
) g1 o0 j% k- C% n/ H1 rV(:,:,t) = inv(prevP);   
2 I  H7 t$ X* ^9 d4 `end   
. H) b/ X$ X0 }end   , e4 u, k2 m4 n4 D% s
loglik = loglik + LL;   - ~. p: C& V, F- M" n3 ~% S
end
作者: 剑游九天    时间: 2011-12-19 23:38
厚积薄发 发表于 2011-11-28 10:48 ! q# ^) [. g% w# {+ u
matlab下面的kalman滤波程序
5 E* n, K  w; H. w! ]. e3 nclear  N=200; w(1)=0;   , ^4 {- `- k0 J, J! M. F
w=randn(1,N)

- q* J$ J" j- p, f5 q强大呀,下了
作者: 工科男    时间: 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/) Powered by Discuz! X2.5