找回密码
 注册
关于网站域名变更的通知
查看: 559|回复: 1
打印 上一主题 下一主题

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

[复制链接]

该用户从未签到

跳转到指定楼层
1#
发表于 2021-4-26 09:58 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式

EDA365欢迎您登录!

您需要 登录 才可以下载或查看,没有帐号?注册

x
卡尔曼滤波仿真程序用于匀速运动的目标跟踪3 X/ y- _, ?; j+ n9 ^8 @2 z

2 H0 v; i# E9 T( Z3 V) q% kalman filtering
; ~' v% }/ U$ ~8 f: o
4 j, {. e& L) F+ y6 V2 Dload initial_track  s; % y:initial data,s:data with noise% z* T% ~3 d: `: L( a3 V
T=0.1;
, `: Q/ v' [" A% H8 k5 O! a) d
% yp denotes the sample value of position
: {" z* L7 A6 F$ T% yv denotes the sample value of velocity7 c6 D; E0 ^" ~* U! ?
% Y=[yp(n);yv(n)];
- r. f7 v) x# d6 j0 W1 a) H2 J% error deviation caused by the random acceleration* X9 e, U& T/ z% h) N  P
% known data- @: `. k# |" R) e
Y=zeros(2,200);
/ p) @6 x1 o0 ^8 [" nY0=[0;1];
; U' W6 i' J" g$ b! fY(:,1)=Y0;
) f! w+ O! d/ f% U2 U2 z8 h: p1 a* X" @A=[1 T9 L1 Q+ {" J  f3 T$ T% ^
    0 1];         ( ?! h# K. q% h. e3 t3 l
B=[1/2*(T)^2 T]';
- h8 _. t$ F; n1 G* L4 n$ R9 bH=[1 0];3 ^9 }- S7 D7 \* E/ Y

+ }' d6 o! s3 h1 a0 J) Y6 _( \3 DC0=[0 0( h/ R7 z8 Q# k  |  u+ s- q2 v: n
    0 1];" [- G; Z4 t( [1 X
C=[C0 zeros(2,2*199)];, M! R' O, i5 j0 ]& Y
Q=(0.25)^2;
; z; O+ g# v+ U  \' h+ P0 ER=(0.25)^2;# A/ R0 ~' J$ L) O( Z

# }' S7 q! W- P3 P: P
$ d& ]# K6 I9 b% kalman algorithm ieration
) H: q8 |$ h; wfor n=1:200
' c4 n, D4 u1 A4 B/ Z4 n) E* O    i=(n-1)*2+1;
: f% e6 G& U( }  ^3 R) ?( {" }$ b    K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
1 d- n) `# Q3 b    Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));0 u% [1 S4 |3 I8 m/ _4 ^
    Y(:,n+1)=A*Y(:,n);
7 i3 N+ ~4 C) D. I1 D. `    C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
# ]9 f; ^5 j: y3 G. @! a& d% n    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';8 q3 H5 |5 \  R% E% P+ y; j4 W
end$ |: ?5 o; p8 m% }  G3 B1 t; E
7 F6 G1 l2 w! \
% the diagram of position after filtering
4 i5 I/ A7 K1 |! j4 L9 G6 ~( }. at=0:0.1:20;9 y% ~6 a4 Z4 R( _
figure(2);8 s9 e) g* _1 r/ L% ~" U" h; {
yp=Y(1,:);7 q9 x  f0 f  H/ d# F# ]
plot(t,yp,'r.-');  \! i$ p, i8 A5 H: k/ y3 C6 v
axis([0 20 0 20]);
: s( R+ [4 Z7 g8 X: Q+ j# V5 Sxlabel('time');
7 z) u% T2 q; cylabel('yp position');4 Z8 R# B! ]- K" h" _$ r
title('the track after kalman filtering');& o' h  Z6 E, L) U1 i, u2 u
hold on;, F5 a7 q4 J+ g( c, ]3 q
# r+ V2 S3 N; Q. S. T3 V) k
% the diagram of velocity after filtering5 q2 L* z3 }4 o, M( S
figure(3);
$ L2 u% ]* Q- |) eyv=Y(2,:);
" w/ ]+ _( B% j0 z4 ]& e6 Cplot(t,yv,'k.-');
; x- A% h, a( ~8 T) Rxlabel('time');/ _. ^* a9 x+ z) \* _5 X' s
ylabel('yv velocity');3 }3 D. n8 o. V! A( ?
title('the velocity caused by random acceleration');8 W  k5 g. G2 S# Z5 ]& D
& i/ p* O1 Y' }

' \2 {# p/ _* V3 O* n
游客,如果您要查看本帖隐藏内容请回复

' e) P( M( l" X; w" A# B0 N9 a  N' K. K" S0 M/ V+ |
! v; X* e' m  {3 y9 n
  • TA的每日心情
    慵懒
    2022-12-26 15:28
  • 签到天数: 1 天

    [LV.1]初来乍到

    2#
    发表于 2021-4-26 11:19 | 只看该作者
    有没有对运动对象本身的仿真啊,分享一下
    您需要登录后才可以回帖 登录 | 注册

    本版积分规则

    关闭

    推荐内容上一条 /1 下一条

    EDA365公众号

    关于我们|手机版|EDA365电子论坛网 ( 粤ICP备18020198号-1 )

    GMT+8, 2025-10-30 13:57 , Processed in 0.156250 second(s), 27 queries , Gzip On.

    深圳市墨知创新科技有限公司

    地址:深圳市南山区科技生态园2栋A座805 电话:19926409050

    快速回复 返回顶部 返回列表