matlab卡尔曼滤波仿真程序（跟踪匀速运动的目标）

monsterskyy

卡尔曼滤波仿真程序用于匀速运动的目标跟踪
/ D- H2 h( j! r, c4 y+ D- |8 N
% kalman filtering
6 c6 k- C; t" t( k' a; w& ^/ J7 ?
load initial_track  s; % y:initial data,s:data with noise
T=0.1;
3 w2 T( o2 K/ `! l- g
% yp denotes the sample value of position
% yv denotes the sample value of velocity
8 H/ z4 {* b& b+ P, K% Y=[yp(n);yv(n)];
% u6 S7 ~( u+ \  B, a! K! F% error deviation caused by the random acceleration
% known data
" r$ |: q  d- RY=zeros(2,200);
: N  j3 A' z! w3 ^4 e, O8 D( nY0=[0;1];
Y(:,1)=Y0;
5 D& Q! c# J3 E) y+ H/ zA=[1 T
% |! w" T$ l5 T8 E  y    0 1];         
B=[1/2*(T)^2 T]';
H=[1 0];

8 e: e  @: [6 `$ Z, S) eC0=[0 0
+ y' ]" h3 H4 G% W6 i# P    0 1];
. q# m4 }# _9 |, iC=[C0 zeros(2,2*199)];
! P/ C# l' @5 }+ i6 HQ=(0.25)^2;
) j; q2 T# H4 m0 {R=(0.25)^2;
4 a/ A' y: P/ u/ f+ g+ K' Y- N7 d
8 @; b9 r% D! B* t0 e  _6 d
9 {! e; G5 L/ l% kalman algorithm ieration
2 {1 _! P& {# W3 jfor n=1:200
' u2 @7 ?% W' J7 g    i=(n-1)*2+1;
- K7 V8 ^' `! m7 o! [: G4 X    K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
9 x0 r! s" m) }- L- U    Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));
    Y(:,n+1)=A*Y(:,n);
    C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
$ c. X" D9 \- q3 n. U4 Cend
% h1 n/ N! }1 p
% the diagram of position after filtering
t=0:0.1:20;
figure(2);
2 |  Q( Y; x; c8 O2 Qyp=Y(1,：);
; |9 T. ?3 \/ Dplot(t,yp,'r.-');
5 S1 b: M3 Y8 p7 }4 maxis([0 20 0 20]);
) i& A% g; i8 l3 ~4 l* Cxlabel('time');
ylabel('yp position');
title('the track after kalman filtering');
# G$ a& P8 Q2 h% L- u1 |6 Uhold on;

% X4 U% D# _" }3 W5 b. ~% the diagram of velocity after filtering
& t6 B0 Q' U, J* u9 _5 d# v* p/ dfigure(3);
. h4 Y) E( o$ h; B: [yv=Y(2,：);
plot(t,yv,'k.-');
( H- M6 }& ]6 V7 O) V" Zxlabel('time');
/ m' \& d$ o# {" Q# @$ b; M2 Hylabel('yv velocity');
title('the velocity caused by random acceleration');

游客，如果您要查看本帖隐藏内容请回复

/ R, m+ [3 W/ G: q, S
- K* Y* }+ Z( S

nevadaooo

有没有对运动对象本身的仿真啊，分享一下