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

转:Kalman滤波的Matlab仿真程序解读

[复制链接]

该用户从未签到

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

EDA365欢迎您登录!

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

x
最近因为要用到Kalman滤波器做东西,所以一直在学习这个东西,鉴于之前的仿真都是用matlab做的,所以呢,kalman滤波器的仿真程序也是用matlab编的。痛苦了几天,几天这个函数终于搞定了,具体的分析如下。

function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y)
: k3 R- R2 [4 Y7 \9 `7 |$ C3 R%----------------输入参数--------------------------------------------------
4 k# D' `, c% g2 @6 _%      A -- 系统矩阵. K! p- l" g- _  i
%      H -- 观察矩阵7 r6 V$ `$ [0 E5 v
%     Rw -- 扰动向量
1 o( Y  W" p' v- N  I8 O9 r%     Rv -- 测量噪声4 |. s0 ]6 S, Q8 i3 w; ^
%   Rw_c -- 扰动向量的协方差. t1 {5 {  Q; U+ S2 l# ^/ D
%   Rv_c -- 测量噪声的协方差
' u$ ^: Y* x: V6 J$ C. m& L%     x0 -- 节点初始位置向量(x,y)'
+ j0 B/ D7 h% B%     p0 -- 初试协方差阵
  ^, Q/ _& `: P/ z1 O- U- O9 o%      y -- 采样周期8 x( m3 v+ O; Y/ _$ ?
%--------------------------------------------------------------------------

2 B, |# c) g4 L, D9 X; h/ o
%--------------------------------------------------------------------------$ s1 X" L# o$ ^
%   X(k) = A*X(k) + Rw(k)     噪声Q
2 u+ W' Q' t4 h; G& M# _( m( H%   Z(k) = H*X(k) + Rv(k)     噪声R
3 l; ?. |1 J5 F( `% O$ Z" d# a) \

%  x(k|k-1) = A*x(k-1|k-1) + B*U(k)7 s: t8 A2 V3 @) {; L+ y
%  P(k|k-1) = A*P(k-1|k-1)*A' + Q2 H9 U. x% S  Q1 E# P. w4 e& {& o
%  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))/ j+ d& T8 |2 D& U( \) z4 q
%  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
( F/ {2 a' A3 X# d) r3 S%  P(k|k) = (I-kg(k)*H)*P(k|k-1)4 o& e6 D/ u- D
%--------------------------------------------------------------------------

len = length( y );                                                        %获取采样点数

r = size(A,1);                                                            %获取系统矩阵A的行数6 @" c  L  j; k9 D' N
I= eye( r(1) );                                                           %生成单位矩阵' M2 D( f: m0 E6 b
P1 = zeros( r(1),r(1) );                                                  %初始化协方差阵

%初始化节点位置,协方差阵
! Z5 W: f* ^% p5 z9 } X = x0;1 I9 z: @, K! D1 S. z! f( Z
P = p0;

len1 = size( H*X ,1);( z( N% q; U: u0 V2 I& I& k% ?
for s=1:1:len; T9 r6 `0 e) c4 \
     z1 = A*X+ Rw;                        % X(k) = A*X(k) + Rw(k)     协方差  Rw_c; s) T" b3 L/ a' p, W. O3 ^
     zx(1:r(1),s) = z1(1:r(1)) ;   
$ Q0 b  w/ W! t2 V' p8 F     z2 = H*X + Rv;                       % Z(k) = H*X(k) + Rv(k)     协方差  Rv_c/ B8 A% B+ I' }4 w
     zy(1:len1,s) = z2(1:len1 );
8 y# e# b1 Y  a: N8 `5 N% F7 n3 Y   
+ `6 x4 V2 b- W. x8 f! V     P1 = A*P*A' + Rw_c;                  %  P(k|k-1) = A*P(k-1|k-1)*A' + Q
& F+ ~" V% u  j6 ]$ H     K = P1*H'*inv( H*P1*H' + Rv_c );     %  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)! Y* j6 ^4 p& I' ~* A8 \5 |
     X = A*X + K*( zy(1:len1,s) - H*A*X );      

                                          %  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
3 b+ W. x# {! N. t  B8 v  Y     P = ( I - K*H ) * P1;                %  P(k|k) = (I-kg(k)*H)*P(k|k-1)
# q" [- k% }$ X$ H: s" m end

1 `, j0 t, ~9 ^; N/ L/ ~+ X
return

原文地址:转:Kalman滤波的Matlab仿真程序解读作者:浩瀚

( q4 x# m5 O2 V% |4 s5 Q/ U

该用户从未签到

2#
发表于 2020-4-27 13:27 | 只看该作者
Kalman滤波的Matlab仿真程序解读
您需要登录后才可以回帖 登录 | 注册

本版积分规则

关闭

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

EDA365公众号

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

GMT+8, 2025-7-24 13:36 , Processed in 0.109375 second(s), 23 queries , Gzip On.

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

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

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