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

monsterskyy

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

" T: h9 S3 m. W( q! t8 l2 {( C% kalman filtering
/ ]& m: ^! @$ `* x
load initial_track  s; % y:initial data,s:data with noise
% T; s  s4 W' r- Z: s! A7 q& KT=0.1;
- W8 A1 [: O/ M% l) A( x
% yp denotes the sample value of position
% yv denotes the sample value of velocity
; z* ~( z. E  U& k( ?/ I& P% Y=[yp(n);yv(n)];
2 u. s7 [1 B, c7 G( o% error deviation caused by the random acceleration
% known data
Y=zeros(2,200);
  D$ D; d. q" |% C( v' JY0=[0;1];
Y(:,1)=Y0;
A=[1 T
3 {$ V6 G/ J+ g# ]& ^/ ~- [    0 1];         
B=[1/2*(T)^2 T]';
H=[1 0];
0 J/ f3 V! i4 T* l. N+ W' R$ X
$ ?7 \% M9 f$ t8 C& j  SC0=[0 0
0 N# s7 d/ M7 g3 ~, m$ ]1 v1 q    0 1];
+ A7 Q* ^9 I& ~! k( ^C=[C0 zeros(2,2*199)];
) p7 J+ l+ p7 ]2 XQ=(0.25)^2;
" V3 e6 [  r8 oR=(0.25)^2;

6 n3 ?# N" g" O" ?! U
5 s6 J. o0 z; l) S* o; x9 |% kalman algorithm ieration
for n=1:200
5 d- Y8 K# `# y8 x# u" |    i=(n-1)*2+1;
. r6 n8 ^9 y* {/ K/ [, ]0 F5 T; {3 G    K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
    Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));
2 l' I+ o7 O" ^$ C9 R% O  @    Y(:,n+1)=A*Y(:,n);
3 p, r5 P9 e7 m4 V6 b    C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
" @% r# V; U& ~( O; L    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
- Q1 ?- e) s6 P& F  p) ~end
! ~$ E; v* P. k
% the diagram of position after filtering
t=0:0.1:20;
2 z  p, {4 ]6 O. j0 G. R. {figure(2);
yp=Y(1,：);
, Z7 I3 X; A7 \0 D- }plot(t,yp,'r.-');
9 R5 r5 k# X& m( gaxis([0 20 0 20]);
xlabel('time');
ylabel('yp position');
) \7 }" w. u, r6 l9 btitle('the track after kalman filtering');
5 h4 Y3 k& X5 f# T4 X4 a6 z2 Bhold on;
6 d- {" O% q( I0 u/ s1 y/ E
% the diagram of velocity after filtering
figure(3);
3 J) t8 @- E6 uyv=Y(2,：);
plot(t,yv,'k.-');
xlabel('time');
ylabel('yv velocity');
title('the velocity caused by random acceleration');
: m( W; C+ ]9 A* X1 G

& N5 o7 F9 @  b

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

9 q4 ~: F2 U8 [8 U- c

nevadaooo

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