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

function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y）
# L. c4 q1 E6 l& w%----------------输入参数--------------------------------------------------  v/ F0 R. i4 f9 P2 B5 ~! B; X
%      A -- 系统矩阵
$ h+ Z* Y' S7 w; o) Q%      H -- 观察矩阵
% F7 v1 G  B1 S& [5 S' h, J  E%     Rw -- 扰动向量
9 K* E; ?, g0 j: n%     Rv -- 测量噪声
/ `% s* j" R% R' w3 `%   Rw_c -- 扰动向量的协方差" G) J# B2 x9 X: D  f$ B% H" r
%   Rv_c -- 测量噪声的协方差
( g6 R1 d) G$ d, Z4 Y%     x0 -- 节点初始位置向量(x,y)'$ w  W7 C! j# {" E+ I
%     p0 -- 初试协方差阵# l% h$ H0 }2 U" ~* x! \( t  _
%      y -- 采样周期
6 X! p' o6 X) A4 ^%--------------------------------------------------------------------------

6 r. Q4 \2 J' P4 q
%--------------------------------------------------------------------------
5 G6 \3 f% T. U6 Y1 j%   X(k) = A*X(k) + Rw(k)     噪声Q
; x+ D# v9 l+ H  e/ w%   Z(k) = H*X(k) + Rv(k)     噪声R
; G7 k1 }! o4 W, F0 {# C: d

%  x(k|k-1) = A*x(k-1|k-1) + B*U(k)
' z% z9 x! p7 G" y4 k" n& ]%  P(k|k-1) = A*P(k-1|k-1)*A' + Q5 r5 ^* J- Z9 Z- A1 R
%  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
+ _- v6 D" X$ ]  w/ x3 f! ~  `6 e$ r%  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)! X+ \+ V/ M* h6 K0 y$ n
%  P(k|k) = (I-kg(k)*H)*P(k|k-1)
6 D: L& }% J; k' p% T% D( e' y5 H% h%--------------------------------------------------------------------------

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

r = size(A,1);                                                            %获取系统矩阵A的行数3 C, i1 C" o' ~  ~- _# k
I= eye( r(1) );                                                           %生成单位矩阵
9 M6 W9 g+ c9 r9 C# n3 q2 K0 a# c P1 = zeros( r(1),r(1) );                                                  %初始化协方差阵

%初始化节点位置，协方差阵
3 m6 l" H' E! O- C* i X = x0;
$ W; W; g/ X$ O$ T. N6 N$ d P = p0;

len1 = size( H*X ,1);
- E, L/ h7 A. L9 f+ C. Y+ F' j6 h for s=1:1:len+ d  m* _# |+ Q- x+ x. `# G, }
     z1 = A*X+ Rw;                        % X(k) = A*X(k) + Rw(k)     协方差  Rw_c, b0 s8 h9 f7 x
     zx(1:r(1),s) = z1(1:r(1)) ;    & U1 g) C+ A9 n7 a6 y0 l9 @' D
     z2 = H*X + Rv;                       % Z(k) = H*X(k) + Rv(k)     协方差  Rv_c( O" x$ v+ v+ N1 h! T4 y
     zy(1:len1,s) = z2(1:len1 );9 ?) i  z( Z% a/ f1 y0 g  Z% K
   
# ]3 ?( v6 S9 V1 Y9 O/ ^. b  y     P1 = A*P*A' + Rw_c;                  %  P(k|k-1) = A*P(k-1|k-1)*A' + Q. x: g* K4 B9 e% w- Y
     K = P1*H'*inv( H*P1*H' + Rv_c );     %  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
# K, J, ^3 N9 P+ X$ _. Q! 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))! \% \" r0 Y3 Z/ n8 i
     P = ( I - K*H ) * P1;                %  P(k|k) = (I-kg(k)*H)*P(k|k-1)
+ S" F6 N. q( `4 F end

' ~; J& f/ n3 M6 E" A% U0 mreturn

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