卡尔曼算法的matlab程序

工科男

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

yulun9988

yulun9988

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

244190977

太令人。。。。

工科男

太强大了

剑游九天

厚积薄发 发表于 2011-11-28 10:48
matlab下面的kalman滤波程序
clear  N=200; w(1)=0; 　　
# D3 S1 y) O6 P0 Z) ^& p- Zw=randn(1,N)

强大呀，下了

厚积薄发

matlab下面的kalman滤波程序
, T: v0 o8 @3 i$ B3 ]6 k1 Lclear  N=200; w(1)=0; 　　
w=randn(1,N)
6 H: E* b; V# l( r& `x(1)=0; 　　
; C1 K, w8 i6 B* J) O* D0 Aa=1; 　　
for k=2:N; 　　
; ]7 B% o0 C" N1 N( Gx(k)=a*x(k-1)+w(k-1); 　　
end 　　
# j1 q* `: H4 }# XV=randn(1,N); 　　
q1=std(V); 　　
Rvv=q1.^2; 　　
* V$ O* D1 [3 G% Xq2=std(x); 　　
Rxx=q2.^2; 　　
2 C! @) f! l$ Iq3=std(w); 　　
; K# b8 X, d$ G6 SRww=q3.^2; 　　
c=0.2; 　　
Y=c*x+V; 　　
p(1)=0; 　　
; K. c* A5 Q5 t+ _7 S* ts(1)=0;
6 |/ f" |9 y# r; r! ?for t=2:N; 　　
& |8 Y3 R, {" C  _/ a( gp1(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)); 　　
# F1 F$ F5 C$ K. S9 o! mp(t)=p1(t)-c*b(t)*p1(t); 　　
end 　　
9 h, |; o4 G# I! `' k  {t=1:N; 　　
( \0 n4 w( P# nplot(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) 　　
* t0 r: y/ m; o+ j8 G% Kalman filter. 　　
' q' q$ ^) {/ H& Z' ?% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...) 　　
% 　　
% INPUTS: 　　
% y(:,t) - the observation at time t 　　
! \' t" E# s# B: K) N( v+ h% A - the system matrix 　　
% C - the observation matrix 　　
$ ?, K" f+ s. t& o* v% C% Q - the system covariance 　　
$ q9 ~% M9 @* d, R! D% R - the observation covariance 　　
& m6 x- s) r9 t/ Z  a/ \% init_x - the initial state (column) vector 　　
. I' q# J  H6 A/ p8 x! k% init_V - the initial state covariance 　　
% 　　
% OPTIONAL INPUTS (string/value pairs [default in brackets]) 　　
5 Y$ Q: X7 {  A% '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, 　　
# n7 H4 E( `8 D7 o% i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m). 　　
' q+ |$ Z' n4 g0 v/ f/ F% However, init_x and init_V are independent of model(1). 　　
% 'u' - u(:,t) the control signal at time t [ [] ] 　　
! Q& T7 A. q2 R6 P1 a" ^% 'B' - B(:,:,m) the input regression matrix for model m 　　
0 p! L% k( s; ~  f% 　　
% 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)] 　　
) I/ q. D! ~. V$ e2 m0 v$ U1 q% 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 　　
  u' K. X' s: _2 g' L6 M1 c- F* g% set default params 　　
model = ones(1,T); 　　
u = []; 　　
% h. T6 P- g  q1 [  W% C3 y: p: NB = []; 　　
ndx = []; 　　
5 N# x2 ]' ]  a; H+ O& l7 o& F- Vargs = varargin; 　　
7 m: O( G1 X6 ~1 g& b6 Y2 `nargs = length(args); 　　
0 ^" ?* [! I0 F7 c8 sfor i=1:2:nargs 　　
( B" s. \7 X$ O6 I& n- ^switch args 　　
) N" [5 h( ^& l! U+ [2 _7 tcase 'model', model = args{i+1}; 　　
" R$ n4 K7 B/ j- }. ^" o9 L$ O/ ]case 'u', u = args{i+1}; 　　
( T3 w+ |; r  c  Hcase 'B', B = args{i+1}; 　　
' `7 P# |3 [5 E" P4 s5 L7 A  Jcase 'ndx', ndx = args{i+1}; 　　
otherwise, error(['unrecognized argument ' args]) 　　
* `/ {6 U( c- c. K8 Qend 　　
end 　　
x = zeros(ss, T); 　　
V = zeros(ss, ss, T); 　　
VV = zeros(ss, ss, T); 　　
loglik = 0; 　　
" F0 z7 s3 y1 j, C- Lfor t=1:T 　　m = model(t); 　　
if t==1 　　%prevx = init_x(:,m); 　　
%prevV = init_V(:,:,m); 　　
prevx = init_x; 　　
- G& ~. R3 J4 L; _) OprevV = init_V; 　　
6 {# b- }9 b$ dinitial = 1; 　　
% Y0 T) @( @* G8 n  Melse 　　prevx = x(:,t-1); 　　
5 S; K6 C1 S; d1 g, HprevV = V(:,:,t-1); 　　
initial = 0; 　　
) l7 Q0 h* [( B3 g2 {& J7 ^6 i! pend 　　
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 　　
( p7 ^( f8 o  H  if isempty(ndx) 　　[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ... 　　
2 U: `4 t4 h- s4 t# m6 o3 u& i     kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ... 　　'initial', initial,     'u', u(:,t), 'B', B(:,:,m)); 　　
; ]8 n# m8 q+ xelse 　　
' E8 k  K  `6 W* t. si = ndx; 　　
% copy over all elements; only some will get updated 　　x(:,t) = prevx; 　　
prevP = inv(prevV); 　　
prevPsmall = prevP(i,i); 　　
& J' y& m; f8 N. ZprevVsmall = inv(prevPsmall); 　　
$ b( X/ U! H$ J' m1 j: s+ ?5 B9 V# ~& 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; 　　
- E# I' V7 ^7 j: Z: ~  zV(:,:,t) = inv(prevP); 　　
end 　　
end 　　
loglik = loglik + LL; 　　
$ |8 Q4 r0 G1 |end