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

monsterskyy

卡尔曼滤波仿真程序用于匀速运动的目标跟踪
* m3 b* f+ I4 t7 p: u. S$ b, _0 a
- h2 V0 v8 h: w8 T  `% kalman filtering
8 N5 T8 c# ~5 }& q7 Q
load initial_track  s; % y:initial data,s:data with noise
T=0.1;
1 ~& ^5 p/ R2 B% ?1 e' ?+ ]- G) J
, u: a6 c0 e6 K+ G! ?; D2 t% w. a  Y% yp denotes the sample value of position
% yv denotes the sample value of velocity
% Y=[yp(n);yv(n)];
" c  N8 C: Y! i1 F% error deviation caused by the random acceleration
% known data
2 T! P5 X& f: ?( P2 ?Y=zeros(2,200);
5 `3 [$ r% F; i+ v0 aY0=[0;1];
Y(:,1)=Y0;
2 F+ {/ J* r9 a9 _3 f8 n( f5 VA=[1 T
    0 1];         
+ A, Y3 H) L; Z( k2 e7 k( d# G' y# g9 mB=[1/2*(T)^2 T]';
2 [* F% T, H# L2 n7 dH=[1 0];

C0=[0 0
    0 1];
7 s0 m6 @) w3 u3 L, }; ?3 p/ N% TC=[C0 zeros(2,2*199)];
Q=(0.25)^2;
R=(0.25)^2;
7 }" m& S( Y, M' |* h: \$ ?
. e) a# w/ }  C6 W" `9 `
! ^* ~/ S/ E, W, v$ @( Z9 \: T* a% kalman algorithm ieration
6 _0 h3 _* L0 `# O. Y( v. X) ~for n=1:200
    i=(n-1)*2+1;
    K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
    Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));
" H" N* m/ Z0 \! G/ Y    Y(:,n+1)=A*Y(:,n);
    C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
0 v' H) x" {3 A    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
end
! }2 p) ^$ A2 a- F1 |! `3 D! {0 B
% the diagram of position after filtering
* J$ K/ x8 ~8 W: x& L4 Q" q7 j) `t=0:0.1:20;
figure(2);
) \2 k9 c3 T, s  c; f  Qyp=Y(1,：);
4 }8 L6 H. t2 U3 @: R9 iplot(t,yp,'r.-');
axis([0 20 0 20]);
; f! `/ ]) Q0 i3 hxlabel('time');
ylabel('yp position');
title('the track after kalman filtering');
* B0 x, \7 ^/ `$ c6 z5 Nhold on;
. q0 h/ b  I* _1 k' d) r
& w$ q5 F8 o: W# O% the diagram of velocity after filtering
/ ]4 }  ^% ^# o& C' j$ tfigure(3);
yv=Y(2,：);
plot(t,yv,'k.-');
xlabel('time');
# i+ _1 {8 V5 F7 t+ Z9 cylabel('yv velocity');
title('the velocity caused by random acceleration');
( t9 E" X4 d! O6 Q4 E. `( [
; l- v: g- w' b9 p2 U
3 A. Z/ d& J3 e" a/ [

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

1 B% N4 U& W+ z& B
% a2 h0 ?1 D  {( c+ G) \

nevadaooo

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