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

monsterskyy

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

3 X3 V9 l0 y0 K- k( M) e% kalman filtering

load initial_track  s; % y:initial data,s:data with noise
T=0.1;
+ N/ @' p, h  `( [+ ]# @, @
% yp denotes the sample value of position
: G+ a2 [8 Q8 e' Y% yv denotes the sample value of velocity
% Y=[yp(n);yv(n)];
6 l, W& [7 G! R% error deviation caused by the random acceleration
3 x/ O& |6 a. g% known data
Y=zeros(2,200);
Y0=[0;1];
Y(:,1)=Y0;
: \5 Z& f( n; \% D# |A=[1 T
3 j0 X4 B! _- x! e    0 1];         
B=[1/2*(T)^2 T]';
; M# ~4 E# B/ g- y* P1 m+ A. XH=[1 0];
6 \5 }" R& g' }' R$ O5 I: }
$ a) M# O5 @- o9 G1 c1 bC0=[0 0
    0 1];
) S, E1 ]  g: t; g4 gC=[C0 zeros(2,2*199)];
Q=(0.25)^2;
6 R* M) ]2 ^/ i) \R=(0.25)^2;
2 u6 Z% A( P0 R
, O0 V% [# ~3 `& v# f  S
7 ]8 X6 H6 c5 H; z% kalman algorithm ieration
for n=1:200
    i=(n-1)*2+1;
    K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
: T/ P1 u* y8 B  J8 h  f* H8 f    Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));
    Y(:,n+1)=A*Y(:,n);
' L. z0 a9 w! ~+ e: @* q' d2 T    C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
" \7 x/ R7 L! B8 r    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
end
, R! \# x" \1 I
# A8 e! I, z% `% ?% the diagram of position after filtering
t=0:0.1:20;
figure(2);
8 S6 M' _! k( `yp=Y(1,：);
plot(t,yp,'r.-');
axis([0 20 0 20]);
xlabel('time');
ylabel('yp position');
title('the track after kalman filtering');
/ ~# F8 q1 Z  M- c1 n" ^hold on;

% the diagram of velocity after filtering
figure(3);
( {( S6 U' [. E% Q  Q; |yv=Y(2,：);
1 p) a. r" f! O6 e' P' Tplot(t,yv,'k.-');
/ P3 [, J; X) Q" ^+ z( Axlabel('time');
5 m( T) R* E8 X3 f( qylabel('yv velocity');
: H0 I* p  s& A1 R5 I6 S2 G: ?9 g/ wtitle('the velocity caused by random acceleration');
# F* p5 P. v$ [% n

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

6 ]! U9 a, H* B; r' v/ v- S0 R2 y
: u$ Z- Z% f2 P% Z

nevadaooo

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