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
|