|
|
EDA365欢迎您登录!
您需要 登录 才可以下载或查看,没有帐号?注册
x
卡尔曼滤波仿真程序用于匀速运动的目标跟踪3 X/ y- _, ?; j+ n9 ^8 @2 z
2 H0 v; i# E9 T( Z3 V) q% kalman filtering
; ~' v% }/ U$ ~8 f: o
4 j, {. e& L) F+ y6 V2 Dload initial_track s; % y:initial data,s:data with noise% z* T% ~3 d: `: L( a3 V
T=0.1;
, `: Q/ v' [" A% H8 k5 O! a) d
% yp denotes the sample value of position
: {" z* L7 A6 F$ T% yv denotes the sample value of velocity7 c6 D; E0 ^" ~* U! ?
% Y=[yp(n);yv(n)];
- r. f7 v) x# d6 j0 W1 a) H2 J% error deviation caused by the random acceleration* X9 e, U& T/ z% h) N P
% known data- @: `. k# |" R) e
Y=zeros(2,200);
/ p) @6 x1 o0 ^8 [" nY0=[0;1];
; U' W6 i' J" g$ b! fY(:,1)=Y0;
) f! w+ O! d/ f% U2 U2 z8 h: p1 a* X" @A=[1 T9 L1 Q+ {" J f3 T$ T% ^
0 1]; ( ?! h# K. q% h. e3 t3 l
B=[1/2*(T)^2 T]';
- h8 _. t$ F; n1 G* L4 n$ R9 bH=[1 0];3 ^9 }- S7 D7 \* E/ Y
+ }' d6 o! s3 h1 a0 J) Y6 _( \3 DC0=[0 0( h/ R7 z8 Q# k | u+ s- q2 v: n
0 1];" [- G; Z4 t( [1 X
C=[C0 zeros(2,2*199)];, M! R' O, i5 j0 ]& Y
Q=(0.25)^2;
; z; O+ g# v+ U \' h+ P0 ER=(0.25)^2;# A/ R0 ~' J$ L) O( Z
# }' S7 q! W- P3 P: P
$ d& ]# K6 I9 b% kalman algorithm ieration
) H: q8 |$ h; wfor n=1:200
' c4 n, D4 u1 A4 B/ Z4 n) E* O i=(n-1)*2+1;
: f% e6 G& U( } ^3 R) ?( {" }$ b K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
1 d- n) `# Q3 b Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));0 u% [1 S4 |3 I8 m/ _4 ^
Y(:,n+1)=A*Y(:,n);
7 i3 N+ ~4 C) D. I1 D. ` C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
# ]9 f; ^5 j: y3 G. @! a& d% n C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';8 q3 H5 |5 \ R% E% P+ y; j4 W
end$ |: ?5 o; p8 m% } G3 B1 t; E
7 F6 G1 l2 w! \
% the diagram of position after filtering
4 i5 I/ A7 K1 |! j4 L9 G6 ~( }. at=0:0.1:20;9 y% ~6 a4 Z4 R( _
figure(2);8 s9 e) g* _1 r/ L% ~" U" h; {
yp=Y(1,:);7 q9 x f0 f H/ d# F# ]
plot(t,yp,'r.-'); \! i$ p, i8 A5 H: k/ y3 C6 v
axis([0 20 0 20]);
: s( R+ [4 Z7 g8 X: Q+ j# V5 Sxlabel('time');
7 z) u% T2 q; cylabel('yp position');4 Z8 R# B! ]- K" h" _$ r
title('the track after kalman filtering');& o' h Z6 E, L) U1 i, u2 u
hold on;, F5 a7 q4 J+ g( c, ]3 q
# r+ V2 S3 N; Q. S. T3 V) k
% the diagram of velocity after filtering5 q2 L* z3 }4 o, M( S
figure(3);
$ L2 u% ]* Q- |) eyv=Y(2,:);
" w/ ]+ _( B% j0 z4 ]& e6 Cplot(t,yv,'k.-');
; x- A% h, a( ~8 T) Rxlabel('time');/ _. ^* a9 x+ z) \* _5 X' s
ylabel('yv velocity');3 }3 D. n8 o. V! A( ?
title('the velocity caused by random acceleration');8 W k5 g. G2 S# Z5 ]& D
& i/ p* O1 Y' }
' \2 {# p/ _* V3 O* n
' e) P( M( l" X; w" A# B0 N9 a N' K. K" S0 M/ V+ |
! v; X* e' m {3 y9 n
|
|