75卡尔曼滤波器.docx
《75卡尔曼滤波器.docx》由会员分享，可在线阅读，更多相关《75卡尔曼滤波器.docx（11页珍藏版）》请在冰点文库上搜索。
75卡尔曼滤波器
7.5 卡尔曼滤波器
由于维纳滤波器难以用于实时处理，满足不了20世纪50年代航天航空技术发展的要求，于是人们开始探索新的理论和技术途径。
20世纪60年代新出现的卡尔曼滤波理论，用信号与噪声的状态空间模型取代了相关函数，用时域的微分方程来表示滤波问题，得到了递推估计算法，适用于计算机实时处理，它突破了维纳滤波的平稳过程的限制，也没有无限时间的要求，这一对维纳滤波理论的重大突破很快地被用于空间技术、自动控制和信号处理等领域。
卡尔曼滤波由滤波方程和预测方程两部分组成。
7.5.1 滤波基本方程
设信号状态方程和量测方程分别为
（7-133）
（7-134）
式中，
为信号的状态向量，
为量测向量，
和
分别为状态噪声和量测噪声，且为互不相关的高斯白噪声向量序列，其协方差分别为
和
；
，
和
分别为状态转移矩阵、输入矩阵和观测矩阵。
卡尔曼滤波基本方程为
（7-135）
（7-136）
（7-137）
（7-138）
（7-139）
其中，残差（新息）定义为
（7-140）
协方差矩阵为
（7-141）
7.5.2 一步预测基本方程
卡尔曼一步预测基本方程为
（7-142）
（7-143）
（7-144）
式中，
为一步预测增益阵。
【例7-16】利用卡尔曼滤波器跟踪机动目标。
例程7-14 基于卡尔曼滤波器的机动目标跟踪
%Makeapointmoveinthe2Dplane
%State=（xyxdotydot）.Weonlyobserve（xy）.
%X（t+1）=Φ（t）X（t）+noise（Q）
%Y（t）=HX（t）+noise（R）
ss=4;%statesize
os=2;%observationsize
F=[1010;0101;0010;0001];
H=[1000;0100];
Q=0.1*eye（ss）;
R=1*eye（os）;
initx=[101010]'; %targetinitialparameters
initV=10*eye（ss）;
seed=9;
rand（'state',seed）;
randn（'state',seed）;
T=15;
[x,y]=sample_lds（F,H,Q,R,initx,T）; %generatetargetdata
%kalmanfilter
[xfilt,Vfilt,VVfilt,loglik]=kalman_filter（y,F,H,Q,R,initx,initV）;
%onesteppredict
[xsmooth,Vsmooth]=kalman_smoother（y,F,H,Q,R,initx,initV）;
%calculatetheerrorbetweenthefiltereddataandtherealdata
dfilt=x（[12],:
）-xfilt（[12],:
）;
mse_filt=sqrt（sum（sum（dfilt.^2）））;
dsmooth=x（[12],:
）-xsmooth（[12],:
）;
mse_smooth=sqrt（sum（sum（dsmooth.^2）））
figure
（1）
clf
%subplot（2,1,1）
holdon
plot（x（1,:
）,x（2,:
）,'ks-'）;
plot（y（1,:
）,y（2,:
）,'g*'）;
plot（xfilt（1,:
）,xfilt（2,:
）,'rx:
'）;
fort=1:
T,plotgauss2d（xfilt（1:
2,t）,Vfilt（1:
2,1:
2,t））;end
holdoff
legend（'true','observed','filtered',3）
xlabel（'x'）
ylabel（'y'）
%3x3inches
set（gcf,'units','inches'）;
set（gcf,'PaperPosition',[0033]）
%print（gcf,'-depsc','/home/eecs/murphyk/public_html/Bayes/Figures/aima_filtered.eps'）;
%print（gcf,'-djpeg','-r100','/home/eecs/murphyk/public_html/Bayes/Figures/aima_filtered.jpg'）;
figure
（2）
%subplot（2,1,2）
holdon
plot（x（1,:
）,x（2,:
）,'ks-'）;
plot（y（1,:
）,y（2,:
）,'g*'）;
plot（xsmooth（1,:
）,xsmooth（2,:
）,'rx:
'）;
fort=1:
T,plotgauss2d（xsmooth（1:
2,t）,Vsmooth（1:
2,1:
2,t））;end
holdoff
legend（'true','observed','smoothed',3）
xlabel（'x'）
ylabel（'y'）
%3x3inches
set（gcf,'units','inches'）;
set（gcf,'PaperPosition',[0033]）
%print（gcf,'-djpeg','-r100','/home/eecs/murphyk/public_html/Bayes/Figures/aima_smoothed.jpg'）;
%print（gcf,'-depsc','/home/eecs/murphyk/public_html/Bayes/Figures/aima_smoothed.eps'）;
图7-27 卡尔曼滤波器跟踪机动目标仿真
图7-27中“□”表示机动目标的真实轨迹，“＊”表示目标观测轨迹，“…”表示经卡尔曼滤波后的目标轨迹，圆表示跟踪波门。
可以看出，卡尔曼滤波器能较为准确地跟踪机动目标。
函数kalman_filter和kalman_update分别是实现卡尔曼滤波方程滤波和状态更新的代码。
function[x,V,VV,loglik]=kalman_filter（y,A,C,Q,R,init_x,init_V,varargin）
%Kalmanfilter.
%[x,V,VV,loglik]=kalman_filter（y,A,C,Q,R,init_x,init_V,...）
%
%INPUTS:
%y（:
t）-theobservationattimet
%A-thesystemmatrix
%C-theobservationmatrix
%Q-thesystemcovariance
%R-theobservationcovariance
%init_x-theinitialstate（column）vector
%init_V-theinitialstatecovariance
%
%OPTIONALINPUTS（string/valuepairs[defaultinbrackets]）
%'model'-model（t）=mmeansuseparamsfrommodelmattimet[ones（1,T）]
%Inthiscase,alltheabovematricestakeanadditionalfinaldimension,
%i.e.,A（:
:
m）,C（:
:
m）,Q（:
:
m）,R（:
:
m）.
%However,init_xandinit_Vareindependentofmodel
（1）.
%'u'-u（:
t）thecontrolsignalattimet[[]]
%'B'-B（:
:
m）theinputregressionmatrixformodelm
%
%OUTPUTS（whereXisthehiddenstatebeingestimated）
%x（:
t）=E[X（:
t）|y（:
1:
t）]
%V（:
:
t）=Cov[X（:
t）|y（:
1:
t）]
%VV（:
:
t）=Cov[X（:
t）,X（:
t-1）|y（:
1:
t）]t>=2
%loglik=sum{t=1}^TlogP（y（:
t））
%
%Ifaninputsignalisspecified,wealsoconditiononit:
%e.g.,x（:
t）=E[X（:
t）|y（:
1:
t）,u（:
1:
t）]
%Ifamodelsequenceisspecified,wealsoconditiononit:
%e.g.,x（:
t）=E[X（:
t）|y（:
1:
t）,u（:
1:
t）,m（1:
t）]
[osT]=size（y）;
ss=size（A,1）;%sizeofstatespace
%setdefaultparams
model=ones（1,T）;
u=[];
B=[];
ndx=[];
args=varargin;
nargs=length（args）;
fori=1:
2:
nargs
switchargs{i}
case'model',model=args{i+1};
case'u',u=args{i+1};
case'B',B=args{i+1};
case'ndx',ndx=args{i+1};
otherwise,error（['unrecognizedargument'args{i}]）
end
end
x=zeros（ss,T）;
V=zeros（ss,ss,T）;
VV=zeros（ss,ss,T）;
loglik=0;
fort=1:
T
m=model（t）;
ift==1
%prevx=init_x（:
m）;
%prevV=init_V（:
:
m）;
prevx=init_x;
prevV=init_V;
initial=1;
else
prevx=x（:
t-1）;
prevV=V（:
:
t-1）;
initial=0;
end
ifisempty（u）
[x（:
t）,V（:
:
t）,LL,VV（:
:
t）]=...
kalman_update（A（:
:
m）,C（:
:
m）,Q（:
:
m）,R（:
:
m）,y（:
t）,prevx,prevV,'initial',initial）;
else
ifisempty（ndx）
[x（:
t）,V（:
:
t）,LL,VV（:
:
t）]=...
kalman_update（A（:
:
m）,C（:
:
m）,Q（:
:
m）,R（:
:
m）,y（:
t）,prevx,prevV,...
'initial',initial,'u',u（:
t）,'B',B（:
:
m））;
else
i=ndx{t};
%copyoverallelements;onlysomewillgetupdated
x（:
t）=prevx;
prevP=inv（prevV）;
prevPsmall=prevP（i,i）;
prevVsmall=inv（prevPsmall）;
[x（i,t）,smallV,LL,VV（i,i,t）]=...
kalman_update（A（i,i,m）,C（:
i,m）,Q（i,i,m）,R（:
:
m）,y（:
t）,prevx（i）,prevVsmall,...
'initial',initial,'u',u（:
t）,'B',B（i,:
m））;
smallP=inv（smallV）;
prevP（i,i）=smallP;
V（:
:
t）=inv（prevP）;
end
end
loglik=loglik+LL;
end
function[xnew,Vnew,loglik,VVnew]=kalman_update（A,C,Q,R,y,x,V,varargin）
%KALMAN_UPDATEDoaonestepupdateoftheKalmanfilter
%[xnew,Vnew,loglik]=kalman_update（A,C,Q,R,y,x,V,...）
%
%INPUTS:
%A-thesystemmatrix
%C-theobservationmatrix
%Q-thesystemcovariance
%R-theobservationcovariance
%y（:
）-theobservationattimet
%x（:
）-E[X|y（:
1:
t-1）]priormean
%V（:
:
）-Cov[X|y（:
1:
t-1）]priorcovariance
%
%OPTIONALINPUTS（string/valuepairs[defaultinbrackets]）
%'initial'-1meansxandVaretakenasinitialconditions（soAandQareignored）[0]
%'u'-u（:
）thecontrolsignalattimet[[]]
%'B'-theinputregressionmatrix
%
%OUTPUTS（whereXisthehiddenstatebeingestimated）
%xnew（:
）=E[X|y（:
1:
t）]
%Vnew（:
:
）=Var[X（t）|y（:
1:
t）]
%VVnew（:
:
）=Cov[X（t）,X（t-1）|y（:
1:
t）]
%loglik=logP（y（:
t）|y（:
1:
t-1））log-likelihoodofinnovatio
%setdefaultparams
u=[];
B=[];
initial=0;
args=varargin;
fori=1:
2:
length（args）
switchargs{i}
case'u',u=args{i+1};
case'B',B=args{i+1};
case'initial',initial=args{i+1};
otherwise,error（['unrecognizedargument'args{i}]）
end
end
% xpred（:
）=E[X_t+1|y（:
1:
t）]
% Vpred（:
:
）=Cov[X_t+1|y（:
1:
t）]
ifinitial
ifisempty（u）
xpred=x;
else
xpred=x+B*u;
end
Vpred=V;
else
ifisempty（u）
xpred=A*x;
else
xpred=A*x+B*u;
end
Vpred=A*V*A'+Q;
end
e=y-C*xpred;%error（innovation）
n=length（e）;
ss=length（A）;
S=C*Vpred*C'+R;
Sinv=inv（S）;
ss=length（V）;
loglik=gaussian_prob（e,zeros（1,length（e））,S,1）;
K=Vpred*C'*Sinv;%Kalmangainmatrix
%Ifthereisnoobservationvector,setK=zeros（ss）.
xnew=xpred+K*e;
Vnew=（eye（ss）-K*C）*Vpred;
VVnew=（eye（ss）-K*C）*A*V;
升级会员 