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

uperrua

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

function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y）
) m& b  ~. {4 U  a" H" m! G%----------------输入参数--------------------------------------------------
! f/ T0 R% N3 X" r& H% ^. _%      A -- 系统矩阵
- h; ?& f4 E1 M%      H -- 观察矩阵
) c9 }1 K5 u- h* H1 w& ?%     Rw -- 扰动向量
%     Rv -- 测量噪声
%   Rw_c -- 扰动向量的协方差
%   Rv_c -- 测量噪声的协方差
7 [6 k* K  L9 w- [0 ?- R% L%     x0 -- 节点初始位置向量(x,y)'
%     p0 -- 初试协方差阵
6 {2 \5 S9 D" T6 B%      y -- 采样周期
* b; y8 ~) S4 M- M/ k+ v5 Y8 C%--------------------------------------------------------------------------

# K% R, ]% J6 F( N; U* J+ @%--------------------------------------------------------------------------
%   X(k) = A*X(k) + Rw(k)     噪声Q
%   Z(k) = H*X(k) + Rv(k)     噪声R

%  x(k|k-1) = A*x(k-1|k-1) + B*U(k)
3 r& P# K, X: y0 @& ]- u%  P(k|k-1) = A*P(k-1|k-1)*A' + Q
* E% `* d. T, N; M%  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
6 H" ^9 c% v% V8 i- w) t1 K! R%  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
%  P(k|k) = (I-kg(k)*H)*P(k|k-1)
%--------------------------------------------------------------------------

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

r = size(A,1);                                                            %获取系统矩阵A的行数
I= eye( r(1) );                                                           %生成单位矩阵
" P3 `* P$ S" H+ Z P1 = zeros( r(1),r(1) );                                                  %初始化协方差阵

%初始化节点位置，协方差阵
X = x0;
% f- i& q& F" m) V% F P = p0;

len1 = size( H*X ,1);
+ G" [' O$ N' l$ }% Z0 T4 c for s=1:1:len
, H" w4 f# p4 z- W     z1 = A*X+ Rw;                        % X(k) = A*X(k) + Rw(k)     协方差  Rw_c
     zx(1:r(1),s) = z1(1:r(1)) ;   
# r9 D  ]- L8 k' O( ?     z2 = H*X + Rv;                       % Z(k) = H*X(k) + Rv(k)     协方差  Rv_c
; l2 C: _6 |5 ^     zy(1:len1,s) = z2(1:len1 );
   
0 D# k' Q" K  b2 B! A. K     P1 = A*P*A' + Rw_c;                  %  P(k|k-1) = A*P(k-1|k-1)*A' + Q
     K = P1*H'*inv( H*P1*H' + Rv_c );     %  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
     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))
( `. |/ ~' h/ y6 {! f+ i     P = ( I - K*H ) * P1;                %  P(k|k) = (I-kg(k)*H)*P(k|k-1)
2 G( n; u9 [5 q. _( z, g" ]2 b end

return

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

: e( u4 F: i5 I

CCxiaom

Kalman滤波的Matlab仿真程序解读