最近因?yàn)橐玫終alman濾波器做東西，所以一直在學(xué)習(xí)這個(gè)東西，鑒于之前的仿真都是用matlab做的，所以呢，kalman濾波器的仿真程序也是用matlab編的。痛苦了幾天，幾天這個(gè)函數(shù)終于搞定了，具體的分析如下。
function [zx,zy]=xyKalmanFliter(A,H,Rw,Rv,Rw_c,Rv_c,x0,p0,y）
%----------------輸入?yún)?shù)--------------------------------------------------
% A -- 系統(tǒng)矩陣
% H -- 觀察矩陣
% Rw -- 擾動(dòng)向量
% Rv -- 測(cè)量噪聲
% Rw_c -- 擾動(dòng)向量的協(xié)方差
% Rv_c -- 測(cè)量噪聲的協(xié)方差
% x0 -- 節(jié)點(diǎn)初始位置向量(x,y)'
% p0 -- 初試協(xié)方差陣
% y -- 采樣周期
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% 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
% 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)
% P(k|k) = (I-kg(k)*H)*P(k|k-1)
%--------------------------------------------------------------------------
len = length( y ); %獲取采樣點(diǎn)數(shù)
r = size(A,1); %獲取系統(tǒng)矩陣A的行數(shù)
I= eye( r(1) ); %生成單位矩陣
P1 = zeros( r(1),r(1) ); %初始化協(xié)方差陣
%初始化節(jié)點(diǎn)位置，協(xié)方差陣
X = x0;
P = p0;
len1 = size( H*X ,1);
for s=1:1:len
z1 = A*X+ Rw; % X(k) = A*X(k) + Rw(k) 協(xié)方差 Rw_c
zx(1:r(1),s) = z1(1:r(1)) ;
z2 = H*X + Rv; % Z(k) = H*X(k) + Rv(k) 協(xié)方差 Rv_c
zy(1:len1,s) = z2(1:len1 );
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)
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)
end
return
