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

function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y）' a# a# A! e, Z7 _0 Z* e
%----------------输入参数--------------------------------------------------' V4 \% Q. g9 k/ Z8 I; q
%      A -- 系统矩阵
+ V& x* c6 y' q) Q! m8 ^+ Z/ M! E7 y%      H -- 观察矩阵; f9 b* S1 i6 u/ e" V  o
%     Rw -- 扰动向量, W( u7 E" i4 ~  y
%     Rv -- 测量噪声
: g1 u+ y: p. T7 I9 ?4 r  z1 _%   Rw_c -- 扰动向量的协方差* L, g7 r6 f' a+ M) _/ h) O
%   Rv_c -- 测量噪声的协方差" ~9 j2 H8 a: Q. S0 s  H
%     x0 -- 节点初始位置向量(x,y)': `. E2 C+ p0 @, N5 w( {- S
%     p0 -- 初试协方差阵
; i8 G5 Y6 L) [+ p7 ~%      y -- 采样周期0 E' ]" ]/ }, i# @& N( o$ u/ g
%--------------------------------------------------------------------------

3 M4 Y/ f3 m3 `" k9 X0 `%--------------------------------------------------------------------------
$ h1 X/ D3 G& X3 I%   X(k) = A*X(k) + Rw(k)     噪声Q& F& i& p/ ]/ D1 d% J
%   Z(k) = H*X(k) + Rv(k)     噪声R
- c6 L, @" P6 N, D+ G

%  x(k|k-1) = A*x(k-1|k-1) + B*U(k)
& C# d/ D$ ]& W8 x%  P(k|k-1) = A*P(k-1|k-1)*A' + Q8 R! m& ^3 ?0 r, g1 |  q8 c# t
%  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))$ R6 Y: b, x! b$ k0 p% d. V
%  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
( s4 k. ~+ A; n2 L2 X. L%  P(k|k) = (I-kg(k)*H)*P(k|k-1)
( j1 t  K3 w1 E' ?1 B# ?%--------------------------------------------------------------------------

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

r = size(A,1);                                                            %获取系统矩阵A的行数" v; s2 f8 J7 N& z" r8 I/ V  V
I= eye( r(1) );                                                           %生成单位矩阵9 M4 C' D  j9 Y9 G
P1 = zeros( r(1),r(1) );                                                  %初始化协方差阵

%初始化节点位置，协方差阵- s- i  P) z7 {8 F7 X2 E
X = x0;
/ L. p) Z; y" s. A* g* { P = p0;

len1 = size( H*X ,1);
3 H( v& S. D) ], n$ p% }. G for s=1:1:len, ~, M  g& M, h) p  z4 n
     z1 = A*X+ Rw;                        % X(k) = A*X(k) + Rw(k)     协方差  Rw_c8 v0 A' P$ R* R+ O
     zx(1:r(1),s) = z1(1:r(1)) ;   
7 ?3 t9 R7 ]2 h" M: @5 O! O     z2 = H*X + Rv;                       % Z(k) = H*X(k) + Rv(k)     协方差  Rv_c
; z* a' t6 A# T& z# f     zy(1:len1,s) = z2(1:len1 );
; U* _9 H  a$ P6 C; C   
$ x2 t/ {9 ]; T* ^5 ]5 Q     P1 = A*P*A' + Rw_c;                  %  P(k|k-1) = A*P(k-1|k-1)*A' + Q; B( L8 `* p, f  J2 X0 {1 Y( U4 H
     K = P1*H'*inv( H*P1*H' + Rv_c );     %  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)( c" {3 F. P3 Y: I  a( a
     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))
' q8 c6 j% Q) E& N* C3 y9 {% P     P = ( I - K*H ) * P1;                %  P(k|k) = (I-kg(k)*H)*P(k|k-1)& ]* x4 ?2 ]( l
end

/ T. b9 g1 j4 ?* Y) [9 T" \! f! _; Y, Dreturn

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