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

monsterskyy

卡尔曼滤波仿真程序用于匀速运动的目标跟踪
! ]0 ]; p; S  n
0 `7 C9 O3 B5 a0 Z% kalman filtering
/ R% |% ?6 X& e
load initial_track  s; % y:initial data,s:data with noise
. v3 o9 H- v! V* w& f1 G' X% OT=0.1;
/ x. {" ~* H+ O. X7 ~
% yp denotes the sample value of position
4 o5 `$ V+ d8 Z: \9 l5 F- q% yv denotes the sample value of velocity
- \$ {, P( X: ^% Y=[yp(n);yv(n)];
% error deviation caused by the random acceleration
% known data
  g8 B+ p* A) e! k& BY=zeros(2,200);
Y0=[0;1];
Y(:,1)=Y0;
4 ~8 F7 ]1 E9 a$ f* s9 `3 TA=[1 T
    0 1];         
/ w0 t- t  _% ]4 C4 k# E6 hB=[1/2*(T)^2 T]';
H=[1 0];
& {2 \) y( R% h- ]
C0=[0 0
' B* Z. v2 H% W- y( w" H$ i" Z! t    0 1];
C=[C0 zeros(2,2*199)];
Q=(0.25)^2;
R=(0.25)^2;


% kalman algorithm ieration
' ?7 j' U! S2 hfor 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, @% k& X6 I# u    Y(:,n+1)=A*Y(:,n);
8 A* b+ S2 Z& j' U    C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
4 o/ b1 Z3 A! Q# Z& o    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
8 Q: D- V  _3 Gend
/ H' h% W/ L0 Y3 h* ~3 p0 t8 ]
% the diagram of position after filtering
9 g; p/ J7 \: ^/ f7 jt=0:0.1:20;
9 Q9 P6 P0 M# g7 |5 ]figure(2);
/ c& ?1 M; T$ a* i/ Fyp=Y(1,：);
plot(t,yp,'r.-');
axis([0 20 0 20]);
xlabel('time');
  J, B3 D) d1 X2 @0 N; Zylabel('yp position');
, Q6 O- y9 N- a: T! y' atitle('the track after kalman filtering');
5 `6 x( m; E+ ]2 c8 e7 n( E2 @hold on;
, n* [) T+ K8 U
) D: d# u: x3 d$ \: j6 e% the diagram of velocity after filtering
figure(3);
yv=Y(2,：);
plot(t,yv,'k.-');
* l4 ~% W! e5 j, ], X( r! K7 Oxlabel('time');
% O7 s) Q: v  O! [6 Kylabel('yv velocity');
title('the velocity caused by random acceleration');
: u6 x+ s! p( q! a6 N3 O6 q
- t6 Y3 F# u+ i

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

nevadaooo

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