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

monsterskyy

卡尔曼滤波仿真程序用于匀速运动的目标跟踪
% D" o, o$ ~! v5 G/ i) k
% kalman filtering

7 c) x; J4 Z% R' aload initial_track  s; % y:initial data,s:data with noise
T=0.1;

% yp denotes the sample value of position
6 y/ {2 [) J- o" l% yv denotes the sample value of velocity
% Y=[yp(n);yv(n)];
% error deviation caused by the random acceleration
% known data
8 d: n" l- b- F& {2 D/ ^" CY=zeros(2,200);
Y0=[0;1];
8 |: V3 `+ D( g2 l* T0 Z0 m; aY(:,1)=Y0;
' o- u3 a, N' hA=[1 T
8 \% f5 U: U3 U2 ~4 D# r2 n    0 1];         
B=[1/2*(T)^2 T]';
H=[1 0];
3 s' q$ x; I1 |/ `0 F' f
C0=[0 0
    0 1];
C=[C0 zeros(2,2*199)];
; H' p+ Y- p4 T: d- ]( H, |+ cQ=(0.25)^2;
R=(0.25)^2;

0 B4 B- s6 l7 R! u2 G
% kalman algorithm ieration
. E( U9 ^& C( u2 Ufor n=1:200
    i=(n-1)*2+1;
! A$ u8 B1 }) S    K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
    Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));
, B; u. l/ i) `' x, B) }. u) A    Y(:,n+1)=A*Y(:,n);
    C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
7 b+ ~. |; _! r" U& _/ B, ~    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
end
( h( F, n; J' ?- B5 F8 A! Y) Q
' R7 _# F3 C0 h* ^' G9 _% the diagram of position after filtering
t=0:0.1:20;
. }* m  I) y: m( qfigure(2);
yp=Y(1,：);
  g& V5 b5 ]7 Z( x" ]  hplot(t,yp,'r.-');
# V$ {# S! K1 e5 K  j+ }axis([0 20 0 20]);
& o; T2 c8 v4 L: \xlabel('time');
( n1 @9 Y* {: {1 cylabel('yp position');
title('the track after kalman filtering');
hold on;

% the diagram of velocity after filtering
figure(3);
yv=Y(2,：);
plot(t,yv,'k.-');
# s+ r! v$ y2 T9 ~. V( Y' ~0 uxlabel('time');
ylabel('yv velocity');
7 p. z# C7 T& T( }title('the velocity caused by random acceleration');

) S+ e; t! c6 ^' b
- W, {4 S* C4 l3 ]. V" h0 y

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

: i) ^1 _0 A/ v0 f) H' z; R+ n/ N8 y

3 \1 a: r4 f. w+ X& |" Z

nevadaooo

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