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

monsterskyy

卡尔曼滤波仿真程序用于匀速运动的目标跟踪

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
T=0.1;
, `: Q/ v' [" A
% yp denotes the sample value of position
: {" z* L7 A6 F$ T% yv denotes the sample value of velocity
% Y=[yp(n);yv(n)];
- r. f7 v) x# d6 j0 W1 a) H2 J% error deviation caused by the random acceleration
% known data
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 T
    0 1];         
B=[1/2*(T)^2 T]';
- h8 _. t$ F; n1 G* L4 n$ R9 bH=[1 0];

+ }' d6 o! s3 h1 a0 J) Y6 _( \3 DC0=[0 0
    0 1];
C=[C0 zeros(2,2*199)];
Q=(0.25)^2;
; z; O+ g# v+ U  \' h+ P0 ER=(0.25)^2;

# }' 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));
    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';
end

% the diagram of position after filtering
4 i5 I/ A7 K1 |! j4 L9 G6 ~( }. at=0:0.1:20;
figure(2);
yp=Y(1,：);
plot(t,yp,'r.-');
axis([0 20 0 20]);
: s( R+ [4 Z7 g8 X: Q+ j# V5 Sxlabel('time');
7 z) u% T2 q; cylabel('yp position');
title('the track after kalman filtering');
hold on;

% the diagram of velocity after filtering
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');
ylabel('yv velocity');
title('the velocity caused by random acceleration');


' \2 {# p/ _* V3 O* n

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

' e) P( M( l" X; w" A# B0 N9 a

nevadaooo

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