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

function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y）- g; l/ F7 b/ b+ H, Y/ H
%----------------输入参数--------------------------------------------------
- R; O1 E. q/ {/ D# k%      A -- 系统矩阵
( G1 X% J8 t" `%      H -- 观察矩阵
! b! F; {. L$ C% _9 y%     Rw -- 扰动向量9 i( Z% K+ \; b+ G7 e0 b0 _
%     Rv -- 测量噪声
" g4 A9 A: v6 F: a' `%   Rw_c -- 扰动向量的协方差
) P; f8 p* a# a5 `) `. d2 I%   Rv_c -- 测量噪声的协方差
0 }& V7 j9 d( E4 T%     x0 -- 节点初始位置向量(x,y)'
' R: I+ j7 z; G; f%     p0 -- 初试协方差阵
' y- k9 Y( r' ^* Q' a" g; o. q+ y%      y -- 采样周期
. o, a% i' W# G5 m: E. O%--------------------------------------------------------------------------

9 c5 q  |  C) F4 B  |+ N%--------------------------------------------------------------------------! k9 v- i( D& }& w4 i" s2 W
%   X(k) = A*X(k) + Rw(k)     噪声Q$ Z4 z4 j& s5 @% G4 i
%   Z(k) = H*X(k) + Rv(k)     噪声R
1 E% M& C5 w! v% z' T. g

%  x(k|k-1) = A*x(k-1|k-1) + B*U(k)
; t% _7 R: Q+ U0 p) [; t- Y%  P(k|k-1) = A*P(k-1|k-1)*A' + Q
7 L: I% A/ f) L: \- l  X1 L%  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))) }5 [- u) k3 R9 p3 _" T+ O" y
%  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)  H" o& f' t# v% L* b) X! r. M. P
%  P(k|k) = (I-kg(k)*H)*P(k|k-1)) V# `" b2 |+ c! F* c  `5 [  {
%--------------------------------------------------------------------------

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

r = size(A,1);                                                            %获取系统矩阵A的行数
) u* }' g+ W0 a3 G% w I= eye( r(1) );                                                           %生成单位矩阵
! N5 r- ]7 o4 y- y0 F" A P1 = zeros( r(1),r(1) );                                                  %初始化协方差阵

%初始化节点位置，协方差阵( x& T( ^. t: f4 X
X = x0;: n5 M; x) ~; E( G' {
P = p0;

len1 = size( H*X ,1);
# Q3 g% e# L! _* h3 M4 e for s=1:1:len; G9 [) s) C6 e8 r% }
     z1 = A*X+ Rw;                        % X(k) = A*X(k) + Rw(k)     协方差  Rw_c/ i- N4 u3 w3 n# K0 h
     zx(1:r(1),s) = z1(1:r(1)) ;   
/ I5 N( e/ Q2 F     z2 = H*X + Rv;                       % Z(k) = H*X(k) + Rv(k)     协方差  Rv_c$ F3 k2 }& K' I5 E+ X
     zy(1:len1,s) = z2(1:len1 );$ g; k$ y9 H/ w; E% X
    / q$ t; F+ a+ i0 S9 y0 f# s5 o
     P1 = A*P*A' + Rw_c;                  %  P(k|k-1) = A*P(k-1|k-1)*A' + Q
4 Y9 b- r  d, ~1 d0 q+ W" R     K = P1*H'*inv( H*P1*H' + Rv_c );     %  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)6 a- q* R# _% |! r2 G- Z! v/ s
     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))
# \* j$ i2 r9 ]3 k6 k     P = ( I - K*H ) * P1;                %  P(k|k) = (I-kg(k)*H)*P(k|k-1)
; d6 x1 j5 n+ ?" ]7 h* p" u- ~ end

5 l; L9 [: \2 h1 Z) S
return

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