找回密码
 注册
关于网站域名变更的通知
查看: 437|回复: 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)( S0 u1 ]( I8 `
%----------------输入参数--------------------------------------------------$ |6 K# L2 d0 @) k
%      A -- 系统矩阵5 o/ }  O. C+ W/ x1 g( q0 A+ G
%      H -- 观察矩阵
' J9 m8 @+ L0 i7 a8 a# H%     Rw -- 扰动向量
+ M2 m/ d- |& ~  p%     Rv -- 测量噪声8 u: e! Q3 i1 C4 Q: }
%   Rw_c -- 扰动向量的协方差/ t- w4 V+ L2 U1 [$ \
%   Rv_c -- 测量噪声的协方差  i- [4 `* O, |% d. L+ I; J
%     x0 -- 节点初始位置向量(x,y)'$ s2 @) H4 Z! s+ l2 T
%     p0 -- 初试协方差阵
1 B8 `% ^: B9 u%      y -- 采样周期/ o5 {2 ~6 {: @/ ?3 u- _
%--------------------------------------------------------------------------

! Y5 Z2 w2 \2 }+ l6 z
%--------------------------------------------------------------------------4 F, l, a6 @0 l3 N3 g3 m
%   X(k) = A*X(k) + Rw(k)     噪声Q9 j% E" F. a; K3 b( D
%   Z(k) = H*X(k) + Rv(k)     噪声R
3 k: h' N: B+ p# A* }! S/ f0 M

%  x(k|k-1) = A*x(k-1|k-1) + B*U(k)2 X: V2 l- Y% q. K2 }/ ~
%  P(k|k-1) = A*P(k-1|k-1)*A' + Q
4 z1 m. X5 E+ c( S" O" e%  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1)). {5 O+ T) s! y3 P. ^
%  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
8 I6 ]) o7 g3 x$ b! h' |8 R%  P(k|k) = (I-kg(k)*H)*P(k|k-1)9 U# D6 O: E0 [3 `2 S
%--------------------------------------------------------------------------

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

r = size(A,1);                                                            %获取系统矩阵A的行数
; V7 W2 h- t9 _, _. ~$ I& b; P+ `5 R I= eye( r(1) );                                                           %生成单位矩阵
0 _/ p3 S6 r7 T) W3 Z' x P1 = zeros( r(1),r(1) );                                                  %初始化协方差阵

%初始化节点位置,协方差阵* [6 N! q/ c& M5 O
X = x0;
; x$ U! U/ Q1 R P = p0;

len1 = size( H*X ,1);
" f& d3 K' F# c/ U; X for s=1:1:len% u/ A* ]  ?# f; m" D
     z1 = A*X+ Rw;                        % X(k) = A*X(k) + Rw(k)     协方差  Rw_c! O. ?8 v9 r6 G+ |0 ~3 i: h& r
     zx(1:r(1),s) = z1(1:r(1)) ;   
- M7 a, ~4 ?7 C, v5 H- P     z2 = H*X + Rv;                       % Z(k) = H*X(k) + Rv(k)     协方差  Rv_c
3 [5 U8 [$ ?" z4 |# E  _     zy(1:len1,s) = z2(1:len1 );: [# s9 N. `( Z9 G
    5 P" ]" {" {4 x' ~- B' t
     P1 = A*P*A' + Rw_c;                  %  P(k|k-1) = A*P(k-1|k-1)*A' + Q
2 q- s- z9 T5 G5 q2 @& V     K = P1*H'*inv( H*P1*H' + Rv_c );     %  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
6 K( H9 E3 d  X3 o     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 h- S1 J1 O/ A: @# }  q& V6 J+ `
     P = ( I - K*H ) * P1;                %  P(k|k) = (I-kg(k)*H)*P(k|k-1)7 {  h& E. {0 h# n$ i/ p! t1 w
end


6 ?5 F: y2 g' Y; [+ W$ ]return

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

& ?8 K7 U4 T, I2 O

该用户从未签到

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

本版积分规则

关闭

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

EDA365公众号

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

GMT+8, 2025-7-25 13:44 , Processed in 0.125000 second(s), 23 queries , Gzip On.

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

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

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