卡尔曼算法的matlab程序

工科男

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

yulun9988

yulun9988

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

244190977

太令人。。。。

工科男

太强大了

剑游九天

厚积薄发 发表于 2011-11-28 10:48
matlab下面的kalman滤波程序
% ~$ O1 A4 R* r& ^7 {2 g1 v* \, e: Mclear  N=200; w(1)=0; 　　
3 j6 J. q. z+ S+ O7 [8 X/ hw=randn(1,N)

强大呀，下了

厚积薄发

matlab下面的kalman滤波程序
clear  N=200; w(1)=0; 　　
w=randn(1,N)
x(1)=0; 　　
+ F; E. v' I7 M" {1 a7 A% _- fa=1; 　　
for k=2:N; 　　
7 L/ S' Y7 |) Zx(k)=a*x(k-1)+w(k-1); 　　
. E) i# O# o+ u/ _# l$ Xend 　　
! B# s  |/ D) ~) |" a8 tV=randn(1,N); 　　
q1=std(V); 　　
Rvv=q1.^2; 　　
q2=std(x); 　　
" E# v7 T/ j% L# rRxx=q2.^2; 　　
5 f" B4 n: e! ]" Z: ^2 Xq3=std(w); 　　
- ]4 a+ m1 x4 G5 xRww=q3.^2; 　　
c=0.2; 　　
! |5 @' i; U! o; yY=c*x+V; 　　
p(1)=0; 　　
s(1)=0;
for t=2:N; 　　
; \% Y4 U" _; @: E; k' F/ i) ]0 |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)); 　　
% [9 `8 `: c) z  B( }- Cp(t)=p1(t)-c*b(t)*p1(t); 　　
, s/ e7 |' K% d/ ^end 　　
" A$ k; P. `4 {( a0 @; x6 Ct=1:N; 　　
plot(t,s,'r',t,Y,'g',t,x,'b'); 　　
4 `/ T/ {7 }' A* s7 n, w- g* kfunction [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, ...) 　　
% 　　
7 g5 b: s0 y- E) P& z% INPUTS: 　　
" I+ |4 r/ a' y+ T7 a$ {' J% y(:,t) - the observation at time t 　　
% A - the system matrix 　　
% _, N5 d: u3 s& y$ o0 P' ]( w" _0 W3 Z% C - the observation matrix 　　
" X0 \' F1 [, N' S3 e% Q - the system covariance 　　
. m: f* m- p( j  T7 N* |% R - the observation covariance 　　
3 s; w1 N9 x+ S% init_x - the initial state (column) vector 　　
% init_V - the initial state covariance 　　
9 H8 \6 C$ K  a4 I% 　　
% OPTIONAL INPUTS (string/value pairs [default in brackets]) 　　
1 J5 {1 N$ q5 m% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ] 　　
9 S, Z4 Y4 x9 R6 t1 c3 G% In this case, all the above matrices take an additional final dimension, 　　
; H5 _" Z+ H) l+ e- _. S% 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 　　
  A5 h/ X" E  ^* O9 n* y/ Y7 X% 　　
2 L& {! |0 ~' t/ a* ~8 k( Q2 b% OUTPUTS (where X is the hidden state being estimated) 　　
' d/ z0 w7 ~+ f% x(:,t) = E[X(:,t) | y(:,1:t)] 　　
% V(:,:,t) = Cov[X(:,t) | y(:,1:t)] 　　
! p2 V/ r/ ?) n0 C- x8 I" D# [2 u% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2 　　
* Y. D  v8 W6 ~: Q0 W% loglik = sum{t=1}^T log P(y(:,t)) 　　
% 　　
0 ^1 D/ u3 p9 a8 R5 `4 u6 l. h5 P% If an input signal is specified, we also condition on it: 　　
% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)] 　　
0 J; r6 S+ }1 U5 C# b1 t# f. F% 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)] 　　
* K+ h) j6 O3 H[os T] = size(y); 　　
" ^4 A" ^! V" ^. ^ss = size(A,1); % size of state space 　　
% set default params 　　
& Y3 I; y4 d& f2 C( F% g- Lmodel = ones(1,T); 　　
u = []; 　　
B = []; 　　
5 x1 k) Z: F0 Z3 d& }7 t5 hndx = []; 　　
args = varargin; 　　
nargs = length(args); 　　
for i=1:2:nargs 　　
switch args 　　
case 'model', model = args{i+1}; 　　
$ O. L( D" N# v+ R3 [; Acase 'u', u = args{i+1}; 　　
3 G. x2 V6 }: Y* E$ O. acase 'B', B = args{i+1}; 　　
' D1 z0 ]: l2 ]  B! zcase 'ndx', ndx = args{i+1}; 　　
otherwise, error(['unrecognized argument ' args]) 　　
3 C7 X5 W- S6 `8 \) S$ Tend 　　
end 　　
x = zeros(ss, T); 　　
V = zeros(ss, ss, T); 　　
" Q' n' \4 r. s! C  ]VV = zeros(ss, ss, T); 　　
loglik = 0; 　　
9 S& e8 d( G  k0 A: f, T7 j: {for t=1:T 　　m = model(t); 　　
if t==1 　　%prevx = init_x(:,m); 　　
! p7 u/ u% A" D& i& `( c) o: h. Y%prevV = init_V(:,:,m); 　　
$ N( e+ K- @/ o# ~8 m9 n. W7 [4 G4 jprevx = init_x; 　　
! M9 F& i9 y; K% r' G6 sprevV = init_V; 　　
initial = 1; 　　
else 　　prevx = x(:,t-1); 　　
  e% r+ b6 {' Q, z+ A; R$ }prevV = V(:,:,t-1); 　　
& z. s6 o* M" E1 ~- Y- F% M% dinitial = 0; 　　
6 A( ~, m3 e) g0 bend 　　
" i4 X  q. t/ _/ k- Hif isempty(u) 　　
5 j! S# n; n" p' Q[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ... 　　
: n3 R) g7 G) E( o) lkalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial); 　　else 　　
2 p" T% A- G2 Y  if isempty(ndx) 　　[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ... 　　
# o. k+ _) M6 n3 T. r' G     kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ... 　　'initial', initial,     'u', u(:,t), 'B', B(:,:,m)); 　　
else 　　
+ X* V* L3 ?8 v0 g- m& t; {i = ndx; 　　
% copy over all elements; only some will get updated 　　x(:,t) = prevx; 　　
/ ?4 q6 s/ D1 B8 n0 F; LprevP = inv(prevV); 　　
- X, ~, ~' @5 K1 T5 y- aprevPsmall = prevP(i,i); 　　
+ `' [$ I0 h( r% i) [! x( SprevVsmall = 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); 　　
1 I, A% a5 H* `% H/ n0 D+ [prevP(i,i) = smallP; 　　
1 U+ S4 @( f/ S( wV(:,:,t) = inv(prevP); 　　
) t4 u) q, n: o  d0 j( Q  R  \end 　　
& [; @* @. Z" F7 F& Gend 　　
loglik = loglik + LL; 　　
9 _/ n; j$ H& ^7 z( @/ ?4 Uend