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
|