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

uperrua

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

function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y）
%----------------输入参数--------------------------------------------------
%      A -- 系统矩阵
* y7 o5 C7 l+ T" L7 Z4 u%      H -- 观察矩阵
9 D3 E! c8 B) @1 x5 x5 V7 j%     Rw -- 扰动向量
# N  D' N/ m& ~9 s. K# O% A! ^) A%     Rv -- 测量噪声
& d* J0 E5 p) W" [/ W" B1 A0 k, W- H- ^# z%   Rw_c -- 扰动向量的协方差
7 h, L' W8 r! R/ E+ W6 x8 Q%   Rv_c -- 测量噪声的协方差
9 S: q- X4 G' s%     x0 -- 节点初始位置向量(x,y)'
%     p0 -- 初试协方差阵
; z/ G/ @/ z- p% Q' y%      y -- 采样周期
%--------------------------------------------------------------------------

4 W# v$ C* e! Q+ O6 x0 F* ?%--------------------------------------------------------------------------
%   X(k) = A*X(k) + Rw(k)     噪声Q
. }$ Z# j+ c. j$ r; f- H1 \%   Z(k) = H*X(k) + Rv(k)     噪声R
- Q6 E! \! s- A/ q

%  x(k|k-1) = A*x(k-1|k-1) + B*U(k)
1 o" m. `4 i4 c%  P(k|k-1) = A*P(k-1|k-1)*A' + Q
5 T! O( _3 c3 F%  x(k|k) = x(k|k-1) + kg(k)*(z(k)-H*s(k|k-1))
%  kg(k) = P(k|k-1)*H' / (H*P(k|k-1)*H' + R)
- V' m9 W( Z7 t4 a" f%  P(k|k) = (I-kg(k)*H)*P(k|k-1)
%--------------------------------------------------------------------------

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

r = size(A,1);                                                            %获取系统矩阵A的行数
7 t8 E3 W5 r/ I0 A  {: `2 S' f I= eye( r(1) );                                                           %生成单位矩阵
& _) H4 G/ K8 P8 {. s: [ P1 = zeros( r(1),r(1) );                                                  %初始化协方差阵

%初始化节点位置，协方差阵
X = x0;
P = p0;

len1 = size( H*X ,1);
" k5 q) {0 N1 s: M8 F9 t for s=1:1:len
+ U) l1 L/ n( y     z1 = A*X+ Rw;                        % X(k) = A*X(k) + Rw(k)     协方差  Rw_c
3 n3 B$ r& U) B0 Y1 i! F8 X     zx(1:r(1),s) = z1(1:r(1)) ;   
     z2 = H*X + Rv;                       % Z(k) = H*X(k) + Rv(k)     协方差  Rv_c
     zy(1:len1,s) = z2(1:len1 );
0 B, a1 T4 o  [+ v   
     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)
1 N9 Q6 M2 i$ E( {4 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))
, |0 s& x: a1 |6 F5 B3 b7 Z5 S     P = ( I - K*H ) * P1;                %  P(k|k) = (I-kg(k)*H)*P(k|k-1)
end

$ V  K5 H) t8 t$ m. u$ Yreturn

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

CCxiaom

Kalman滤波的Matlab仿真程序解读