资源描述：
《有偏卡尔曼滤波器.doc》由会员上传分享，免费在线阅读，更多相关内容在教育资源-天天文库。
1、有偏卡尔曼滤波器可以用来消除无线定位中的非视距误差，首次使用有偏卡尔曼滤波器的参考文献是下面两篇论文[1]刘琚，李静.一种在非视距环境中的TDOA/AOA混合定位方法[J].通信学报，2005，26(5):63-67.[2]BaoLongLe,K.Ahmed,H.Tsuji.MobilelocationestimatorwithNLOSmitigationusingKalmanfiltering[C].IEEEWirelessCommunicationsandNetworking,2003,3:1969-
2、1973.本源码为GreenSim团队原创，转载请注明。function[X,Flag]=BiasedKalmanFilter(x,X0,P0,Phi,Gamma,Q,H,R,SIGMAD,SIGMA_NLOS)%%用于校正NLOS误差的有偏卡尔曼滤波器%GreenSim团队原创作品，转载请注明%%本程序加入了非视距检测模块，对于NLOS使用有偏卡尔曼滤波，对于LOS使用标准卡尔曼滤波%%输入参数列表%x输入距离观测值序列，1×N的行向量%X0预测序列的初始值，1×2的列向量，包括距离和径向速度两个分量%
3、P0预测误差矩阵的初始值，2×2的矩阵%Phi状态转移矩阵,2×2的矩阵%Gamma噪声输入矩阵，2×1的矩阵%Q输入噪声协方差矩阵，1×1的矩阵%H观测矩阵，1×2的向量%R观测噪声协方差矩阵，1×1的矩阵%%输出参数列表%X预测输出值%FlagNLOS标记，为1表示LOS，为0表示非视距%%N=length(x);X=zeros(1,N);X(1)=X0(1);Flag=ones(N,1);fori=2:Nifi>15%因为检测区间为15个样本YB=x((i-15):(i));%待检测的样本%YB=x
4、((i-15):(i))-X((i-15):(i));改成这个以后，无论怎么调整参数都不收敛YBstd=std(YB);elseYBstd=0;endifYBstd>3*sqrt(SIGMAD)%如果局部样本标准差大于三倍（可改）观测误差的标准差，则判断为NLOSFlag(i)=0;%设置调整参数lambda=1.4;%由状态方程得到的预测值X1=Phi*X0;%计算上述预测的协方差矩阵P1=Phi*P0*(Phi')+Gamma*Q*(Gamma');%计算滤波增益（加权系数）K=P1*(H')*inv
5、(Q*P1*(H')+lambda*R);%计算观察值Y=x(i)+lambda*R;%加权得到滤波输出值X2=X1+K*(Y-S*X1-SIGMA_NLOS);else%由状态方程得到的预测值X1=Phi*X0;%计算上述预测的协方差矩阵P1=Phi*P0*(Phi')+Gamma*Q*(Gamma');%计算滤波增益（加权系数）K=P1*(H')*inv(P*P1*(H')+R);%计算观察值Y=x(i)+R;%加权得到滤波输出值X2=X1+K*(Y-H*X1);end%记录和更新P2=([1,0;0
6、,1]-K*H)*P1;X(i)=X2(1);X0=X2;P0=P2;end卡尔曼滤波仿真程序用于匀速运动的目标跟踪%kalmanfilteringloadinitial_tracks;%y:initialdata,s:datawithnoiseT=0.1;%ypdenotesthesamplevalueofposition%yvdenotesthesamplevalueofvelocity%Y=[yp(n);yv(n)];%errordeviationcausedbytherandomaccelerat
7、ion%knowndataY=zeros(2,200);Y0=[0;1];Y(:,1)=Y0;A=[1T01];B=[1/2*(T)^2T]';H=[10];C0=[0001];C=[C0zeros(2,2*199)];Q=(0.25)^2;R=(0.25)^2;%kalmanalgorithmierationforn=1:200i=(n-1)*2+1;K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));Y(:,
8、n+1)=A*Y(:,n);C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';end%thediagramofpositionafterfilteringt=0:0.1:20;figure(2);yp=Y(1,:);plot(t,yp,'r.-');axis([020020]);xlabel('time');ylabel('yppo
