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

uperrua

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

function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y）
%----------------输入参数--------------------------------------------------
% m4 g/ D& ~2 K; Y- ~# ?+ v5 t%      A -- 系统矩阵
%      H -- 观察矩阵
! c0 K/ j7 k  r) j: ?" y8 b9 q%     Rw -- 扰动向量
%     Rv -- 测量噪声
4 I& a  ^2 V+ k  m' P6 |) K%   Rw_c -- 扰动向量的协方差
" t. u5 z" ?6 Z% r$ l& b%   Rv_c -- 测量噪声的协方差
1 d4 _# s$ u; X9 c8 C%     x0 -- 节点初始位置向量(x,y)'
4 m, t7 [% M* q/ {% C%     p0 -- 初试协方差阵
0 X4 B: h9 c5 S8 y%      y -- 采样周期
1 R# I' m3 `/ V' c4 O%--------------------------------------------------------------------------

3 F/ J9 Y' J1 Q& R" P! f%--------------------------------------------------------------------------
%   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)
%  P(k|k-1) = A*P(k-1|k-1)*A' + Q
3 r% r. a- b& i0 d  B  p& ?. @# h%  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
# ~8 F; F; Y8 E%  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)
  @' a, l5 H0 a% s%--------------------------------------------------------------------------

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

r = size(A,1);                                                            %获取系统矩阵A的行数
I= eye( r(1) );                                                           %生成单位矩阵
P1 = zeros( r(1),r(1) );                                                  %初始化协方差阵

%初始化节点位置，协方差阵
X = x0;
+ Z4 [1 M" S" m* @3 K4 Y7 l; n" ] P = p0;

len1 = size( H*X ,1);
* g8 X; \7 }" X  r& c. _9 ^" `6 Y# t for s=1:1:len
     z1 = A*X+ Rw;                        % X(k) = A*X(k) + Rw(k)     协方差  Rw_c
     zx(1:r(1),s) = z1(1:r(1)) ;   
     z2 = H*X + Rv;                       % Z(k) = H*X(k) + Rv(k)     协方差  Rv_c
& `7 T5 C6 N2 {( a( |. Z     zy(1:len1,s) = z2(1:len1 );
' b. m# r8 s& o. m   
     P1 = A*P*A' + Rw_c;                  %  P(k|k-1) = A*P(k-1|k-1)*A' + Q
7 n4 \% I# C4 E( Y     K = P1*H'*inv( H*P1*H' + Rv_c );     %  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
) z* S* g, l' p) \  W4 a- g     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))
     P = ( I - K*H ) * P1;                %  P(k|k) = (I-kg(k)*H)*P(k|k-1)
6 C1 k9 o) O' O& X0 o+ O2 ?2 N end

return

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

CCxiaom

Kalman滤波的Matlab仿真程序解读